Merge branch 'fix-sp-segmenter-error' of github.com:cpaxton/costar_st…

…ack into fix-sp-segmenter-error
cpaxton · Jul 11, 2017 · f11c691 · f11c691
2 parents 5152394 + 50cceae
commit f11c691
Show file tree

Hide file tree

Showing 68 changed files with 1,058 additions and 1,220 deletions.
diff --git a/Readme.md b/Readme.md
@@ -15,11 +15,12 @@ To take full advantage of CoSTAR, you will need an RGB-D camera and supported ha
 
 This is a project by members of the JHU Laboratory for Computational Sensing and Robotics, namely Chris Paxton, Kel Guerin, Andrew Hundt, and Felix Jonathan. If you find this code useful, please cite:
 ```
-@article{paxton2016costar,
+@article{paxton2017costar,
   title={Co{STAR}: Instructing Collaborative Robots with Behavior Trees and Vision},
   author={Paxton, Chris and Hundt, Andrew and Jonathan, Felix and Guerin, Kelleher and Hager, Gregory D},
-  journal={arXiv preprint arXiv:1611.06145},
-  year={2016}
+  journal={Robotics and Automation (ICRA), 2017 IEEE International Conference on},
+  note={Available as arXiv preprint arXiv:1611.06145},
+  year={2017}
 }
 ```
 

diff --git a/costar_bringup/config/iiwa_s_model_camera.rviz b/costar_bringup/config/iiwa_s_model_camera.rviz
@@ -14,6 +14,7 @@ Panels:
         - /PlanningScene1/Scene Robot1
         - /PointCloud22
         - /Planned Path1
+        - /PointCloud23
       Splitter Ratio: 0.413174
     Tree Height: 948
   - Class: rviz/Selection
@@ -102,44 +103,10 @@ Visualization Manager:
       Enabled: true
       Frame Timeout: 15
       Frames:
-        01 Center:
-          Value: false
+        00 Home:
+          Value: true
         All Enabled: false
-        C1 01 Over:
-          Value: false
-        C1 02 Pick:
-          Value: false
-        C1 03 Lift:
-          Value: false
-        C1 04 Place:
-          Value: false
-        C1 05 Escape:
-          Value: false
-        C1 11 Over:
-          Value: false
-        C1 12 Pick:
-          Value: false
-        C1 13 Lift:
-          Value: false
-        C1 14 Place:
-          Value: false
-        C1 21 Over:
-          Value: false
-        C1 22 Pick:
-          Value: false
-        C1 23 Avoid:
-          Value: false
-        C1 24 Over:
-          Value: false
-        C1 25 Place:
-          Value: false
-        C3 01 Over:
-          Value: false
-        C3 02 Pick:
-          Value: false
-        C3 03 Over:
-          Value: false
-        C3 04 Pick:
+        ar_marker_0:
           Value: false
         base:
           Value: false
@@ -173,6 +140,8 @@ Visualization Manager:
           Value: true
         obj_link_uniform_2:
           Value: true
+        obj_link_uniform_3:
+          Value: true
         obj_node_uniform_1:
           Value: true
         obj_node_uniform_2:
@@ -184,25 +153,23 @@ Visualization Manager:
         objects/link_uniform/1:
           Value: false
         objects/link_uniform/2:
-          Value: false
+          Value: true
+        objects/link_uniform/3:
+          Value: true
         objects/node_uniform/1:
           Value: false
         objects/node_uniform/2:
           Value: false
         objects/node_uniform/3:
-          Value: false
+          Value: true
         objects/node_uniform/4:
-          Value: false
+          Value: true
         preferred_orientation:
           Value: false
         shoulder_link:
           Value: false
         table_frame:
           Value: false
-        test:
-          Value: false
-        test_place_link:
-          Value: false
         tool0_controller:
           Value: false
         upper_arm_link:
@@ -227,10 +194,14 @@ Visualization Manager:
               {}
           camera_rgb_frame:
             camera_rgb_optical_frame:
