def __init__(self):
rospy.loginfo("Initalizing PickAndPlaceServer...")
self.sg = SphericalGrasps()
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
rospy.loginfo("Connecting to place AS")
self.place_ac = SimpleActionClient('/place', PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
self.scene = PlanningSceneInterface()
rospy.loginfo("Connecting to /get_planning_scene service")
self.scene_srv = rospy.ServiceProxy('/get_planning_scene',
GetPlanningScene)
self.scene_srv.wait_for_service()
rospy.loginfo("Connected.")
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
# Get the object size
self.object_height = rospy.get_param('~object_height')
self.object_width = rospy.get_param('~object_width')
self.object_depth = rospy.get_param('~object_depth')
# Get the links of the end effector exclude from collisions
self.links_to_allow_contact = rospy.get_param(
'~links_to_allow_contact', None)
if self.links_to_allow_contact is None:
rospy.logwarn(
"Didn't find any links to allow contacts... at param ~links_to_allow_contact"
)
else:
rospy.loginfo("Found links to allow contacts: " +
str(self.links_to_allow_contact))
self.pick_as = SimpleActionServer('/pickup_pose',
PickUpPoseAction,
execute_cb=self.pick_cb,
auto_start=False)
self.pick_as.start()
self.place_as = SimpleActionServer('/place_pose',
PickUpPoseAction,
execute_cb=self.place_cb,
auto_start=False)
self.place_as.start()
class PickAndPlaceServer(object):
def __init__(self):
rospy.loginfo("Initalizing PickAndPlaceServer...")
self.sg = SphericalGrasps()
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
rospy.loginfo("Connecting to place AS")
self.place_ac = SimpleActionClient('/place', PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
self.scene = PlanningSceneInterface()
rospy.loginfo("Connecting to /get_planning_scene service")
self.scene_srv = rospy.ServiceProxy(
'/get_planning_scene', GetPlanningScene)
self.scene_srv.wait_for_service()
rospy.loginfo("Connected.")
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy(
'/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
# Get the object size
self.object_height = rospy.get_param('~object_height')
self.object_width = rospy.get_param('~object_width')
self.object_depth = rospy.get_param('~object_depth')
# Get the links of the end effector exclude from collisions
self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None)
if self.links_to_allow_contact is None:
rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact")
else:
rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact))
self.pick_as = SimpleActionServer(
'/pickup_pose', PickUpPoseAction,
execute_cb=self.pick_cb, auto_start=False)
self.pick_as.start()
self.place_as = SimpleActionServer(
'/place_pose', PickUpPoseAction,
execute_cb=self.place_cb, auto_start=False)
self.place_as.start()
def pick_cb(self, goal):
"""
:type goal: PickUpPoseGoal
"""
error_code = self.grasp_object(goal.object_pose)
p_res = PickUpPoseResult()
p_res.error_code = error_code
if error_code != 1:
self.pick_as.set_aborted(p_res)
else:
self.pick_as.set_succeeded(p_res)
def place_cb(self, goal):
"""
:type goal: PickUpPoseGoal
"""
error_code = self.place_object(goal.object_pose)
p_res = PickUpPoseResult()
p_res.error_code = error_code
if error_code != 1:
self.place_as.set_aborted(p_res)
else:
self.place_as.set_succeeded(p_res)
def wait_for_planning_scene_object(self, object_name='part'):
rospy.loginfo(
"Waiting for object '" + object_name + "'' to appear in planning scene...")
gps_req = GetPlanningSceneRequest()
gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES
part_in_scene = False
while not rospy.is_shutdown() and not part_in_scene:
# This call takes a while when rgbd sensor is set
gps_resp = self.scene_srv.call(gps_req)
# check if 'part' is in the answer
for collision_obj in gps_resp.scene.world.collision_objects:
if collision_obj.id == object_name:
part_in_scene = True
break
else:
rospy.sleep(1.0)
rospy.loginfo("'" + object_name + "'' is in scene!")
def grasp_object(self, object_pose):
rospy.loginfo("Removing any previous 'part' object")
self.scene.remove_attached_object("arm_tool_link")
self.scene.remove_world_object("part")
self.scene.remove_world_object("table")
self.scene.remove_world_object("left")
self.scene.remove_world_object("right")
self.scene.remove_world_object("up")
self.scene.remove_world_object("back")
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0) # Removing is fast
rospy.loginfo("Adding new 'part' object")
rospy.loginfo("Object pose: %s", object_pose.pose)
#Add object description in scene
self.scene.add_box("part", object_pose, (self.object_depth, self.object_width, self.object_height))
rospy.loginfo("Second%s", object_pose.pose)
table_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
table_height = 1.8 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8
table_width = 1.8
table_depth = 0.8
table_pose.pose.position.z += -(2*self.object_height)/2 -table_height/2 +0.11 #before also -0.1 and was object width instead of object height
table_height -= 0.1 #remove few milimeters to prevent contact between the object and the table
self.scene.add_box("table", table_pose, (table_depth, table_width, table_height))
# # We need to wait for the object part to appear
self.wait_for_planning_scene_object()
self.wait_for_planning_scene_object("table")
right_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
right_height = 1.8 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8
right_width = 1.8
right_depth = 0.8
right_pose.pose.position.y += 0.5 #before also -0.1 and was object width instead of object height
right_width = 0.2 #remove few milimeters to prevent contact between the object and the table
self.scene.add_box("right", right_pose, (right_depth, right_width, right_height))
# # We need to wait for the object part to appear
self.wait_for_planning_scene_object()
self.wait_for_planning_scene_object("right")
left_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
left_height = 0.8 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8
left_width = 0.8
left_depth = 0.2
left_pose.pose.position.x += 0.7 #before also -0.1 and was object width instead of object height
self.scene.add_box("back", left_pose, (left_depth, left_width, left_height))
self.wait_for_planning_scene_object()
self.wait_for_planning_scene_object("back")
up_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
up_height = 0.2 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8
up_width = 1.8
up_depth = 0.8
up_pose.pose.position.z += 0.5 #before also -0.1 and was object width instead of object height
self.scene.add_box("up", up_pose, (up_depth, up_width, up_height))
back_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
back_height = 1.8 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8
back_width = 1.8
back_depth = 0.8
back_pose.pose.position.y -= 0.5 #before also -0.1 and was object width instead of object height
back_width = 0.2 #remove few milimeters to prevent contact between the object and the table
self.scene.add_box("left", back_pose, (back_depth, back_width, back_height))
self.wait_for_planning_scene_object()
self.wait_for_planning_scene_object("left")
# compute grasps
possible_grasps = self.sg.create_grasps_from_object_pose(object_pose)
self.pickup_ac
goal = createPickupGoal(
"arm", "part", object_pose, possible_grasps, self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.logdebug("Using torso result: " + str(result))
rospy.loginfo(
"Pick result: " +
str(moveit_error_dict[result.error_code.val]))
return result.error_code.val
def place_object(self, object_pose):
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
possible_placings = self.sg.create_placings_from_object_pose(
object_pose)
# Try only with arm
rospy.loginfo("Trying to place using only arm")
goal = createPlaceGoal(
object_pose, possible_placings, "arm", "part", self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.loginfo(str(moveit_error_dict[result.error_code.val]))
if str(moveit_error_dict[result.error_code.val]) != "SUCCESS":
rospy.loginfo(
"Trying to place with arm and torso")
# Try with arm and torso
goal = createPlaceGoal(
object_pose, possible_placings, "arm", "part", self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.logerr(str(moveit_error_dict[result.error_code.val]))
# print result
rospy.loginfo(
"Result: " +
str(moveit_error_dict[result.error_code.val]))
rospy.loginfo("Removing previous 'part' object")
self.scene.remove_world_object("part")
return result.error_code.val
class PickAndPlaceServer(object):
def __init__(self):
rospy.loginfo("Initalizing PickAndPlaceServer...")
self.sg = SphericalGrasps()
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
'''rospy.loginfo("Connecting to place AS")
self.place_ac = SimpleActionClient('/place', PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")'''
self.scene = PlanningSceneInterface()
rospy.loginfo("Connecting to /get_planning_scene service")
self.scene_srv = rospy.ServiceProxy(
'/get_planning_scene', GetPlanningScene)
self.scene_srv.wait_for_service()
rospy.loginfo("Connected.")
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy(
'/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
def set_height(height):
self.object_height = height.data
rospy.loginfo("Object height set to " + str(self.object_height))
rospy.Subscriber("height", Float64, set_height)
# Get the object size
self.object_height = rospy.get_param('~object_height')
self.object_width = rospy.get_param('~object_width')
self.object_depth = rospy.get_param('~object_depth')
# Get the links of the end effector exclude from collisions
self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None)
if self.links_to_allow_contact is None:
rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact")
else:
rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact))
self.pick_as = SimpleActionServer(
'/pickup_pose', PickUpPoseAction,
execute_cb=self.pick_cb, auto_start=False)
self.pick_as.start()
'''self.place_as = SimpleActionServer(
'/place_pose', PickUpPoseAction,
execute_cb=self.place_cb, auto_start=False)
self.place_as.start()'''
def pick_cb(self, goal):
"""
:type goal: PickUpPoseGoal
"""
error_code = self.grasp_object(goal.object_pose)
p_res = PickUpPoseResult()
p_res.error_code = error_code
if error_code != 1:
self.pick_as.set_aborted(p_res)
else:
self.pick_as.set_succeeded(p_res)
'''def place_cb(self, goal):
"""
:type goal: PickUpPoseGoal
"""
error_code = self.place_object(goal.object_pose)
p_res = PickUpPoseResult()
p_res.error_code = error_code
if error_code != 1:
self.place_as.set_aborted(p_res)
else:
self.place_as.set_succeeded(p_res)'''
def wait_for_planning_scene_object(self, object_name='part'):
rospy.loginfo(
"Waiting for object '" + object_name + "'' to appear in planning scene...")
gps_req = GetPlanningSceneRequest()
gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES
part_in_scene = False
while not rospy.is_shutdown() and not part_in_scene:
# This call takes a while when rgbd sensor is set
gps_resp = self.scene_srv.call(gps_req)
# check if 'part' is in the answer
for collision_obj in gps_resp.scene.world.collision_objects:
if collision_obj.id == object_name:
part_in_scene = True
break
else:
rospy.sleep(1.0)
rospy.loginfo("'" + object_name + "'' is in scene!")
#HEARTS tried inputting multiple cubes to approximate cylinder
#TODO re-investigate if time allows
#def rotate_quat(self,quat,angle_deg,axis_euler):
'''
Rotate about axis_euler by angle_deg,the quaternion quat,
returned as a quaternion
'''
#euler = tf.transformations.euler_from_quaternion(quat)
#euler(2) = euler(0.5*np.pi)
def grasp_object(self, object_pose):
rospy.loginfo("Removing any previous 'part' object")
self.scene.remove_attached_object("arm_tool_link")
self.scene.remove_world_object("part")
self.scene.remove_world_object("table")
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0) # Removing is fast
rospy.loginfo("Adding new 'part' object")
rospy.loginfo("Object pose: %s", object_pose.pose)
part_pose = copy.deepcopy(object_pose)
part_pose.pose.position.z -= self.object_height/4
#Add object description in scene
self.scene.add_box("part", part_pose, (self.object_depth, self.object_width,
self.object_height))
# Add rotated boxes (for collision avoidance)
#box1_pose = copy.deepcopy(object_pose)
rospy.loginfo("Second%s", object_pose.pose)
table_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
table_height = object_pose.pose.position.z - self.object_height
table_width = 1.8
table_depth = 0.5
#TODO update so it works when detecting centre of object
table_pose.pose.position.z += -(self.object_height)/2 -table_height/2
table_height -= 0.008 #remove few milimeters to prevent contact between the object and the table
self.scene.add_box("table", table_pose, (table_depth, table_width, table_height))
# # We need to wait for the object part to appear
self.wait_for_planning_scene_object("part")
self.wait_for_planning_scene_object("table")
# compute grasps
possible_grasps = self.sg.create_grasps_from_object_pose(object_pose)
self.pickup_ac
goal = createPickupGoal(
"arm_torso", "part", object_pose, possible_grasps, self.links_to_allow_contact)
rospy.loginfo("Waiting for final pick instruction")
rospy.wait_for_message('/pick_it',String)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.logdebug("Using torso result: " + str(result))
rospy.loginfo(
"Pick result: " +
str(moveit_error_dict[result.error_code.val]))
return result.error_code.val
'''def place_object(self, object_pose):