add hironx_calibration

hironx_calibration/CMakeLists.txt

@@ -0,0 +1,12 @@
+
+cmake_minimum_required(VERSION 2.8.3)
+project(hironx_calibration)
+
+find_package(catkin)
+catkin_package()
+
+install(DIRECTORY capture_data estimate_params # view_results
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
+

hironx_calibration/capture_data/all_pipelines.launch

@@ -0,0 +1,29 @@
+
+<launch>
+  <include file="$(find calibration_launch)/capture_data/chain_settler.launch" ns="arm_chain" />
+  <include file="$(find calibration_launch)/capture_data/chain_settler.launch" ns="head_chain" />
+
+  <arg name="use_kinect" default="false" />
+  <group if="$(arg use_kinect)" >
+    <include file="$(find calibration_launch)/capture_data/kinect_pipeline.launch" ns="/camera/rgb">
+      <arg name="image_topic" value="image_rect_color"/> <!-- openni2.launch publishes image_raw -->
+      <arg name="depth_topic" value="/camera/depth/image"/> <!-- this is floar value -->
+      <arg name="camera_info_topic" value="camera_info"/>
+    </include>
+  </group>
+  <group unless="$(arg use_kinect)" >
+    <include file="$(find calibration_launch)/capture_data/monocam_pipeline.launch" ns="/camera/rgb">
+      <arg name="image_topic" value="image_rect_color"/> <!-- this should be image proc -->
+    </include>
+  </group>
+
+  <node type="interval_intersection_action"
+        pkg="interval_intersection"
+        name="interval_intersection"
+        output="screen">
+    <remap from="head_chain" to="head_chain/settled_interval" />
+    <remap from="arm_chain" to="arm_chain/settled_interval" />
+    <remap from="head_camera" to="/camera/rgb/settled_interval" />
+  </node>
+
+</launch>

hironx_calibration/capture_data/all_viewers.launch

@@ -0,0 +1,15 @@
+
+<launch>
+
+  <!-- Hack to create the directory -->
+  <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/hironx_calibration" />
+  <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/hironx_calibration/cb_fail" />
+
+  <param name="/camera/rgb/annotated_viewer/filename_format" type="string" value="/tmp/hironx_calibration/cb_fail/cb_monocam_%04i.jpg" />
+
+  <include file="$(find calibration_launch)/capture_data/annotated_viewer.launch"
+           ns="/camera/rgb" >
+   <arg name="image_topic" value="image_rect_annotated" />
+  </include>
+
+</launch>

hironx_calibration/capture_data/capture_data.launch

@@ -0,0 +1,6 @@
+
+<launch>
+  <include file="$(find hironx_calibration)/capture_data/all_viewers.launch"/>
+  <include file="$(find hironx_calibration)/capture_data/all_pipelines.launch"/>
+  <include file="$(find hironx_calibration)/capture_data/capture_exec.launch"/>
+</launch>

hironx_calibration/capture_data/capture_exec.launch

@@ -0,0 +1,23 @@
+
+<launch>
+
+  <node type="capture_exec.py"
+        pkg="calibration_launch"
+        name="calibration_exec"
+        args="$(find hironx_calibration)/capture_data/samples/ $(find hironx_calibration)/capture_data/hardware_config $(find hironx_calibration)/estimate_params/config/system.yaml"
+        output="screen" >
+    <remap from="head_camera/camera_info" to="/camera/rgb/camera_info"/>
+    <remap from="head_camera/image_rect"  to="/camera/rgb/image_rect_throttle"/>
+    <remap from="head_camera/image"       to="/camera/rgb/image_rect_throttle"/>
+    <remap from="head_camera/features"    to="/camera/rgb/features"/>
+  </node>
+
+  <node type="urdf_pub.py" pkg="calibration_launch" name="urdf_pub"/>
+
+  <node type="record" pkg="rosbag" name="robot_measurement_recorder" output="screen"
+        args="-O /tmp/hironx_calibration/cal_measurements robot_measurement robot_description" >
+    <!-- Hack to create the directory -->
+    <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/hironx_calibration" />
+  </node>
+
+</launch>

hironx_calibration/capture_data/hardware_config/cam_config.yaml

@@ -0,0 +1,24 @@
+
+# ----- Microsoft Kinect -----
+head_camera: ###FIX
+  cb_detector_config:  /camera/rgb/cb_detector_config ## FIXME
+  led_detector_config: /camera/rgb/led_detector
+  settler_config:      /camera/rgb/monocam_settler_config ## FIXME
+
+  configs:
+    small_cb_4x5:
+      settler:
+        tolerance: 2.00
+        ignore_failures: True
+        max_step: 3.0
+        cache_size: 100
+      cb_detector:
+        active: True
+        num_x: 4
+        num_y: 5
+        width_scaling: 0.5
+        height_scaling: 0.5
+        subpixel_window: 4
+        subpixel_zero_zone: 1
+      led_detector:
+        active: False

hironx_calibration/capture_data/hardware_config/chain_config.yaml

@@ -0,0 +1,40 @@
+# ----- arm -----
+arm_chain:
+  settler_config:  /arm_chain/settler_config
+
+  configs:
+    tight_tol:
+      settler:
+        joint_names:
+        - RARM_JOINT0
+        - RARM_JOINT1
+        - RARM_JOINT2
+        - RARM_JOINT3
+        - RARM_JOINT4
+        - RARM_JOINT5
+        tolerances:
+        - 0.002
+        - 0.002
+        - 0.002
+        - 0.002
+        - 0.002
+        - 0.002
+        max_step:   1.0
+        cache_size: 1500
+
+# ----- head -----
+head_chain:
+  settler_config:  /head_chain/settler_config
+
+  configs:
+    tight_tol:
+      settler:
+        joint_names:
+        - HEAD_JOINT0
+        - HEAD_JOINT1
+        tolerances:
+        - 0.002
+        - 0.002
+        max_step:   1.0
+        cache_size: 1500
+

hironx_calibration/capture_data/hardware_config/controller_config.yaml

@@ -0,0 +1,14 @@
+arm_controller:
+  topic: rarm_controller/command
+  joint_names:
+  - RARM_JOINT0
+  - RARM_JOINT1
+  - RARM_JOINT2
+  - RARM_JOINT3
+  - RARM_JOINT4
+  - RARM_JOINT5
+head_controller:
+  topic: head_controller/command
+  joint_names:
+  - HEAD_JOINT0
+  - HEAD_JOINT1

hironx_calibration/capture_data/hardware_config/laser_config.yaml

@@ -0,0 +1 @@
+# laser config

hironx_calibration/capture_data/make_samples.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python
+
+# capture samples!!!!1!one
+
+# this script should eventually be replaced by something that finds
+# samples automatically
+
+import rospy
+from sensor_msgs.msg import JointState
+
+import string, os
+
+header1 = """camera_measurements:
+- {cam_id: head_camera, config: small_cb_4x5}
+joint_commands:
+"""
+
+header2_arm = """- controller: arm_controller
+  segments:
+  - duration: 2.0
+    positions: """
+
+header2_head = """- controller: head_controller
+  segments:
+  - duration: 2.0
+    positions: """
+
+header3 = """joint_measurements:
+- {chain_id: arm_chain, config: tight_tol}
+- {chain_id: head_chain, config: tight_tol}
+
+sample_id: arm_"""
+
+header4 = """target: {chain_id: arm_chain, target_id: small_cb_4x5}"""
+
+class SampleMaker:
+
+    def __init__(self):
+        rospy.init_node("make_samples")
+        rospy.Subscriber("joint_states", JointState, self.callback)
+        self.arm_joints = ["RARM_JOINT0", "RARM_JOINT1", "RARM_JOINT2", "RARM_JOINT3", "RARM_JOINT4", "RARM_JOINT5", ]
+        self.arm_state = [0.0 for joint in self.arm_joints]
+        self.head_joints = ["HEAD_JOINT0", "HEAD_JOINT1", ]
+        self.head_state = [0.0 for joint in self.head_joints]
+
+        self.count = 0
+
+        while not rospy.is_shutdown():
+            print "Move arm/head to a new sample position."
+            resp = raw_input("press <enter> ")
+            if string.upper(resp) == "EXIT":
+                break
+            else:
+                # save a sample:
+                count = str(self.count).zfill(4)
+                f = open(os.path.dirname(__file__)+"/samples/arm/arm_"+count+".yaml", "w")
+                f.write(header1)
+                print('saving ... {0}'.format(self.count))
+                print('  arm_state: {0}'.format(self.arm_state))
+
+                f.write(header2_arm)
+                print>>f, self.arm_state
+                print('  head_state: {0}'.format(self.head_state))
+
+                f.write(header2_head)
+                print>>f, self.head_state
+
+                f.write(header3)
+                print>>f, count
+                f.write(header4)
+            self.count += 1
+
+    def callback(self, msg):
+
+        for i in range(len(self.arm_joints)):
+            try:
+                idx = msg.name.index(self.arm_joints[i])
+                self.arm_state[i] = msg.position[idx]
+            except: 
+                pass
+
+        for i in range(len(self.head_joints)):
+            try:
+                idx = msg.name.index(self.head_joints[i])
+                self.head_state[i] = msg.position[idx]
+            except: 
+                pass
+
+
+if __name__=="__main__":
+    SampleMaker()

hironx_calibration/capture_data/samples/arm/arm_0000.yaml

@@ -0,0 +1,17 @@
+camera_measurements:
+- {cam_id: head_camera, config: small_cb_4x5}
+joint_commands:
+- controller: arm_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.18101682337059188, -0.9064317603682494, -1.6154942089384714, -0.9835513802615894, 0.3053644421751016, 0.00011415670979614074]
+- controller: head_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.5234862836738539, 0.5235911651509791]
+joint_measurements:
+- {chain_id: arm_chain, config: tight_tol}
+- {chain_id: head_chain, config: tight_tol}
+
+sample_id: arm_0000
+target: {chain_id: arm_chain, target_id: small_cb_4x5}