+              ar_marker_0:
+                {}
               obj_link_uniform_1:
                 {}
               obj_link_uniform_2:
                 {}
+              obj_link_uniform_3:
+                {}
               obj_node_uniform_1:
                 {}
               obj_node_uniform_2:
@@ -240,43 +211,7 @@ Visualization Manager:
               obj_node_uniform_4:
                 {}
           world:
-            01 Center:
-              {}
-            C1 01 Over:
-              {}
-            C1 02 Pick:
-              {}
-            C1 03 Lift:
-              {}
-            C1 04 Place:
-              {}
-            C1 05 Escape:
-              {}
-            C1 11 Over:
-              {}
-            C1 12 Pick:
-              {}
-            C1 13 Lift:
-              {}
-            C1 14 Place:
-              {}
-            C1 21 Over:
-              {}
-            C1 22 Pick:
-              {}
-            C1 23 Avoid:
-              {}
-            C1 24 Over:
-              {}
-            C1 25 Place:
-              {}
-            C3 01 Over:
-              {}
-            C3 02 Pick:
-              {}
-            C3 03 Over:
-              {}
-            C3 04 Pick:
+            00 Home:
               {}
             base_link:
               preferred_orientation:
@@ -289,10 +224,11 @@ Visualization Manager:
                         wrist_3_link:
                           ee_link:
                             endpoint:
-                              endpoint_marker:
-                                {}
-                              gripper_center:
-                                {}
+                              {}
+                            endpoint_marker:
+                              {}
+                            gripper_center:
+                              {}
                             gripper_link:
                               {}
                             marker_link:
@@ -303,6 +239,8 @@ Visualization Manager:
               {}
             objects/link_uniform/2:
               {}
+            objects/link_uniform/3:
+              {}
             objects/node_uniform/1:
               {}
             objects/node_uniform/2:
@@ -311,10 +249,6 @@ Visualization Manager:
               {}
             objects/node_uniform/4:
               {}
-            test:
-              {}
-            test_place_link:
-              {}
       Update Interval: 0
       Value: true
     - Alpha: 0.5
@@ -645,6 +579,36 @@ Visualization Manager:
       Radius: 0.01
       Reference Frame: <Fixed Frame>
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.002
+      Style: Spheres
+      Topic: /SPServer/points_out
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -669,32 +633,32 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.07482
+      Distance: 0.655068
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.265794
-        Y: 0.0850819
-        Z: 0.055867
+        X: 0.723894
+        Y: 0.0879182
+        Z: 0.196104
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.754796
+      Pitch: 0.269797
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.3483
+      Yaw: 0.080116
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: true
+    collapsed: false
   Height: 1151
-  Hide Left Dock: true
+  Hide Left Dock: false
   Hide Right Dock: true
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000002ae00000446fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000001b00000446000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002d7000000c90000001600ffffff000000010000010f00000378fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000378000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000063e0000003efc0100000002fb0000000800540069006d006502000000020000046d0000063e00000043fb0000000800540069006d00650100000000000004500000000000000000000006400000044600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002ae00000446fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000001b00000446000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002d7000000c90000001600ffffff000000010000010f00000378fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000378000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000063e0000003efc0100000002fb0000000800540069006d006502000000020000046d0000063e00000043fb0000000800540069006d006501000000000000045000000000000000000000038c0000044600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

