Merge pull request #1805 from wkentaro/yuyu2172-collect-sib-launch

Collect data for segmentation in bin while overall run in main.launch
start-jsk · Jun 25, 2016 · 4a1ed7f · 4a1ed7f
2 parents 7d58720 + 509467a
commit 4a1ed7f
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 6 deletions.
diff --git a/jsk_2016_01_baxter_apc/euslisp/main.l b/jsk_2016_01_baxter_apc/euslisp/main.l
@@ -114,10 +114,12 @@
               (send *ri* :move-arm-body->bin-overlook-pose arm target-bin)
               (send *ri* :wait-interpolation)
               (unix::sleep 6)
-              (ros::set-param
-                (format nil "~a_collect_sib_data/ready_to_save" (send *ri* :arm-symbol2str arm))
-                t)
-              (unix::sleep 2)
+              (when (ros::get-param "~collect_data")
+                (ros::set-param
+                  (format nil "~a_collect_sib_data/ready_to_save" (send *ri* :arm-symbol2str arm))
+                  t)
+                (unix::sleep 2)
+                )
               (ros::ros-info "[main] Segmentating objects in bin" target-bin)
               (send *ri* :recognize-objects-segmentation-in-bin arm target-bin
                     :stamp (ros::time-now))

diff --git a/...baxter_apc/launch/collect_sib_data.launch → ...pc/launch/include/collect_sib_data.launch b/...baxter_apc/launch/collect_sib_data.launch → ...pc/launch/include/collect_sib_data.launch
@@ -1,15 +1,16 @@
 <launch>
   <arg name="hand" />
   <arg name="json" default=""/>
+  <arg name="save_dir" default="~/.ros/sib_data"/>
 
   <node name="$(arg hand)_collect_sib_data"
         pkg="jsk_apc2016_common" type="collect_sib_data.py"
         output="screen">
     <remap from="~input" to="$(arg hand)_sib_topic_sync/output"/>
     <remap from="~input/bin_info_array" to="/publish_bin_info/bin_array"/>
-    <remap from="~input/depth" to="/right_hand_camera/depth_registered/image_raw"/>
+    <remap from="~input/depth" to="/$(arg hand)_hand_camera/depth_registered/image_raw"/>
     <remap from="~target_bin_name" to="$(arg hand)_hand/target_bin"/>
-    <param name="~save_dir" value="$(find jsk_apc2016_common)/data/tokyo_run/1" />
+    <param name="~save_dir" value="$(arg save_dir)" />
     <param name="~json" value="$(arg json)"/>
     <param name="always_subscribe" value="True" />
     <param name="~ready_to_save" value="false" />

diff --git a/jsk_2016_01_baxter_apc/launch/main.launch b/jsk_2016_01_baxter_apc/launch/main.launch
@@ -1,6 +1,7 @@
 <launch>
   <arg name="launch_main" default="true" />
   <arg name="json" />
+  <arg name="collect_data" default="false" />
 
   <!-- bin contents data -->
   <node pkg="jsk_2015_05_baxter_apc" type="bin_contents.py" name="bin_contents" respawn="true">
@@ -81,11 +82,17 @@
     args="&quot;(progn (apc-init :larm-controller) (apc-mainloop :larm))&quot;"
     respawn="true" output="screen">
     <env name="DISPLAY" value="" />
+    <rosparam subst_value="true">
+      collect_data: $(arg collect_data)
+    </rosparam>
   </node>
   <node if="$(arg launch_main)" pkg="jsk_2016_01_baxter_apc" type="main.l" name="right_hand"
     args="&quot;(progn (apc-init :rarm-controller) (apc-mainloop :rarm))&quot;"
     respawn="true" output="screen">
     <env name="DISPLAY" value="" />
+    <rosparam subst_value="true">
+      collect_data: $(arg collect_data)
+    </rosparam>
   </node>
 
   <!-- visualization -->
@@ -97,4 +104,16 @@
     <param name="json" value="$(arg json)" />
   </node>
 
+  <!-- collecting data for sib -->
+  <group if="$(arg collect_data)">
+    <include file="$(find jsk_2016_01_baxter_apc)/launch/include/collect_sib_data.launch">
+      <arg name="hand" value="right"/>
+      <arg name="json" value="$(arg json)"/>
+    </include>
+    <include file="$(find jsk_2016_01_baxter_apc)/launch/include/collect_sib_data.launch">
+      <arg name="hand" value="left"/>
+      <arg name="json" value="$(arg json)"/>
+    </include>
+  </group>
+
 </launch>