Merge branch 'devel' into real_robot

src/sorting_demo/launch/sorting_demo.launch

@@ -3,5 +3,5 @@
     <arg name="electric_gripper" value="true"/>
   </include>
 
-  <include file="$(find sorting_demo)/launch/sorting_demo_control.launch"/>
+  <include file="$(find sorting_demo)/launch/robot_sorting_demo_control.launch"/>
 </launch>

src/sorting_demo/src/sorting_demo/cv_detection_head.py

@@ -250,7 +250,7 @@ def test_head_debug():
     """
 
     # Get files
-    path = rospkg.RosPack().get_path('sorting_demo') + "/share/test_head"
+    path = rospkg.RosPack().get_path('sorting_demo') + "/share/test_head_simulator"
     files = [f for f in os.listdir(path) if os.path.isfile(os.path.join(path, f))]
     #print(files)
 

src/sorting_demo/src/sorting_demo/cv_detection_right_hand.py

@@ -329,7 +329,7 @@ def test_right_hand_debug():
     """
 
     # Get files
-    path = rospkg.RosPack().get_path('sorting_demo') + "/share/test_right_hand"
+    path = rospkg.RosPack().get_path('sorting_demo') + "/share/test_right_hand_simulator"
     files = [f for f in os.listdir(path) if os.path.isfile(os.path.join(path, f))]
     #files = ["border.png"]
     #print(files)

src/sorting_demo/src/sorting_demo/task_planner.py

@@ -859,7 +859,7 @@ class TaskPlanner:
             self.moveit_tabletop_pick(target_block).result()
 
             rospy.sleep(0.1)
-            if self.sawyer_robot._gripper.get_position() < 0.03:
+            if self.sawyer_robot._gripper.get_position() < 0.01:
                 rospy.logerr("LOOKS LIKE THE GRASPING FAILED")
                 self.trajectory_planner.clear_attached_object(target_block)
                 self.gripper_state.holding_block = None