def place_aruco(self, string_operation):
rospy.sleep(2.0)
#self.prepare_robot()
if string_operation == "place":
# pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
# Place the object back to its position
rospy.loginfo("Gonna place near where it was")
pick_g = PickUpPoseGoal()
pick_g.object_pose.pose.position.x = 0.85
pick_g.object_pose.pose.position.y = 0.00
pick_g.object_pose.pose.position.z = 1.00
# pick_g.object_pose.pose.orientation = aruco_ps.pose.position
# pick_g.object_pose.pose.position = aruco_ps.pose.position
# pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
pick_g.object_pose.pose.position.z += 0.05
self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
def __init__(self):
# initialise state variables
self._reset()
# manipulation services
s = rospy.Service('/gaan/manipulate', Manipulate,
self.manipulateHandler)
# manipulation clients
self.play_motion_client = SimpleActionClient('/play_motion',
PlayMotionAction)
rospy.loginfo("Waiting for Action Server...")
self.play_motion_client.wait_for_server()
self.playmotion_goal = PlayMotionGoal()
self.playmotion_goal.skip_planning = False
self.playmotion_goal.priority = 0 # Optional
self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction)
self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction)
self.pickup_goal = PickUpPoseGoal()
self.pickup_goal.object_pose.header.frame_id = "base_footprint"
# end initialisation
rospy.loginfo("Ready to manipulate!")
def place_aruco(self, string_operation):
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an put position")
aruco_pose = rospy.wait_for_message('/Perception/Place_Pose',
PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
aruco_pose.pose.position.x = 0.0
aruco_pose.pose.position.y = 0.0
aruco_pose.pose.position.z = 0.0
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
#ps.pose.position.x = 0.9
#ps.pose.position.y = 0.0
#ps.pose.position.z = 0.8
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
if string_operation == "place":
rospy.loginfo("Setting cube pose based on bottle detection")
pick_g.object_pose.pose.position.x = aruco_ps.pose.position.x + 0.95
pick_g.object_pose.pose.position.y = aruco_ps.pose.position.y
pick_g.object_pose.pose.position.z = aruco_ps.pose.position.z + 1.0
# pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
rospy.loginfo("place pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna place:" + str(pick_g))
self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
def place(self, goal_pose):
# move arm to desired pose
# get goal and plan path
# rospy.loginfo('Planning path to new goal...')
# self.group.set_pose_target(goal_pose)
# plan = self.group.plan()
# # # visualize path in rviz
# # rospy.loginfo('Visualizing in RViz...')
# # display_trajectory = moveit_msgs.msg.DisplayTrajectory()
# # display_trajectory.trajectory_start = self.robot.get_current_state()
# # display_trajectory.trajectory.append(plan)
# # self.display_trajectory_publisher.publish(display_trajectory)
# # rospy.sleep(2) # allow time for visualization
# # perform movement
# self.group.go(wait=True)
# print('Path planning and movement successful!')
# # clear current goal
# self.group.stop()
# self.group.clear_pose_targets()
# open gripper so object falls out
# self.open_gripper()
# Remove leading slash from frame id
goal_pose.header.frame_id = self.strip_leading_slash(
goal_pose.header.frame_id)
rospy.loginfo("Goal received:\n" + str(goal_pose))
# Create and initialize pickup goal with position from goal pose
pick_g = PickUpPoseGoal()
pick_g.object_pose.pose.position = goal_pose.pose.position
# rospy.loginfo("goal pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
rospy.loginfo("Placing object at goal pose")
self.place_as.send_goal_and_wait(pick_g)
def pickup(self, goal_pose):
'''
Move the gripper to goal_pose and pick up the object at that position.
Parameters:
goal_pose: PoseStamped object indicating the position of the object
to be picked up. Currently the frame of the object has to
be 'base_footprint'
'''
# Remove leading slash from frame id
goal_pose.header.frame_id = self.strip_leading_slash(
goal_pose.header.frame_id)
rospy.loginfo("Goal received:\n" + str(goal_pose))
self.prepare_robot()
rospy.sleep(2.0)
# Create and initialize pickup goal with position from goal pose
# TODO: if the goal pose frame is not in base, need to transform
pick_g = PickUpPoseGoal()
pick_g.object_pose.pose.position = goal_pose.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
# rospy.loginfo("goal pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
# Send pickup goal to pick and place server
rospy.loginfo("Attempting to pick up:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
# Check that the pickup was successful
# TODO: what to do if pickup wasn't successful??
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr(
"Failed to pick, not trying further - error code was {}".
format(str(moveit_error_dict[result.error_code])))
# return
# move arm to final position
self.lift_torso(height=0.2)
def place_aruco(self, string_operation):
rospy.sleep(2.0)
#self.prepare_robot()
if string_operation == "place":
# pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
# Place the object back to its position
rospy.loginfo("Gonna place near where it was")
pick_g = PickUpPoseGoal()
pick_g.object_pose.pose.position.x = 0.85
pick_g.object_pose.pose.position.y = 0.00
pick_g.object_pose.pose.position.z = 1.00
# pick_g.object_pose.pose.orientation = aruco_ps.pose.position
# pick_g.object_pose.pose.position = aruco_ps.pose.position
# pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
pick_g.object_pose.pose.position.z += 0.05
self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
# Raise arm
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
rospy.loginfo("Done!")
self.place_finish.wait_for_service()
finish_flag = send_flags()
finish_flag.Request.flag = 1
self.place_finish.call(finish_flag)
rospy.loginfo("call speak done.")
def pick_aruco(self, string_operation):
self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tf_l.getLatestCommonTime(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
aruco_ps = self.tf_l.transformPose("/base_footprint", ps)
transform_ok = True
except tf.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tf_l.getLatestCommonTime(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
if string_operation == "pick":
rospy.loginfo("Setting cube pose based on ArUco detection")
pick_g.object_pose.pose.position = aruco_ps.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
# Move torso to its maximum height
self.lift_torso()
# Raise arm
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
# Place the object back to its position
rospy.loginfo("Gonna place near where it was")
pick_g.object_pose.pose.position.z += 0.05
self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
def pick_aruco(self, string_operation):
self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
# get aruco pose here: from marker
aruco_pose = rospy.wait_for_message('/cylinder_detector/cylinder_pose',
PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
print("aruco pose: ", aruco_pose)
# # manually input aruco pose
# aruco_pose = PoseStamped()
# aruco_pose.header.frame_id = 'base_footprint'
# # # original aruco pose
# # aruco_pose.pose.position.x = 0.528450879403
# # aruco_pose.pose.position.y = -0.0486955713869
# # aruco_pose.pose.position.z = 0.922035729832
# # 0.545863 0.049354 0.724881
# aruco_pose.pose.position.x = 0.545863
# aruco_pose.pose.position.y = 0.049354
# aruco_pose.pose.position.z = 0.724881 + 0.05
# aruco_pose.pose.orientation.x = 0.5
# aruco_pose.pose.orientation.y = 0.5
# aruco_pose.pose.orientation.z = 0.5
# aruco_pose.pose.orientation.w = 0.5
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
if string_operation == "pick":
rospy.loginfo("Setting cube pose based on ArUco detection")
pick_g.object_pose.pose.position = aruco_ps.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
# save the original pose ##########
original_pose = deepcopy(aruco_pose)
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
rospy.sleep(2.0)
# Move torso to its maximum height
self.lift_torso()
rospy.sleep(2.0)
# aruco_pose.pose.position.x = 0.733477 - 0.2
# aruco_pose.pose.position.y = 0.057126 + 0.07
# aruco_pose.pose.position.z += 0.2 # 0.2 is the allowance for pouring
pour_pose = deepcopy(aruco_pose)
pour_pose.pose.position.x = 0.733477 - 0.2
pour_pose.pose.position.y = 0.057126 + 0.07
pour_pose.pose.position.z += 0.2
"""
The following are good good_candidates for pose selection (in eular angles):
[[0, 0, 0], [0, 0, 270], [0, 180, 0], [0, 180, 180] - no, [0, 180, 270], [0, 270, 0], [0, 270, 180], [0, 270, 270]]
"""
# some more successful grasp candidates
# x, y, z, w = eular_to_q(0,270,180)
# x, y, z, w = eular_to_q(0,270,270)
# get pick_pose:
# x, y, z, w = get_pose(args)
pour_pose.pose.orientation.x = -0.5
pour_pose.pose.orientation.y = 0
pour_pose.pose.orientation.z = 0
pour_pose.pose.orientation.w = 1.2
success = False
while success == False:
rospy.sleep(2.0)
# x_e, y_e, z_e = random.choice([(0,270,180), (0,270,270)])
# x, y, z, w = eular_to_q(x_e, y_e, z_e)
# pour_pose.pose.orientation.x = x
# pour_pose.pose.orientation.y = y
# pour_pose.pose.orientation.z = z
# pour_pose.pose.orientation.w = w
result = cartesian_move_to(pour_pose, True)
rospy.loginfo("Success of trajectory: " + str(result))
# define a goal tolerance for replanning and manipulation
x_arm, y_arm, z_arm = arm_pose()
x_aim = pour_pose.pose.position.x
y_aim = pour_pose.pose.position.y
z_aim = pour_pose.pose.position.z
goal_deviation = eular_dist(x_arm, y_arm, z_arm, x_aim, y_aim,
z_aim)
rospy.loginfo("Deviation from target pose: " +
str(goal_deviation))
# pour_pose.pose.position.x -= 0.0001
# pour_pose.pose.position.y -= 0.0001
if result > 0.9 and goal_deviation < 0.05:
success = True
# x, y, z = q_to_eular(-0.5, 0, 0, 1.2)
# rospy.loginfo("End effector eular angles: " + str([x,y,z]))
rospy.sleep(3.0)
# pour liquid
self.turn_wrist()
rospy.sleep(5.0)
# turn wrist back to original pose
self.turn_wrist(pour=False)
rospy.sleep(3.0)
pour_pose.pose.position.x += 0.2
pour_pose.pose.position.y += 0.2
success = False
while success == False:
# x_e, y_e, z_e = random.choice([(0,270,180), (0,270,270)])
# x, y, z, w = eular_to_q(x_e, y_e, z_e)
# pour_pose.pose.orientation.x = x
# pour_pose.pose.orientation.y = y
# pour_pose.pose.orientation.z = z
# pour_pose.pose.orientation.w = w
result = cartesian_move_to(pour_pose, True)
rospy.loginfo("Success of trajectory: " + str(result))
# define a goal tolerance for replanning and manipulation
x_arm, y_arm, z_arm = arm_pose()
x_aim = pour_pose.pose.position.x
y_aim = pour_pose.pose.position.y
z_aim = pour_pose.pose.position.z
goal_deviation = eular_dist(x_arm, y_arm, z_arm, x_aim, y_aim,
z_aim)
rospy.loginfo("Deviation from target pose: " +
str(goal_deviation))
pour_pose.pose.position.x -= 0.0001
pour_pose.pose.position.y -= 0.0001
if result > 0.9 and goal_deviation < 0.05:
success = True
rospy.sleep(3.0)
# # return bottle to original pose
# x, y, z, w = eular_to_q(0,270,180)
# # x, y, z, w = eular_to_q(0,270,270)
# original_pose.pose.position.z = pour_pose.pose.position.z
# original_pose.pose.orientation.x = -0.5
# original_pose.pose.orientation.y = 0
# original_pose.pose.orientation.z = 0
# original_pose.pose.orientation.w = 1.2
# success = False
# while success==False:
# result = cartesian_move_to(original_pose, True)
# rospy.loginfo("success of trajectory: "+str(result))
# # define a goal tolerance for replanning and manipulation
# x_arm, y_arm, z_arm = arm_pose()
# x_aim = original_pose.pose.position.x
# y_aim = original_pose.pose.position.y
# z_aim = original_pose.pose.position.z
# goal_deviation = eular_dist(x_arm, y_arm, z_arm, x_aim, y_aim, z_aim)
# rospy.loginfo("Deviation from target pose: "+str(goal_deviation))
# original_pose.pose.position.x -= 0.01
# original_pose.pose.position.y -= 0.01
# if result > 0.9 and goal_deviation < 0.1:
# success = True
# rospy.loginfo("Gonna place near where it was")
# ps = PoseStamped()
# ps.pose.position = original_pose.pose.position
# ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", original_pose.header.frame_id)
# ps.header.frame_id = original_pose.header.frame_id
# transform_ok = False
# while not transform_ok and not rospy.is_shutdown():
# try:
# transform = self.tfBuffer.lookup_transform("base_footprint",
# ps.header.frame_id,
# rospy.Time(0))
# aruco_ps = do_transform_pose(ps, transform)
# transform_ok = True
# except tf2_ros.ExtrapolationException as e:
# rospy.logwarn(
# "Exception on transforming point... trying again \n(" +
# str(e) + ")")
# rospy.sleep(0.01)
# ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", original_pose.header.frame_id)
# pick_g = PickUpPoseGoal()
# arm=MoveGroupCommander('arm')
# arm.allow_replanning(True)
# end_effector_link=arm.get_end_effector_link()
# arm.set_goal_position_tolerance(0.03)
# arm.set_goal_orientation_tolerance(0.025)
# arm.allow_replanning(True)
# reference_frame='base_footprint'
# arm.set_pose_reference_frame(reference_frame)
# arm.set_planning_time(5)
# rospy.sleep(2)
# start_pose=arm.get_current_pose(end_effector_link).pose
# rospy.loginfo("End effector start pose: " + str(start_pose))
# x = start_pose.orientation.x
# y = start_pose.orientation.y
# z = start_pose.orientation.z
# w = start_pose.orientation.w
# x, y, z = q_to_eular(x, y, z, w)
# rospy.loginfo("End effector eular angles: " + str([x,y,z]))
# # Raise arm
# rospy.loginfo("Moving arm to a safe pose")
# pmg = PlayMotionGoal()
# pmg.motion_name = 'pick_final_pose'
# pmg.skip_planning = False
# self.play_m_as.send_goal_and_wait(pmg)
# rospy.loginfo("Raise object done.")
# Place the object back to its position
rospy.loginfo("Gonna place near where it was ...........")
# pick_g.object_pose.pose.position.z += 0.05
pick_g.object_pose.pose.position.z += 0.05
pick_g.object_pose.pose.position.y += 0.2
self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
def pick_aruco(self, string_operation):
if string_operation == "place":
# Place the object back to its position
rospy.loginfo("Gonna place near where it was")
self.pick_g.object_pose.pose.position.z += 0.05
self.place_as.send_goal_and_wait(self.pick_g)
rospy.loginfo("Done!")
return
self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
self.ps = PoseStamped()
self.ps.pose.position = aruco_pose.pose.position
self.ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
self.ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform("base_footprint",
self.ps.header.frame_id,
rospy.Time(0))
self.aruco_ps = do_transform_pose(self.ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
self.ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
self.pick_g = PickUpPoseGoal()
if string_operation == "pick":
rospy.loginfo("Setting cube pose based on ArUco detection")
self.pick_g.object_pose.pose.position = self.aruco_ps.pose.position
self.pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(self.pick_g))
self.pick_g.object_pose.header.frame_id = 'base_footprint'
self.pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(self.pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(self.pick_g))
self.pick_as.send_goal_and_wait(self.pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
# Move torso to its maximum height
self.lift_torso()
def pick_aruco(self, string_operation):
# self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
# aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
# test_pose = rospy.Publisher('/Mypose',PoseStamped, queue_size=100)
# aruco_pose = rospy.wait_for_messgae('/Point3D',PostStamped)
#aruco_pose = rospy.wait_for_message('/Mypose', PoseStamped)
while self.ifreceive == False:
rospy.loginfo("Waiting Info")
pick_ready = final_msg_srv.msg.done_pick()
pick_ready.done_pick = True
self.pub_ready.publish(pick_ready)
rospy.sleep(0.1)
self.ifreceive = False
pick_ready = final_msg_srv.msg.done_pick()
pick_ready.done_pick = False
self.pub_ready.publish(pick_ready)
rospy.loginfo("Mypose has been recevied.")
aruco_pose = self.test_call
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
if string_operation == "pick":
rospy.loginfo("Setting cube pose based on ArUco detection")
pick_g.object_pose.pose.position = aruco_ps.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
self.pick_fail.done = True
self.pub_pick_fail.publish(self.pick_fail)
return
# Move torso to its maximum height
# self.lift_torso()
# Raise arm
self.postgrasp_place()
# ======================= def new function to pick it otherplace else=============================
#Raise arm
#rospy.loginfo("Unfold arm safely")
# pmg = PlayMotionGoal()
# pmg.motion_name = 'pregrasp'
# pmg.skip_planning = False
# self.play_m_as.send_goal_and_wait(pmg)
# rospy.loginfo("Done.")
self.safe_place()
pick_msg = final_msg_srv.msg.done()
pick_msg.done = True
self.pub_done.publish(pick_msg)
rospy.loginfo("Pick part has done. Navigation begins")
if string_operation == "power":
self.postgrasp_place()
# ======================= def new function to pick it otherplace else=============================
if string_operation == "place":
# #rospy.lofinfo("Gonna move the object to home place")
# #rospy.loginfo("Gonna place near where it was")
# #pick_g.object_pose.pose.position.x = 1.00
# #pick_g.object_pose.pose.position.y = 1.00
# #pick_g.object_pose.pose.position.z = 1.00
# #self.place_as.send_goal_and_wait(pick_g)
# rospy.loginfo("Done!")
#
# # Place the object back to its position
# #rospy.loginfo("Gonna place near where it was")
# #pick_g.object_pose.pose.position.z += 0.05
# #self.place_as.send_goal_and_wait(pick_g)
# #rospy.loginfo("Done!")
# rospy.loginfo("Setting cube pose based on ArUco detection")
# pick_g.object_pose.pose.position = aruco_ps.pose.position
# pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
#
# rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
#
# pick_g.object_pose.header.frame_id = 'base_footprint'
# pick_g.object_pose.pose.orientation.z += 0.05
# pick_g.object_pose.pose.orientation.w = 1.0
# self.detected_pose_pub.publish(pick_g.object_pose)
# rospy.loginfo("Gonna pick:" + str(pick_g))
# self.place_as.send_goal_and_wait(pick_g)
# rospy.loginfo("Done!")
#
# result = self.place_as.get_result()
# if str(moveit_error_dict[result.error_code]) != "SUCCESS":
# rospy.logerr("Failed to pick, not trying further")
# return
#
# # Move torso to its maximum height
# # self.lift_torso()
# # Raise arm
# self.look_around()
# self.postgrasp_place()
# self.safe_place()
self.open_hand()
time.sleep(5)
self.safe_place()
def pick_aruco(self, string_operation):
self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
#rospy.sleep(5.0)
aruco_pose = rospy.wait_for_message('/object_detection/Pick_Pose',
PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
if string_operation == "pick":
rospy.loginfo("Setting cube pose based on bottle detection")
pick_g.object_pose.pose.position.x = aruco_ps.pose.position.x + 0.04
pick_g.object_pose.pose.position.y = aruco_ps.pose.position.y - 0.04
pick_g.object_pose.pose.position.z = aruco_ps.pose.position.z + 0.14
# pick_g.object_pose.pose.orientation = aruco_ps.pose.position
# pick_g.object_pose.pose.position = aruco_ps.pose.position
# pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
rospy.loginfo("######before")
# Move torso to its maximum height
self.lift_torso()
# Raise arm
#rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
data = rospy.wait_for_message('/wrist_ft', WrenchStamped)
force_x = np.absolute(data.wrench.force.x)
force_y = np.absolute(data.wrench.force.y)
force_z = np.absolute(data.wrench.force.z)
force_abs = force_x + force_y + force_z
rospy.loginfo("I have force_abs")
if force_abs >= 10.0:
rospy.loginfo("I got the bag.")
#data = rospy.Subscriber('/wrist_ft', WrenchStamped, self.force_value)
rospy.loginfo("Raise object done.")
rospy.loginfo("#######end")
def pick_aruco(self, aruco_pose):
# rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
# aruco_pose = PoseStamped()
# aruco_pose.header.frame_id = "base_footprint"
# aruco_pose.pose.position.x = 0.492726171711
# aruco_pose.pose.position.y = -0.140456914268
# aruco_pose.pose.position.z = 0.133627714861
# aruco_pose.pose.orientation.x = 0.544020678534
# aruco_pose.pose.orientation.y = 0.507215268602
# aruco_pose.pose.orientation.z = 0.500950386556
# aruco_pose.pose.orientation.w = 0.442518793765
# aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
self.prepare_robot()
rospy.sleep(2.0)
# rospy.loginfo("spherical_grasp_gui: Transforming from frame: {} to 'base_footprint'".format(aruco_pose.header.frame_id))
# ps = PoseStamped()
# ps.pose.position = aruco_pose.pose.position
# ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
# ps.header.frame_id = aruco_pose.header.frame_id
# transform_ok = False
# while not transform_ok and not rospy.is_shutdown():
# try:
# transform = self.tfBuffer.lookup_transform("base_footprint",
# ps.header.frame_id,
# rospy.Time(0))
# aruco_ps = do_transform_pose(ps, transform)
# transform_ok = True
# except tf2_ros.ExtrapolationException as e:
# rospy.logwarn(
# "Exception on transforming point... trying again \n(" +
# str(e) + ")")
# rospy.sleep(0.01)
# ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
# pick_g = PickUpPoseGoal()
pick_g = PickUpPoseGoal()
aruco_ps = aruco_pose
# if string_operation == "pick":
rospy.loginfo("Setting cube pose based on ArUco detection")
pick_g.object_pose.pose.position = aruco_ps.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr(
"Failed to pick, not trying further - error code was {}".
format(str(moveit_error_dict[result.error_code])))
# return
# move to final position
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
# Move torso to its maximum height
# self.lift_torso()
#self.lower_torso()
# # Raise arm
# rospy.loginfo("Moving arm to a safe pose")
# pmg = PlayMotionGoal()
# pmg.motion_name = 'pick_final_pose'
# pmg.skip_planning = False
# self.play_m_as.send_goal_and_wait(pmg)
# rospy.loginfo("Raise object done.")
# # Place the object back to its position
# rospy.loginfo("Gonna place near where it was")
# pick_g.object_pose.pose.position.z += 0.05
# self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
def pick_aruco(self, string_operation):
self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
# self.turn_hand() # experimental function
# get aruco pose here: from marker
aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
if string_operation == "pick":
rospy.loginfo("Setting cube pose based on ArUco detection")
pick_g.object_pose.pose.position = aruco_ps.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
# some_pose=arm.get_current_pose(end_effector_link).pose
# rospy.loginfo("current hand pose "+str(some_pose))
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
# Move torso to its maximum height
self.lift_torso()
aruco_pose.pose.position.x += 0.2
aruco_pose.pose.position.y += 0.2
aruco_pose.pose.position.z += 0.2
drop_pose = deepcopy(aruco_pose)
# drop_pose.pose.position.x += 0.2
# drop_pose.pose.position.y += 0.3
# drop_pose.pose.position.z += 0.2
# optimize_goal_pose(drop_pose)
"""
The following are good good_candidates for pose selection (in eular angles):
[[0, 0, 0], [0, 0, 270], [0, 180, 0], [0, 180, 180] - no, [0, 180, 270], [0, 270, 0], [0, 270, 180], [0, 270, 270]]
"""
x, y, z, w = eular_to_q(0, 270, 180)
# x, y, z, w = eular_to_q(0,270,270)
drop_pose.pose.orientation.x = -0.5
drop_pose.pose.orientation.y = 0
drop_pose.pose.orientation.z = 0
drop_pose.pose.orientation.w = 1.2
cartesian_move_to(drop_pose, True)
x, y, z = q_to_eular(-0.5, 0, 0, 1.2)
rospy.loginfo("End effector eular angles: " + str([x, y, z]))
rospy.sleep(3)
self.open_gripper()
def pick_aruco(self, string_operation):
if self.first_go:
self.prepare_robot()
self.pick_scene.randomize_object_poses()
rospy.sleep(2.0)
pick_g = PickUpPoseGoal()
if string_operation == "pick":
pick_order = sorted(
self.pick_scene.pick_scene.box_objects_in_scene,
key=lambda object: object.object_pose.pose.position.y,
reverse=False)
print pick_order
for idx in range(
len(self.pick_scene.pick_scene.box_objects_in_scene)):
object = pick_order[idx]
pick_g.object_to_pick = object.object_name
pick_g.pick_scene.box_objects_in_scene = self.pick_scene.pick_scene.box_objects_in_scene
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
# Move torso to its maximum height
self.lift_torso()
# Raise arm
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
# Rotate to kit
rotation_speed = 0.628 #spin ot 0.3 rads/s
twist_command = Twist()
twist_command.linear.x = 0.0
twist_command.linear.y = 0.0
twist_command.linear.z = 0.0
twist_command.angular.x = 0.0
twist_command.angular.y = 0.0
twist_command.angular.z = rotation_speed
cycles_per_second = 3
rate = rospy.Rate(cycles_per_second)
number_of_cycles = 3.14 / rotation_speed * cycles_per_second
current_cycle = 0
rospy.loginfo("Spin to kit tray")
while (not rospy.is_shutdown()) and (current_cycle <
number_of_cycles + 2):
self.cmd_vel_pub.publish(twist_command)
rate.sleep()
current_cycle += 1
rospy.sleep(2.0)
#Place object
place_pose = PoseStamped()
place_pose.header.frame_id = 'base_footprint'
place_pose.header.stamp = rospy.get_rostime()
place_pose.pose = self.get_model_state("table",
"base_footprint")
place_pose.pose.position.z += 0.01 + 0.125
place_pose.pose.position.y = place_pose.pose.position.y - 0.25 + 0.1 * idx
rospy.loginfo(place_pose)
place_pose.pose.orientation.x = 0.0
place_pose.pose.orientation.y = 0.0
place_pose.pose.orientation.z = 0.0
place_pose.pose.orientation.w = 1.0
rospy.loginfo("Place object on tray")
pick_g.object_pose = place_pose
pick_g.object_to_pick = object.object_name #Action naming is currently wrong. It should be called object to place
pick_g.pick_scene = self.pick_scene.pick_scene #objects to be removed from scene
self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
#Rotate back
current_cycle = 0
while (not rospy.is_shutdown()) and (current_cycle <
number_of_cycles + 2):
self.cmd_vel_pub.publish(twist_command)
rate.sleep()
current_cycle += 1
def pick_aruco(self, string_operation):
#rospy.sleep(2.0)
if string_operation == "pick":
self.prepare_robot()
# Detect object
rospy.loginfo(
"spherical_grasp_gui: Waiting for an aruco detection")
aruco_pose = rospy.wait_for_message('/aruco_single/pose',
PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.pose.orientation = aruco_pose.pose.orientation
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
rospy.loginfo("Setting cube pose based on ArUco detection")
pick_g.object_pose.pose.position = aruco_ps.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
# rospy.loginfo("Moving arm to a safe pose")
# pmg = PlayMotionGoal()
# pmg.motion_name = 'pick_final_pose'
# pmg.skip_planning = False
# self.play_m_as.send_goal_and_wait(pmg)
# rospy.loginfo("Raise object done.")
# Move to safe
home = False
i = 0
#self.move_back(5)
while not home and i < 3:
home = self.move_arm_home()
i += 1
rospy.sleep(0.5)
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return TriggerResponse(False, "Failed to pick")
self.pick_g = pick_g
return TriggerResponse(True, "Succeeded")
elif string_operation == "place":
if not self.pick_g:
rospy.logerr("Failed to place, no object position known")
return TriggerResponse(
False, "Failed to place, no object position known")
pick_g = self.pick_g
# Move torso to its maximum height
self.lift_torso()
# Raise arm
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'prepare_grasp' #'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
# Place the object back to its position
rospy.loginfo("Gonna place near where it was")
pick_g.object_pose.pose.position.z += 0.05
self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
# Move to safe
home = False
i = 0
#self.move_back(5)
while not home and i < 3:
home = self.move_arm_home()
i += 1
rospy.sleep(0.5)
self.pick_g = None
result = self.place_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to place")
return TriggerResponse(False, "Failed to place")
return TriggerResponse(True, "Succeeded")
def pick_aruco(self, string_operation):
self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
if string_operation == "pick":
# Place the arm safe above table
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_approach_pose'
self.play_m_as.send_goal_and_wait(pmg)
pmg.skip_planning = False
rospy.loginfo("Setting ball pose based on Perception")
pick_g.object_pose.pose.position = aruco_ps.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
# Move torso to its maximum height
self.lift_torso()
# Raise arm
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
# Place the arm safe above table
pmg = PlayMotionGoal()
pmg.motion_name = 'home_approach_pose'
self.play_m_as.send_goal_and_wait(pmg)
pmg.skip_planning = False
# Place the object to safe navigation position
rospy.loginfo("Gonna place the ball near my heart..")
pmg = PlayMotionGoal()
pmg.motion_name = 'home_pose'
self.play_m_as.send_goal_and_wait(pmg)
pmg.skip_planning = False
rospy.loginfo("Ready to move !")
if string_operation == "place":
pass
def pick_aruco(self, string_operation):
#self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
#wait for aruco to be detected
#TODO integrate with vision team's code
aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
#PoseStamped is the messgage type published to /detected_aruco_pose
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint",
aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
#Loop until transform is done
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform("base_footprint",
ps.header.frame_id,
rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint",
aruco_pose.header.frame_id)
#define goal that will be sent to pick_and_place_server
pick_g = PickUpPoseGoal()
#argument defined during service call
if string_operation == "pick":
rospy.loginfo("Setting cube pose based on ArUco detection")
pick_g.object_pose.pose.position = aruco_ps.pose.position
pick_g.object_pose.pose.position.z -= 0.1
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
#pick_as action client sends the PickUpPoseGoal
#rospy.wait_for_message('/pick_it', String)
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
# Move torso to its maximum height
#self.lift_torso()
# Raise arm
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")