tork-a · k-okada · Oct 17, 2017 · Oct 19, 2017 · Oct 19, 2017 · Oct 19, 2017
diff --git 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.rosinstall b/baxter_seminar_helper.rosinstall
@@ -1,19 +1,28 @@
+# - git:
+#     local-name: baxter_tasker
+#     uri: [email protected]:baxter_tasker
+#     version: de9f70f
 - git:
-    local-name: baxter_tasker
-    uri: [email protected].com:baxter_tasker
-    version: de9f70f
+    local-name: baxter
+    uri: https://github.com/RethinkRobotics/baxter.git
+    version: master
 - git:
-    local-name: baxter_simulator
-    uri: [email protected]: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
diff --git a/baxter_seminar_helper/config/camera.yaml 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
@@ -0,0 +1,25 @@
+<launch>
+  <arg name="camera_driver" default="false" />
+
+  <node ns="cameras/right_hand_camera" pkg="uvc_camera" type="uvc_camera_node" name="right_hand_camera" if="$(arg camera_driver)" >
+    <param name="camera_info_url" value="file://$(find baxter_seminar_helper)/config/camera.yaml" />
+    <param name="frame_id" value="/cameras/right_hand_camera" />
+    <remap from="image_raw" to="image" />
+  </node>
+
+
+  <!-- tf publish for hand cameras -->
+  <node pkg="tf" type="static_transform_publisher" name="camera_hand_r_link_to_frame" 
+              args="0 0 0 -1.5708 0 0 /right_hand_camera_axis /cameras/right_hand_camera 100" />
+  <node pkg="tf" type="static_transform_publisher" name="camera_hand_l_link_to_frame"
+              args="0 0 0 -1.5708 0 0 /left_hand_camera_axis /cameras/left_hand_camera 100" />
+
+  <!-- workaround for roi problem of ar_marker-->
+  <include file="$(find baxter_seminar_helper)/launch/camera_info_fixer.launch" />
+  <node pkg="ar_track_alvar" type="individualMarkersNoKinect"
+        name="ar_track"
+        args="4.5 0.1 0.1
+              /cameras/right_hand_camera/image
+              /cameras/right_hand_camera/camera_info_fixed
+              /right_hand_camera_axis" />
+</launch>
diff --git a/baxter_seminar_helper/launch/camera_info_fixer.launch b/baxter_seminar_helper/launch/camera_info_fixer.launch
@@ -0,0 +1,11 @@
+<launch>
+  <node pkg="baxter_seminar_helper" type="camera_info_fixer.py"
+        name="right_hand_camera_fixer" >
+    <remap from="camera_info" to="/cameras/right_hand_camera/camera_info" />
+  </node>
+  <node pkg="baxter_seminar_helper" type="camera_info_fixer.py"
+        name="left_hand_camera_fixer" >
+    <remap from="camera_info" to="/cameras/left_hand_camera/camera_info" />
+  </node>
+
+</launch>
diff --git a/baxter_seminar_helper/launch/demo.launch b/baxter_seminar_helper/launch/demo.launch
@@ -28,16 +28,9 @@
 	  clear_params="true" />
   </group>
 
-  <!-- workaround for roi problem of ar_marker-->
-  <include file="$(find jsk_baxter_startup)/jsk_baxter_sensors/camera_info_fixer.launch" />
-  <node pkg="ar_track_alvar" type="individualMarkersNoKinect"
-        name="ar_track"
-        args="4.5 0.1 0.1
-              /cameras/right_hand_camera/image
-              /cameras/right_hand_camera/camera_info_fixed
-              /right_hand_camera_axis" />
+  <include file="$(dind baxter_seminar_helper)/launch/ar.launch" />
 
-  <include file="$(find baxter_moveit_config)/launch/demo_baxter.launch">
+  <include file="$(find baxter_moveit_config)/launch/baxter_grippers.launch">
     <arg name="rviz_config" value="$(arg rviz_config)" />
   </include>
 

diff --git a/baxter_seminar_helper/launch/kinect_checkerboard.launch b/baxter_seminar_helper/launch/kinect_checkerboard.launch
@@ -32,7 +32,8 @@
     </node>
   </group>
   <!--  right or left(0.1) / up or down (0.0)/ fornt or back (0.15) -->
-  <node pkg="tf" type="static_transform_publisher" name="base_to_checkerboard" args="0.080 0.120 0.150 0.0 1.5708 1.5708 /object /base 100" />
+  <node pkg="tf" type="static_transform_publisher" name="checkerboard_to_world"
+        args="0.080 0.120 0.150 0.0 1.5708 1.5708 /object /world 100" />
   <node pkg="dynamic_tf_publisher" name="tf_publish" type="tf_publish.py"/>
 
   <!-- end checkerboard  -->

diff --git a/baxter_seminar_helper/scripts/camera_info_fixer.py 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()