Merge pull request #9 from SharedAutonomyToolkit/clear_table

Clear table

.gitignore

@@ -0,0 +1,2 @@
+*~
+*.pyc

assemble_kinect/scripts/extract_images.py

@@ -36,7 +36,7 @@
     bag = rosbag.Bag(infile)
     for topic, msg, t in bag.read_messages():
         if topic == "assemble_kinect_response":
-            outfile = outfile_base + str(msg_count) + '.pgm'
+            outfile = outfile_base + '_' + str(msg_count) + '.pgm'
             msg_count = msg_count + 1
             resp = msg
             cv_image = cv_bridge.CvBridge().imgmsg_to_cv(resp.image, "bgr8")

assemble_kinect/src/assemble_kinect.cpp

@@ -73,6 +73,9 @@ KinectAssembler::~KinectAssembler() {
 
 // TODO: Do I want fancier logic here where we can only send a given set of data once?
 void KinectAssembler::approxCB(const ImageConstPtr& image, const ImageConstPtr& depth, const CameraInfoConstPtr& info, const PointCloud2ConstPtr& points) {
+  if (!has_data_) {
+    ROS_INFO("assemble_kinect has data set!");
+  }
   resp_.header.stamp = ros::Time::now();
   resp_.header.seq = num_frames_;
   num_frames_++;
@@ -81,6 +84,7 @@ void KinectAssembler::approxCB(const ImageConstPtr& image, const ImageConstPtr&
   resp_.info = *info;
   resp_.points = *points;
   has_data_ = true;
+
 }
 
 bool KinectAssembler::serviceCB(shared_autonomy_msgs::KinectAssembly::Request &req,

clear_table/CMakeLists.txt

@@ -4,7 +4,7 @@ project(clear_table)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS actionlib rospy sensor_msgs shared_autonomy_msgs smach smach_ros)
+find_package(catkin REQUIRED COMPONENTS actionlib rospy cluster_grasp_planner moveit_msgs sensor_msgs shape_msgs shared_autonomy_msgs smach smach_ros)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS actionlib rospy sensor_msgs shared_auton
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 #######################################
 ## Declare ROS messages and services ##

clear_table/package.xml

@@ -40,15 +40,23 @@
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+
   <build_depend>actionlib</build_depend>
+  <build_depend>cluster_grasp_planner</build_depend>
+  <build_depend>moveit_msgs</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>sensor_msgs</build_depend>
+  <build_depend>shape_msgs</build_depend>
   <build_depend>shared_autonomy_msgs</build_depend>
   <build_depend>smach</build_depend>
   <build_depend>smach_ros</build_depend>
+
   <run_depend>actionlib</run_depend>
+  <run_depend>cluster_grasp_planner</run_depend>
+  <run_depend>moveit_msgs</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>sensor_msgs</run_depend>
+  <run_depend>shape_msgs</run_depend>
   <run_depend>shared_autonomy_msgs</run_depend>
   <run_depend>smach</run_depend>
   <run_depend>smach_ros</run_depend>

clear_table/scripts/clear_table_moveit.py

@@ -0,0 +1,129 @@
+#! /usr/bin/env python
+
+import rospy
+
+import smach
+import smach_ros
+
+from actionlib_msgs.msg import GoalStatus
+from clear_table.grasp_handler import GraspHandler
+from clear_table.moveit_handler import PickupHandler, DropHandler
+from clear_table.scene_handler import SceneHandler
+from clear_table.sensor_handler import SensorHandler
+
+class Home(smach.State):
+     def __init__(self):
+         smach.State.__init__(self, outcomes=['at_home'])
+     def execute(self, userdata):
+         return 'at_home'
+
+class Segment(smach.State):
+     def __init__(self):
+         smach.State.__init__(self, outcomes=['done_segmenting', 'segmentation_failed', 'restart_segmentation'],
+                              output_keys=['object_points', 'object_name'])
+
+         self.sensors = SensorHandler()
+         self.scene = SceneHandler()
+
+     def execute(self, userdata):
+
+         # we're willing to wait 15seconds to get the data
+         kinect_data = self.sensors.get_kinect(15)
+         if not kinect_data:
+             return 'segmentation_failed'
+
+         (state, result) = self.sensors.get_segmentation(kinect_data)
+         if state != GoalStatus.SUCCEEDED:
+             return 'segmentation_failed'
+
+         points = self.sensors.get_point_cloud(kinect_data, result.mask)
+         if not points:
+             return 'segmentation_failed'
+
+         userdata.object_points = points
+         userdata.object_name = 'obj1'
+
+         # need to set up the planning scene ...
+         self.scene.clear_scene()
+         self.scene.add_table()
+         self.scene.add_object('obj1', points)
+
+         print 'successful segmentation!'
+         return 'done_segmenting'
+
+class GenerateGrasps(smach.State):
+    def __init__(self):
+        smach.State.__init__(self,
+                             outcomes = ['no_grasps', 'grasps_found'], 
+                             input_keys = ['object_points'],
+                             output_keys = ['grasps'])
+        self.gh = GraspHandler()
+
+    def execute(self, userdata):
+        grasps = self.gh.get_grasps(userdata.object_points)
+        if grasps:
+            userdata.grasps = grasps
+            return 'grasps_found'
+        else:
+            return 'no_grasps'
+
+class Pickup(smach.State):
+    def __init__(self):
+        smach.State.__init__(self, outcomes=['pickup_failed', 'pickup_done'],
+                             input_keys=['object_name', 'grasps'])
+        self.pickup = PickupHandler()
+
+    def execute(self, userdata):
+        success = self.pickup.run_pick(userdata.object_name, userdata.grasps)
+        if success:
+            return 'pickup_done'
+        else:
+            return 'pickup_failed'
+
+class Drop(smach.State):
+    def __init__(self):
+        smach.State.__init__(self,
+                             outcomes = ['drop_done'],
+                             input_keys = ['object_name'])
+        self.drop = DropHandler()
+        self.scene = SceneHandler()
+
+    def execute(self, userdata):
+        self.drop.run_drop(userdata.object_name)
+        self.scene.remove_object(userdata.object_name)
+        return 'drop_done'
+
+def main():
+
+    rospy.init_node('clear_table')
+    sm = smach.StateMachine(outcomes=['DONE'])
+
+    with sm:
+        smach.StateMachine.add('HOME', Home(),
+                               transitions = {'at_home':'SEGMENT'})
+
+        smach.StateMachine.add('SEGMENT', Segment(),
+                               transitions = {'done_segmenting':'GENERATE_GRASPS',
+                                              'segmentation_failed':'SEGMENT',
+                                              'restart_segmentation':'SEGMENT'})
+        smach.StateMachine.add('GENERATE_GRASPS', GenerateGrasps(),
+                               transitions = {'no_grasps':'HOME', 
+                                              'grasps_found':'PICKUP'})
+        smach.StateMachine.add('PICKUP', Pickup(),
+                               transitions = {'pickup_failed':'HOME',
+                                              'pickup_done':'DROP'})
+        smach.StateMachine.add('DROP', Drop(),
+                               transitions = {'drop_done':'HOME'})
+
+
+    sis = smach_ros.IntrospectionServer('clear_table_sis', sm, '/SM_ROOT')
+    sis.start()
+
+    outcome = sm.execute()
+
+    rospy.spin()
+    sis.stop()
+
+if __name__ == "__main__":
+    main()
+