hironx_calibration/capture_data/samples/arm/arm_0001.yaml

@@ -0,0 +1,17 @@
+camera_measurements:
+- {cam_id: head_camera, config: small_cb_4x5}
+joint_commands:
+- controller: arm_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.13610077507051782, -0.823673233893684, -1.6098917020395695, -1.383122695890041, 0.2834387434469229, 0.00011415670979614074]
+- controller: head_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.5234862836738539, 0.5235911651509791]
+joint_measurements:
+- {chain_id: arm_chain, config: tight_tol}
+- {chain_id: head_chain, config: tight_tol}
+
+sample_id: arm_0001
+target: {chain_id: arm_chain, target_id: small_cb_4x5}

hironx_calibration/capture_data/samples/arm/arm_0002.yaml

@@ -0,0 +1,17 @@
+camera_measurements:
+- {cam_id: head_camera, config: small_cb_4x5}
+joint_commands:
+- controller: arm_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.1115439924949576, -0.7629956157843496, -1.6519977702439328, -1.3822322735536312, -0.24333025911300632, 0.012237599290146287]
+- controller: head_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.5234862836738539, 0.5235911651509791]
+joint_measurements:
+- {chain_id: arm_chain, config: tight_tol}
+- {chain_id: head_chain, config: tight_tol}
+
+sample_id: arm_0002
+target: {chain_id: arm_chain, target_id: small_cb_4x5}