diff --git a/costar_bringup/launch/config/dvrk.launch b/costar_bringup/launch/config/dvrk.launch
@@ -0,0 +1,41 @@
+<launch>
+  <!-- Smartmove params -->
+  <group ns="costar">
+    <rosparam param="home">[0.30, -1.33, -1.80, -0.27, 1.50, 1.60]</rosparam>
+    <group ns="robot">
+      <rosparam param="has_gripper">false</rosparam>
+      <rosparam param="has_planning_scene">false</rosparam>
+      <rosparam param="dof">6</rosparam>
+      <rosparam param="base_link">PSM1_psm_base_link</rosparam>
+      <rosparam param="end_link">PSM1_tool_tip_link_virtual</rosparam>
+    </group>
+    <group ns="smartmove">
+      <rosparam param="available_objects">
+        - sander_makita
+        - node_uniform
+        - link_uniform
+        - drill
+        - table
+      </rosparam>
+      <rosparam param="regions">
+        - in_back_of
+        - in_front_of
+        - right_of
+        - left_of
+        - up_from
+        - down_from
+      </rosparam>
+      <!-- Unused reference
+          - ar_marker_0
+          - ar_marker_1
+          - ar_marker_2
+          - ar_marker_3
+          - table_frame
+      -->
+      <rosparam param="references">
+        - world
+      </rosparam>
+    </group>
+  </group>
+</launch>
+
diff --git a/costar_bringup/launch/config/iiwa14.launch b/costar_bringup/launch/config/iiwa14.launch
@@ -10,6 +10,8 @@
       <rosparam param = "joint_weights">[1.0, 1.0, 1.0, 1.0, 1.0, 1.0]</rosparam>
       <rosparam param = "translation_weight">1.0</rosparam>
       <rosparam param = "rotation_weight">0.2</rosparam>
+      <rosparam param = "has_planning_scene">true</rosparam>
+      <rosparam param = "has_gripper">true</rosparam>
     </group>
     <group ns="smartmove">
       <rosparam param="available_objects">

diff --git a/costar_bringup/launch/config/ur5.launch b/costar_bringup/launch/config/ur5.launch
@@ -10,6 +10,8 @@
       <rosparam param = "joint_weights">[1.0, 1.0, 1.0, 1.0, 1.0, 1.0]</rosparam>
       <rosparam param = "translation_weight">1.0</rosparam>
       <rosparam param = "rotation_weight">1.0</rosparam>
+      <rosparam param = "has_planning_scene">true</rosparam>
+      <rosparam param = "has_gripper">true</rosparam>
     </group>
     <group ns="smartmove">
       <rosparam param="available_objects">

diff --git a/costar_bringup/launch/dvrk_psm.launch b/costar_bringup/launch/dvrk_psm.launch
@@ -13,6 +13,8 @@
   <!-- CoSTAR bringup -->
   <include file="$(find costar_bringup)/launch/utilities.launch"/>
   <include file="$(find costar_robot_manager)/launch/simple_psm_driver.launch"/>
+  <include file="$(find costar_bringup)/launch/config/dvrk.launch"/>
+
 
   <!-- Interactive Marker -->
   <node name="instructor_marker" pkg="costar_robot_manager" type="instructor_marker.py"/>

diff --git a/costar_bringup/scripts/reset_files.sh b/costar_bringup/scripts/reset_files.sh
@@ -0,0 +1,5 @@
+#!/usr/bin/env bash
+
+mv $HOME/.costar user_$1_files
+git clone [email protected]:cpaxton/costar_files.git $HOME/.costar --branch study2
+
diff --git a/costar_component/src/costar_component/component.py b/costar_component/src/costar_component/component.py
@@ -23,9 +23,9 @@ def make_service_proxy(self, name, srv_t, use_namespace=True):
       service_name = os.path.join(self.namespace, name)
     else:
       service_name = name
-    rospy.loginfo("Connecting to service with name: %s"%service_name)
+    rospy.logerr("Connecting to service with name: %s"%service_name)
     rospy.wait_for_service(service_name)
-    rospy.loginfo("Connected to service successfully.")
+    rospy.logerr("Connected to service successfully.")
     return rospy.ServiceProxy(service_name,srv_t)
 
   def __init__(self, name, namespace):
@@ -38,4 +38,4 @@ def __init__(self, name, namespace):
   etc.
   '''
   def tick(self):
-    raise RuntimeError('tick() not implemented in abstract component!')
+    raise RuntimeError('tick() not implemented in abstract component!')
diff --git a/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py b/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py
@@ -73,5 +73,6 @@ def status_cb(self,msg):
       if self.verbose:
           rospy.loginfo(self.statusInterpreter(msg))
       self.predicator.handle(msg)
+      self.predicator.tick()