Feature/ui

.gitignore

@@ -4,3 +4,5 @@
 *.zip
 *.bag
 *.swo
+*.dat
+*.pcd

.travis.yml

@@ -47,8 +47,6 @@ dist: trusty
 sudo: required
 language:
   - generic
-cache:
-  - apt
 
 # Configuration variables. All variables are global now, but this can be used to
 # trigger a build matrix for different ROS distributions if desired.
@@ -59,6 +57,7 @@ env:
     - CI_SOURCE_PATH=$(pwd)
     - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options
     - ROS_PARALLEL_JOBS='-j8 -l6'
+    - PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages
 #- ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall
 
 ################################################################################

costar_bringup/config/iiwa_s_model_camera.rviz

@@ -6,17 +6,14 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
-        - /TF1
         - /TF1/Frames1
         - /TF1/Tree1
-        - /PlanningScene1
         - /PlanningScene1/Scene Geometry1
         - /PlanningScene1/Scene Robot1
-        - /PointCloud22
         - /Planned Path1
-        - /PointCloud23
+        - /PointCloud24
       Splitter Ratio: 0.413174
-    Tree Height: 948
+    Tree Height: 852
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -87,27 +84,23 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
-    - Class: rviz/Image
-      Enabled: false
-      Image Topic: /camera/color/image_raw
-      Max Value: 1
-      Median window: 5
-      Min Value: 0
-      Name: Image
-      Normalize Range: true
-      Queue Size: 2
-      Transport Hint: raw
-      Unreliable: false
-      Value: false
     - Class: rviz/TF
       Enabled: true
       Frame Timeout: 15
       Frames:
         00 Home:
           Value: true
         All Enabled: false
-        ar_marker_0:
-          Value: false
+        a1:
+          Value: true
+        a2:
+          Value: true
+        a3:
+          Value: true
+        a4:
+          Value: true
+        ar_marker_2:
+          Value: true
         base:
           Value: false
         base_link:
@@ -136,33 +129,13 @@ Visualization Manager:
           Value: false
         marker_link:
           Value: false
-        obj_link_uniform_1:
-          Value: true
-        obj_link_uniform_2:
-          Value: true
-        obj_link_uniform_3:
-          Value: true
-        obj_node_uniform_1:
+        place1:
           Value: true
-        obj_node_uniform_2:
+        place2:
           Value: true
-        obj_node_uniform_3:
+        place3:
           Value: true
-        obj_node_uniform_4:
-          Value: true
-        objects/link_uniform/1:
-          Value: false
-        objects/link_uniform/2:
-          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: true
-        objects/node_uniform/4:
+        place4:
           Value: true
         preferred_orientation:
           Value: false
@@ -194,25 +167,19 @@ 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:
-                {}
-              obj_node_uniform_3:
-                {}
-              obj_node_uniform_4:
+              ar_marker_2:
                 {}
           world:
             00 Home:
               {}
+            a1:
+              {}
+            a2:
+              {}
+            a3:
+              {}
+            a4:
+              {}
             base_link:
               preferred_orientation:
                 {}
@@ -235,19 +202,13 @@ Visualization Manager:
                               {}
               table_frame:
                 {}
-            objects/link_uniform/1:
-              {}
-            objects/link_uniform/2:
+            place1:
               {}
-            objects/link_uniform/3:
+            place2:
               {}
-            objects/node_uniform/1:
+            place3:
               {}
-            objects/node_uniform/2:
-              {}
-            objects/node_uniform/3:
-              {}
-            objects/node_uniform/4:
+            place4:
               {}
       Update Interval: 0
       Value: true
@@ -609,6 +570,36 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
+    - 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: true
+      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.005
+      Style: Spheres
+      Topic: /pcl_color_segmenter/visualized_cloud_topic
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -633,32 +624,30 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.655068
+      Distance: 0.612647
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.723894
-        Y: 0.0879182
-        Z: 0.196104
+        X: 0.727214
+        Y: 0.00867184
+        Z: 0.164382
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.269797
+      Pitch: 0.369797
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.080116
+      Yaw: 0.460128
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1151
+  Height: 1055
   Hide Left Dock: false
   Hide Right Dock: true
-  Image:
-    collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000002ae00000446fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000001b00000446000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002d7000000c90000001600ffffff000000010000010f00000378fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000378000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000063e0000003efc0100000002fb0000000800540069006d006502000000020000046d0000063e00000043fb0000000800540069006d006501000000000000045000000000000000000000038c0000044600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002ae000003e6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000001b000003e6000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000338000000c90000000000000000000000010000010f00000378fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000378000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000063e0000003efc0100000002fb0000000800540069006d006502000000020000046d0000063e00000043fb0000000800540069006d006501000000000000045000000000000000000000038c000003e600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -668,5 +657,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1600
-  X: 0
-  Y: 25
+  X: 1759
+  Y: 0

costar_bringup/launch/config/ur5.launch

@@ -1,4 +1,7 @@
 <launch>
+  <arg name="use_scene_parsing_pose" />
+  <arg name="use_external_segmenter" />
+
   <!-- Smartmove params -->
   <group ns="costar">
     <group ns="robot">
@@ -20,6 +23,7 @@
         - link_uniform
         - drill
         - table
+        - wood_cube
       </rosparam>
       <rosparam param="regions">
         - in_back_of
