From 3c40bd3746d418f47be7c24045be2192d070b7bb Mon Sep 17 00:00:00 2001 From: Yosuke Yamamoto Date: Tue, 17 Oct 2017 17:39:28 +0900 Subject: [PATCH 1/3] mod ar_demo.py for real-robot mod kinect_checkerboard.launch adjust to /world frame and it works well --- baxter_seminar/scripts/ar_demo.py | 2 +- baxter_seminar_helper/launch/demo.launch | 8 +++++++- baxter_seminar_helper/launch/kinect_checkerboard.launch | 3 ++- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/baxter_seminar/scripts/ar_demo.py b/baxter_seminar/scripts/ar_demo.py index 2e57331..9a24d1a 100755 --- a/baxter_seminar/scripts/ar_demo.py +++ b/baxter_seminar/scripts/ar_demo.py @@ -70,7 +70,7 @@ def gripper_close(): sys.exit(1) # move to a way point - pose_st_target.pose.position.z += 0.2 + pose_st_target.pose.position.z += 0.05 rospy.loginfo("set target to {}".format(pose_st_target.pose)) group.set_pose_target(pose_st_target.pose) plan = group.plan() diff --git a/baxter_seminar_helper/launch/demo.launch b/baxter_seminar_helper/launch/demo.launch index 30a6de5..65f3c86 100644 --- a/baxter_seminar_helper/launch/demo.launch +++ b/baxter_seminar_helper/launch/demo.launch @@ -28,6 +28,12 @@ clear_params="true" /> + + + + - + diff --git a/baxter_seminar_helper/launch/kinect_checkerboard.launch b/baxter_seminar_helper/launch/kinect_checkerboard.launch index eb3644c..fd253b1 100644 --- a/baxter_seminar_helper/launch/kinect_checkerboard.launch +++ b/baxter_seminar_helper/launch/kinect_checkerboard.launch @@ -32,7 +32,8 @@ - + From c29a37abdf8b9df2613776d4e967d29204651251 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 19 Oct 2017 17:04:10 +0900 Subject: [PATCH 2/3] update baxter_seminar_helper.rosinstall to master --- baxter_seminar_helper.rosinstall | 39 ++++++++++++++++++++------------ 1 file changed, 24 insertions(+), 15 deletions(-) diff --git a/baxter_seminar_helper.rosinstall b/baxter_seminar_helper.rosinstall index 0720d30..87419b2 100644 --- a/baxter_seminar_helper.rosinstall +++ b/baxter_seminar_helper.rosinstall @@ -1,19 +1,28 @@ +# - git: +# local-name: baxter_tasker +# uri: git@dev.humarobotics.com:baxter_tasker +# version: de9f70f - git: - local-name: baxter_tasker - uri: git@dev.humarobotics.com:baxter_tasker - version: de9f70f + local-name: baxter + uri: https://github.com/RethinkRobotics/baxter.git + version: master - git: - local-name: baxter_simulator - uri: git@github.com:RethinkRobotics/baxter_simulator.git - version: release-0.8.1 + local-name: baxter_common + uri: https://github.com/RethinkRobotics/baxter_common.git + version: master +- git: + local-name: baxter_examples + uri: https://github.com/RethinkRobotics/baxter_examples.git + version: master + - git: + local-name: baxter_interface + uri: https://github.com/RethinkRobotics/baxter_interface.git + version: master - git: - local-name: gazebo_ros_pkgs - uri: https://github.com/RethinkRobotics/gazebo_ros_pkgs.git - version: release-0.8.0 + local-name: baxter_simulator + uri: https://github.com/RethinkRobotics/baxter_simulator.git + version: master - git: - local-name: ros_control - uri: https://github.com/RethinkRobotics/ros_control.git - version: release-0.8.0 - - - + local-name: baxter_tools + uri: https://github.com/RethinkRobotics/baxter_tools.git + version: master From ed2a712fa63045315fa38e912aca73693fdc8064 Mon Sep 17 00:00:00 2001 From: Yosuke Yamamoto Date: Thu, 19 Oct 2017 17:29:57 +0900 Subject: [PATCH 3/3] enable to run only ar.launch with roslaunch --screen ar.launch camera_driver:=true --- baxter_seminar_helper/config/camera.yaml | 20 +++++++++++ baxter_seminar_helper/launch/ar.launch | 25 ++++++++++++++ .../launch/camera_info_fixer.launch | 11 +++++++ baxter_seminar_helper/launch/demo.launch | 15 +-------- .../scripts/camera_info_fixer.py | 33 +++++++++++++++++++ 5 files changed, 90 insertions(+), 14 deletions(-) create mode 100644 baxter_seminar_helper/config/camera.yaml create mode 100644 baxter_seminar_helper/launch/ar.launch create mode 100644 baxter_seminar_helper/launch/camera_info_fixer.launch create mode 100755 baxter_seminar_helper/scripts/camera_info_fixer.py diff --git a/baxter_seminar_helper/config/camera.yaml b/baxter_seminar_helper/config/camera.yaml new file mode 100644 index 0000000..2494ad9 --- /dev/null +++ b/baxter_seminar_helper/config/camera.yaml @@ -0,0 +1,20 @@ +image_width: 640 +image_height: 480 +camera_name: camera +camera_matrix: + rows: 3 + cols: 3 + data: [650.205745, 0, 300.450355, 0, 648.217984, 247.490523, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.449179, 0.300371, -0.002295, 0.006749, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] +projection_matrix: + rows: 3 + cols: 4 + data: [579.071289, 0, 300.542379, 0, 0, 606.336548, 247.186137, 0, 0, 0, 1, 0] diff --git a/baxter_seminar_helper/launch/ar.launch b/baxter_seminar_helper/launch/ar.launch new file mode 100644 index 0000000..5d16bca --- /dev/null +++ b/baxter_seminar_helper/launch/ar.launch @@ -0,0 +1,25 @@ + + + + + + + +  + + + + + + + + + + diff --git a/baxter_seminar_helper/launch/camera_info_fixer.launch b/baxter_seminar_helper/launch/camera_info_fixer.launch new file mode 100644 index 0000000..4b3006c --- /dev/null +++ b/baxter_seminar_helper/launch/camera_info_fixer.launch @@ -0,0 +1,11 @@ + + + + + + + + + diff --git a/baxter_seminar_helper/launch/demo.launch b/baxter_seminar_helper/launch/demo.launch index 65f3c86..cb379d7 100644 --- a/baxter_seminar_helper/launch/demo.launch +++ b/baxter_seminar_helper/launch/demo.launch @@ -28,20 +28,7 @@ clear_params="true" /> - - - - - - - + diff --git a/baxter_seminar_helper/scripts/camera_info_fixer.py b/baxter_seminar_helper/scripts/camera_info_fixer.py new file mode 100755 index 0000000..a21fc35 --- /dev/null +++ b/baxter_seminar_helper/scripts/camera_info_fixer.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python + +import rospy +from sensor_msgs.msg import CameraInfo + +class CameraInfoFixer: + def __init__(self): + topic_name = rospy.resolve_name('camera_info') + self.sub = rospy.Subscriber(topic_name, CameraInfo, self.callback) + self.pub = rospy.Publisher(topic_name + '_fixed', CameraInfo, queue_size=1) + + def callback(self, m): + # https://github.com/ros-perception/vision_opencv/blob/8216fb5df7eb262601f12ac4b0c9415477717514/image_geometry/src/pinhole_camera_model.cpp#L149 + K = list(m.K) + P = list(m.P) + K[0*3+2] -= m.roi.x_offset + K[1*3+2] -= m.roi.y_offset + P[0*4+2] -= m.roi.x_offset/2 + P[1*4+2] -= m.roi.y_offset/2 + m.K = tuple(K) + m.P = tuple(P) + + m.roi.x_offset = 0 + m.roi.y_offset = 0 + m.roi.height = m.height + m.roi.width = m.width + + self.pub.publish(m) + +if __name__ == '__main__': + rospy.init_node('camera_info_fixer', anonymous=True) + app = CameraInfoFixer() + rospy.spin()