Merge pull request #505 from irvs/integration

added a subtask_grasp service for task scheduler

tms_rc/tms_rc_smartpal/smartpal_virtual_control/src/smartpal5_virtual_control.cpp

@@ -81,7 +81,7 @@ double g_x = 3.0;
 double g_y = 4.0;
 double g_t = 0.0;
 double g_jR[7] = {0.0,-0.08,0.0,0.0,0.0,0.0,0.0};
-double g_jL[7] = {0.0,0.08,0.0,0.0,0.0,0.0,0.0};
+double g_jL[7] = {0.0, 0.08,0.0,0.0,0.0,0.0,0.0};
 
 double g_gripper_right = -0.3;
 double g_gripper_left  =  0.3;

tms_rp/smartpal5_arm_navigation/scripts/subtask_grasp.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 
 """
-    grasp.py - Version 0.0.1 2015-09-18
+    subtask_grasp.py - Version 0.0.2 2015-09-20
 
     Command the gripper to grasp a target object and move it to a new location, all
     while avoiding simulated obstacles.
@@ -33,18 +33,16 @@
 from copy import deepcopy
 from tms_msg_db.msg import TmsdbStamped, Tmsdb
 from tms_msg_db.srv import *
+from tms_msg_rp.srv import *
 
 GROUP_NAME_ARM = 'l_arm'
 GROUP_NAME_GRIPPER = 'l_gripper'
 
 GRIPPER_FRAME = 'l_end_effector_link'
-
 GRIPPER_OPEN = [-1.0]
 GRIPPER_CLOSED = [-0.7]
 GRIPPER_NEUTRAL = [0.0]
-
 GRIPPER_JOINT_NAMES = ['l_gripper_thumb_joint']
-
 GRIPPER_EFFORT = [1.0]
 
 REFERENCE_FRAME = 'world_link'
@@ -56,10 +54,16 @@ def __init__(self):
         rospy.init_node('subtask_grasp')
         rospy.on_shutdown(self.shutdown)
 
+        self.grasp_srv = rospy.Service('subtask_grasp', rp_grasp, self.graspSrvCallback)
+
+    def graspSrvCallback(self, req):
+        rospy.loginfo("Received the service call!")
+        rospy.loginfo(req)
+
         temp_dbdata = Tmsdb()
         target = Tmsdb()
 
-        temp_dbdata.name = 'chipstar_red'
+        temp_dbdata.id = req.object_id
 
         rospy.wait_for_service('tms_db_reader')
         try:
@@ -107,7 +111,6 @@ def __init__(self):
         # Give the scene a chance to catch up
         rospy.sleep(2)
 
-#
         target_id = 'chipstar_red'
         scene.remove_world_object(target_id)
         scene.remove_attached_object(GRIPPER_FRAME, target_id)
@@ -116,23 +119,20 @@ def __init__(self):
 
         arm.set_named_target('l_arm_init')
         arm.go()
-        print('test1')
-
-        # Open the gripper to the neutral position
         gripper.set_joint_value_target(GRIPPER_NEUTRAL)
         gripper.go()
 
         rospy.sleep(1)
-        print('test2')
 
-        target_size = [(res.tmsdb[0].offset_x*2), (res.tmsdb[0].offset_y*2), (res.tmsdb[0].offset_z*2)]
+        # target_size = [(target.offset_x*2), (target.offset_y*2), (target.offset_z*2)]
+        target_size = [0.03, 0.03, 0.12]
         target_pose = PoseStamped()
         target_pose.header.frame_id = REFERENCE_FRAME
-        target_pose.pose.position.x = res.tmsdb[0].x
-        target_pose.pose.position.y = res.tmsdb[0].y
-        target_pose.pose.position.z = res.tmsdb[0].z + res.tmsdb[0].offset_z
-        # q = quaternion_from_euler(res.tmsdb[0].rr, res.tmsdb[0].rp, res.tmsdb[0].ry)
-        q = quaternion_from_euler(0, 0, -1.57079633)
+        target_pose.pose.position.x = target.x
+        target_pose.pose.position.y = target.y
+        target_pose.pose.position.z = target.z + target.offset_z
+        # q = quaternion_from_euler(target.rr, target.rp, target.ry)
+        q = quaternion_from_euler(0, 0, 0)
         target_pose.pose.orientation.x = q[0]
         target_pose.pose.orientation.y = q[1]
         target_pose.pose.orientation.z = q[2]
@@ -142,24 +142,6 @@ def __init__(self):
 
         rospy.sleep(2)
 
-        self.setColor(target_id, 1, 0, 0, 1.0)
-        self.sendColors()
-#
-
-        # Set the support surface name to the table object
-        # arm.set_support_surface_name(table_id)
-        # target_id = target.name
-        # target_size = [(target.offset_x*2), (target.offset_y*2), (target.offset_z*2)]
-        # target_pose = PoseStamped()
-        # target_pose.header.frame_id = REFERENCE_FRAME
-        # target_pose.pose.position.x = target.x
-        # target_pose.pose.position.y = target.y
-        # target_pose.pose.position.z = target.z + target.offset_z
-        # q = quaternion_from_euler(target.rr, target.rp, target.ry)
-        # target_pose.pose.orientation.x = q[0]
-        # target_pose.pose.orientation.y = q[1]
-        # target_pose.pose.orientation.z = q[2]
-        # target_pose.pose.orientation.w = q[3]
         print('test2-1')
         # Initialize the grasp pose to the target pose
         grasp_pose = target_pose
@@ -189,24 +171,16 @@ def __init__(self):
             if result != MoveItErrorCodes.SUCCESS:
                 scene.remove_attached_object(GRIPPER_FRAME, target_id)
 
+        ret = rp_graspResponse()
         # If the pick was successful, attempt the place operation
-        if result != MoveItErrorCodes.SUCCESS:
+        if result == MoveItErrorCodes.SUCCESS:
+            rospy.loginfo("Success the pick operation")
+            ret.result = True
+        else:
             rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
+            ret.result = False
 
-        # Return the arm to the "resting" pose stored in the SRDF file
-        arm.set_named_target('l_arm_init')
-        arm.go()
-
-        # Open the gripper to the neutral position
-        # gripper.set_joint_value_target(GRIPPER_NEUTRAL)
-        # gripper.go()
-        scene.remove_attached_object(GRIPPER_FRAME, target_id)
-
-        rospy.sleep(1)
-
-        # Shut down MoveIt cleanly and exit the script
-        moveit_commander.roscpp_shutdown()
-        moveit_commander.os._exit(0)
+        return ret
 
     # Get the gripper posture as a JointTrajectory
     def make_gripper_posture(self, joint_positions):
@@ -358,43 +332,15 @@ def make_places(self, init_pose):
         # Return the list
         return places
 
-    # Set the color of an object
-    def setColor(self, name, r, g, b, a = 0.9):
-        # Initialize a MoveIt color object
-        color = ObjectColor()
-
-        # Set the id to the name given as an argument
-        color.id = name
-
-        # Set the rgb and alpha values given as input
-        color.color.r = r
-        color.color.g = g
-        color.color.b = b
-        color.color.a = a
-
-        # Update the global color dictionary
-        self.colors[name] = color
-
-    # Actually send the colors to MoveIt!
-    def sendColors(self):
-        # Initialize a planning scene object
-        p = PlanningScene()
-
-        # Need to publish a planning scene diff
-        p.is_diff = True
-
-        # Append the colors from the global color dictionary
-        for color in self.colors.values():
-            p.object_colors.append(color)
-
-        # Publish the scene diff
-        self.scene_pub.publish(p)
-
     def shutdown(self):
         rospy.loginfo("Stopping the node")
+        # Shut down MoveIt cleanly and exit the script
+        moveit_commander.roscpp_shutdown()
+        moveit_commander.os._exit(0)
 
 if __name__ == "__main__":
     try:
         SubTaskGrasp()
+        rospy.spin()
     except rospy.ROSInterruptException:
         rospy.loginfo("subtask_grasp node terminated.")

tms_ts/tms_ts_subtask/include/tms_ts_subtask/tms_ts_subtask.h

@@ -5,6 +5,7 @@
 #include <tms_msg_ts/ts_state_control.h>
 #include <tms_msg_rp/rp_cmd.h>
 #include <tms_msg_rp/rp_arrow.h>
+#include <tms_msg_rp/rp_grasp.h>
 #include <tms_msg_rp/rps_voronoi_path_planning.h>
 #include <tms_msg_rp/rps_goal_planning.h>
 #include <tms_msg_rp/rps_joint_angle.h>
@@ -93,7 +94,7 @@ class TmsRpSubtask
 
   // for thread
   bool move(SubtaskData sd); // 9001
-//	bool grasp(SubtaskData sd); // 9002
+  bool grasp(SubtaskData sd); // 9002
 //	bool give(SubtaskData sd); // 9003
 //	bool open_ref(void);  // 9004
 //	bool close_ref(void); // 9005
@@ -104,6 +105,7 @@ class TmsRpSubtask
   ros::ServiceClient get_data_client_;
   ros::ServiceClient sp5_control_client_;
   ros::ServiceClient sp5_virtual_control_client;
+  ros::ServiceClient  subtask_grasp_client;
 //	ros::ServiceClient kxp_virtual_control_client;
 //	ros::ServiceClient kxp_mbase_client;
 //	ros::ServiceClient v_kxp_mbase_client;