@@ -39,6 +43,22 @@
       <rosparam param="references">
         - world
       </rosparam>
+
+      <group if="$(arg use_scene_parsing_pose)">
+        <rosparam param="scene_parsing_pose">true</rosparam>
+      </group>
+      <group unless="$(arg use_scene_parsing_pose)">
+        <rosparam param="scene_parsing_pose">false</rosparam>
+      </group>
+
+      <group if="$(arg use_external_segmenter)">
+        <rosparam param="sp_segmenter_namespace">/costar_sp_segmenter</rosparam>
+      </group>
+      <group unless="$(arg use_external_segmenter)">
+        <rosparam param="sp_segmenter_namespace">/costar_perception</rosparam>
+      </group>
+
+      <rosparam param="scene_parsing_namespace">/sequential_scene_parsing</rosparam>
     </group>
   </group>
 </launch>

costar_bringup/launch/predicator_nodes.launch

@@ -3,9 +3,18 @@
   <arg name="output" default="screen"/>
   <arg name="publish_values" default="0"/>
 
+  <!-- <arg name="pose_estimator" default="/costar_sp_segmenter" /> -->
+
   <!-- detected objects node -->
+  <node pkg="detected_object_tf_republisher" name="detected_obj_republisher_node" type="det_obj_republisher.py" />
+
   <node pkg="predicator_detected_objects" name="predicator_detected_objects_node" type="detected_objects_node.py">
-    <remap from="/detected_object_list" to="/SPServer/detected_object_list"/>
+    <remap from="/detected_object_list" to="/costar/detected_object_list"/>
+  </node>
+
+  <!-- detected objects node -->
+  <node pkg="predicator_scene_structure" name="predicator_scene_structure_node" type="scene_structure_node.py">
+    <remap from="/scene_structure_list" to="/sequential_scene_parsing/scene_structure_list"/>
   </node>
 
   <!-- geometry node -->
@@ -25,12 +34,10 @@
     <rosparam param="frames">
       - obj_link_uniform_1
       - obj_link_uniform_2
-      - obj_link_uniform_3
-      - obj_link_uniform_4
       - obj_node_uniform_1
       - obj_node_uniform_2
-      - obj_node_uniform_3
-      - obj_node_uniform_4
+      - obj_wood_cube_1
+      - obj_wood_cube_2
       - obj_sander_makita_1
       - obj_sander_makita_2
       - gbeam_link_1/gbeam_link
@@ -46,11 +53,13 @@
     <rosparam param="reference_frames">
       - world
     </rosparam>
+
+    <remap from="/detected_object_list" to="/costar/detected_object_list" />
 
   </node>
 
   <node pkg="predicator_landmark" name="predicator_landmark_service" type="get_waypoints_service.py">
-    <remap from="/detected_object_list" to="/SPServer/detected_object_list"/>
+    <remap from="/detected_object_list" to="/costar/detected_object_list"/>
   </node>
 
   <node name="predicator_fake_class_node"

costar_bringup/launch/ur5_c_model.launch

@@ -16,14 +16,20 @@
   <arg name="ee_frame" value="ee_link"/>
   <arg name="base_frame" value="base_link"/>
 
+  <arg name="use_scene_parsing_pose" value="false" doc="Use the stable pose generated from the scene parsing algorithm"/>
+  <arg name="use_external_segmenter" value="false" doc="Use external point cloud segmentation for sp_segmenter, e.g. color_nn_segmenter"/>
+
   <include file="$(find costar_bringup)/launch/utilities.launch">
     <arg name="start_robotiq_force_sensor" value="$(arg start_robotiq_force_sensor)"/>
   </include>
 
   <include file="$(find costar_bringup)/launch/predicator_nodes.launch"/>
 
   <!-- Bringup default configuration for the UR5 -->
-  <include file="$(find costar_bringup)/launch/config/ur5.launch"/>
+  <include file="$(find costar_bringup)/launch/config/ur5.launch">
+    <arg name="use_scene_parsing_pose" value="$(arg use_scene_parsing_pose)"/>
+    <arg name="use_external_segmenter" value="$(arg use_external_segmenter)"/>
+  </include>
 
   <group unless="$(arg sim)">
 

costar_gripper/gripper_robotiq/launch/robotiq_c_model_endpoint.launch

@@ -4,6 +4,6 @@
 
   <node pkg="tf" type="static_transform_publisher" name="endpoint_to_marker" args="0.007, 0.096, -0.005 0.035, 0.713, 0.700, 0.025 $(arg ee_frame) /endpoint_marker 10"/> 
 
-  <node pkg="tf" type="static_transform_publisher" name="gripper_center" args=" 0.19 -0.010 -0.010  0  0  0  1   $(arg ee_frame) /gripper_center 10"/> 
+  <node pkg="tf" type="static_transform_publisher" name="gripper_center" args=" 0.19 -0.020 -0.010  0  0  0  1   $(arg ee_frame) /gripper_center 10"/> 
 
 </launch>

costar_instructor/instructor_core/launch/instructor.launch

@@ -5,7 +5,7 @@
 
   <!-- CORE -->
   <node pkg="instructor_core" type="instructor_view.py" name="instructor_view" >
-    <remap from="/detected_object_list" to="/SPServer/detected_object_list"/>
+    <remap from="/detected_object_list" to="/costar/detected_object_list"/>
     <param name="cases" value="$(arg instructor_cases)"/>
   </node>