hironx_calibration/capture_data/samples/arm/arm_0003.yaml

@@ -0,0 +1,17 @@
+camera_measurements:
+- {cam_id: head_camera, config: small_cb_4x5}
+joint_commands:
+- controller: arm_controller
+  segments:
+  - duration: 2.0
+    positions: [0.04715006974262681, -0.7031183558034299, -1.6352077028397474, -1.3834195033355112, -0.009878836273983529, 0.012237599290146287]
+- controller: head_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.5234862836738539, 0.5235911651509791]
+joint_measurements:
+- {chain_id: arm_chain, config: tight_tol}
+- {chain_id: head_chain, config: tight_tol}
+
+sample_id: arm_0003
+target: {chain_id: arm_chain, target_id: small_cb_4x5}

hironx_calibration/capture_data/samples/arm/arm_0004.yaml

@@ -0,0 +1,17 @@
+camera_measurements:
+- {cam_id: head_camera, config: small_cb_4x5}
+joint_commands:
+- controller: arm_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.2991058005605282, -0.7796759767784097, -1.6973763307957852, -1.1807000180795244, -0.007792622402459057, 0.012237599290146287]
+- controller: head_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.5234862836738539, 0.5235911651509791]
+joint_measurements:
+- {chain_id: arm_chain, config: tight_tol}
+- {chain_id: head_chain, config: tight_tol}
+
+sample_id: arm_0004
+target: {chain_id: arm_chain, target_id: small_cb_4x5}

hironx_calibration/capture_data/samples/arm/arm_0005.yaml

@@ -0,0 +1,17 @@
+camera_measurements:
+- {cam_id: head_camera, config: small_cb_4x5}
+joint_commands:
+- controller: arm_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.2338915730597601, -0.7503320053973793, -1.6716763575601687, -1.7210950509124954, 0.13488804394807738, 0.006849402587768444]
+- controller: head_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.5234862836738539, 0.5235911651509791]
+joint_measurements:
+- {chain_id: arm_chain, config: tight_tol}
+- {chain_id: head_chain, config: tight_tol}
+
+sample_id: arm_0005
+target: {chain_id: arm_chain, target_id: small_cb_4x5}

hironx_calibration/capture_data/samples/arm/arm_0006.yaml

@@ -0,0 +1,17 @@
+camera_measurements:
+- {cam_id: head_camera, config: small_cb_4x5}
+joint_commands:
+- controller: arm_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.38515925933010864, -0.7507658443828751, -1.8413049075614978, -0.7852155126617745, 0.09672260194548263, 0.0494755180256474]
+- controller: head_controller
+  segments:
+  - duration: 2.0
+    positions: [-0.5234862836738539, 0.5235911651509791]
+joint_measurements:
+- {chain_id: arm_chain, config: tight_tol}
+- {chain_id: head_chain, config: tight_tol}
+
+sample_id: arm_0006
+target: {chain_id: arm_chain, target_id: small_cb_4x5}