class tuckarm(pt.behaviour.Behaviour):
def __init__(self):
rospy.loginfo("Initialising tuck arm behaviour.")
# Set up action client
self.play_motion_ac = SimpleActionClient("/play_motion",
PlayMotionAction)
# personal goal setting
self.goal = PlayMotionGoal()
self.goal.motion_name = 'home'
self.goal.skip_planning = True
# execution checker
self.sent_goal = False
self.finished = False
# become a behaviour
super(tuckarm, self).__init__("Tuck arm!")
def update(self):
# Initialise again if tree has to be repeated
global flag_repeat_tree_1
if flag_repeat_tree_1:
self.sent_goal = False
self.finished = False
flag_repeat_tree_1 = False
# already tucked the arm
if self.finished:
return pt.common.Status.SUCCESS
# command to tuck arm if haven't already
elif not self.sent_goal:
# send the goal
self.play_motion_ac.send_goal(self.goal)
self.sent_goal = True
# tell the tree you're running
return pt.common.Status.RUNNING
# if I was succesful! :)))))))))
elif self.play_motion_ac.get_result():
# than I'm finished!
self.finished = True
return pt.common.Status.SUCCESS
# if failed
elif not self.play_motion_ac.get_result():
return pt.common.Status.FAILURE
# if I'm still trying :|
else:
return pt.common.Status.RUNNING
def main():
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
rospy.loginfo('Waiting for action')
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
client.wait_for_server()
goal = DARRTGoal()
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.arm_name = 'right_arm'
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = 'map'
place_pose_stamped.pose.position.x -= 1.3
place_pose_stamped.pose.position.y -= 0.3
place_pose_stamped.pose.position.z = 0.98
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name = det.table_name,
collision_object_name = goal.pickup_goal.collision_object_name)
goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
goal.min_grasp_distance_from_surface = 0.17
goal.object_sampling_fraction = 0.9
goal.retreat_distance = 0.3
goal.debug_level = 2
goal.do_pause = False
goal.planning_time = 6000
goal.tries = 1
goal.goal_bias = 0.2
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code == result.SUCCESS:
#pickle everything!! great excitement
filename = 'pickplace_and_objects.pck'
rospy.loginfo('Pickling to '+filename)
#the last bit allows us to recreate the planning
pickle.dump([client.get_result(), goal, wi.collision_objects(), wi.get_robot_state()], open(filename, 'wb'))
marray = MarkerArray()
if result.error_code == result.SUCCESS:
for t in result.primitive_trajectories:
marray.markers += vt.trajectory_markers(t, ns='trajectory', resolution=3).markers
for (i, m) in enumerate(marray.markers):
m.id = i
(m.color.r, m.color.g, m.color.b) = vt.hsv_to_rgb(i/float(len(marray.markers))*300.0, 1, 1)
while not rospy.is_shutdown():
pub.publish(marray)
rospy.sleep(0.1)
class IntroduceSelf(smach.State):
def __init__(self,
profession=True,
residence=True,
date_of_birth=True,
timeout=120.0,
action_server='/mdr_actions/introduce_self_action_server'):
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.profession = profession
self.residence = residence
self.date_of_birth = date_of_birth
self.timeout = timeout
self.introduce_self_client = SimpleActionClient(
action_server, IntroduceSelfAction)
self.introduce_self_client.wait_for_server()
def execute(self, userdata):
goal = IntroduceSelfGoal()
goal.profession = self.profession
goal.residence = self.residence
goal.date_of_birth = self.date_of_birth
rospy.loginfo('Calling introduce_self action')
self.introduce_self_client.send_goal(goal)
duration = rospy.Duration.from_sec(self.timeout)
self.introduce_self_client.wait_for_result(duration)
rospy.loginfo('introduce_self call completed')
res = self.introduce_self_client.get_result()
if res and res.success:
return 'succeeded'
else:
return 'failed'
class Terminal(object):
def __init__(self, name='terminal'):
self.client = SimpleActionClient(resolve_name(name), ExchangeAction)
self.client.wait_for_server()
def __del__(self):
self.close()
def close(self):
del self.client
def send(self, mode, text, handler=None, block=True):
goal = ExchangeGoal()
goal.mode = mode
goal.text = text
self.client.send_goal(goal, feedback_cb=handler)
if block:
self.client.wait_for_result()
def write(self, text):
self.send(PRINT, text)
def prompt(self, text):
self.send(LINE, text)
return self.client.get_result().line
def get(self, text, escape, handler, block=True):
callback = lambda feedback: handler(feedback.key)
self.send(KEY, escape + text, callback, block)
class execute_plan(smach.State):
def __init__(self):
smach.State.__init__(self,
outcomes=['success', 'failure', 'preempted'],
input_keys=['already_once_executed', 'plan'],
output_keys=['already_once_executed'])
self.planner_executor_client = SimpleActionClient(
'/task_planning/execute_plan', ExecutePlanAction)
self.planner_executor_client.wait_for_server()
rospy.sleep(0.2)
def execute(self, userdata):
userdata.already_once_executed = True
success = self.execute_plan(userdata.plan)
if not success:
return 'failure'
return 'success'
def execute_plan(self, plan):
goal = ExecutePlanGoal()
goal.plan = plan
self.planner_executor_client.send_goal(goal)
finished = self.planner_executor_client.wait_for_result()
if not finished:
return (False)
res = self.planner_executor_client.get_result()
return res.success
class MoveBase(smach.State):
def __init__(self,
destination_locations,
timeout=120.0,
action_server='/mdr_actions/move_base_safe_server'):
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.destination_locations = list(destination_locations)
self.timeout = timeout
self.action_server = action_server
self.client = SimpleActionClient(action_server, MoveBaseSafeAction)
self.client.wait_for_server()
def execute(self, userdata):
goal = MoveBaseSafeGoal()
for i, destination_location in enumerate(self.destination_locations):
goal.destination_location = destination_location
rospy.loginfo('Sending the base to %s' % destination_location)
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration.from_sec(self.timeout))
res = self.client.get_result()
if res and res.success:
rospy.loginfo('Successfully reached %s' % destination_location)
else:
rospy.logerr('Could not reach %s' % destination_location)
return 'failed'
return 'succeeded'
class RecognizeGenders(smach.State):
def __init__(self, **kwargs):
smach.State.__init__(
self,
outcomes=['succeeded', 'failed'],
input_keys=['image', 'number_of_faces', 'bounding_boxes'])
self.timeout = kwargs.get('timeout', 10)
self.say_topic = kwargs.get('say_topic', '/say')
self.say_pub = rospy.Publisher(self.say_topic, String, queue_size=1)
self.gender_client = SimpleActionClient(
'mdr_actions/gender_recognition_server', GenderRecognitionAction)
self.gender_client.wait_for_server()
def execute(self, userdata):
# Recognize gender
goal = GenderRecognitionGoal()
goal.image = userdata.image
goal.number_of_faces = userdata.number_of_faces
goal.bounding_boxes = userdata.bounding_boxes
self.gender_client.send_goal(goal)
self.gender_client.wait_for_result()
result = self.gender_client.get_result()
men = result.genders.count('man')
women = result.genders.count('woman')
# Say x men and y women
msg = String()
msg.data = 'There are %i men and %i women' % (men, women)
rospy.loginfo(msg.data)
self.say_pub.publish(msg)
return 'succeeded'
def move_base_client():
client = SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
# line - 3, -1, -math.pi/2
theta = eval(sys.argv[3])
quat = quaternion_from_euler(0, 0, theta)
goal.target_pose.pose.position.x = float(sys.argv[1])
goal.target_pose.pose.position.y = float(sys.argv[2])
goal.target_pose.pose.orientation.x = quat[0]
goal.target_pose.pose.orientation.y = quat[1]
goal.target_pose.pose.orientation.z = quat[2]
goal.target_pose.pose.orientation.w = quat[3]
client.send_goal(goal)
wait = client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
return client.get_result()
class plan_task(smach.State):
def __init__(self, mode=PlanGoal.NORMAL):
smach.State.__init__(
self,
outcomes=['success', 'failure'],
input_keys=['domain_file', 'problem_file', 'planner'],
output_keys=['plan'])
self.mode = mode
self.planner_client = SimpleActionClient(
'/mir_task_planning/task_planner_server/plan_task', PlanAction)
self.planner_client.wait_for_server()
rospy.sleep(0.2)
def execute(self, userdata):
goal = PlanGoal(domain_file=userdata.domain_file,
problem_file=userdata.problem_file,
planner=userdata.planner,
mode=self.mode)
self.planner_client.send_goal(goal)
self.planner_client.wait_for_result()
state = self.planner_client.get_state()
result = self.planner_client.get_result()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Found a plan with %i actions",
len(result.plan.plan))
userdata.plan = result.plan
return 'success'
elif state == GoalStatus.ABORTED:
rospy.logerr("Failed to plan")
rospy.sleep(2.0)
return 'failure'
class Gripper():
LEFT = 'l'
RIGHT = 'r'
def __init__(self, direction):
assert (direction == Gripper.LEFT or direction == Gripper.RIGHT)
self.direction = direction
name_space = '/{0}_gripper_controller/gripper_action'.format(
self.direction)
self.gripper_client = SimpleActionClient(name_space,
GripperCommandAction)
self.gripper_client.wait_for_server()
def open_gripper(self, wait=False):
self.change_state(0.08, wait)
def close_gripper(self, wait=False):
self.change_state(0.0, wait)
def change_state(self, value, wait):
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = value
gripper_goal.command.max_effort = 30.0
self.gripper_client.send_goal(gripper_goal)
if wait:
self.gripper_client.wait_for_result()
if not self.gripper_client.get_result().reached_goal:
time.sleep(1)
class ActionTasks:
def __init__(self, script_path):
rospy.init_node('action_testr')
rospy.on_shutdown(self.cleanup)
self.fridge = (Pose(Point(0.295, -2.304, 0.0), Quaternion(0.0, 0.0, -0.715, 0.699)))
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
self.move_to(self.fridge)
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = location
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
#self.client.wait_for_result()
self.client.wait_for_result(rospy.Duration.from_sec(40.0))
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
def cleanup(self):
rospy.loginfo("Shutting down talk node...")
class EnterDoor(smach.State):
def __init__(self,
timeout=120.0,
move_forward_server='/mdr_actions/move_forward_server',
movement_duration=15.,
speed=0.1):
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.timeout = timeout
self.movement_duration = movement_duration
self.speed = speed
self.move_forward_client = SimpleActionClient(move_forward_server,
MoveForwardAction)
self.move_forward_client.wait_for_server()
self.entered = False
def execute(self, userdata):
goal = MoveForwardGoal()
goal.movement_duration = self.movement_duration
goal.speed = self.speed
self.move_forward_client.send_goal(goal)
timeout_duration = rospy.Duration.from_sec(self.timeout)
self.move_forward_client.wait_for_result(timeout_duration)
result = self.move_forward_client.get_result()
if result and result.success:
return 'succeeded'
else:
return 'failed'
class ActionTasks:
def __init__(self, script_path):
rospy.init_node('action_testr')
rospy.on_shutdown(self.cleanup)
self.fridge = (Pose(Point(0, 0, 0.0), Quaternion(0.0, 0.0, 0, 1)))
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
self.move_to(self.fridge)
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = location
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
#self.client.wait_for_result()
self.client.wait_for_result(rospy.Duration.from_sec(40.0))
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
def cleanup(self):
rospy.loginfo("Shutting down talk node...")
def time_plate_main():
rospy.loginfo('Reading file')
[result, goal, collision_objects, robot_state] = pickle.load(open(sys.argv[1], 'r'))
rospy.loginfo('Original starting state was\n'+str(robot_state))
markers = vt.robot_marker(robot_state, ns='robot_starting_state', a = 0.5)
pub = rospy.Publisher('/darrt_planning/robot_state_markers', MarkerArray)
for i in range(10):
rospy.loginfo('Publishing starting state!')
pub.publish(markers)
rospy.sleep(0.1)
wi = WorldInterface()
wi.reset(repopulate=False)
psi = get_planning_scene_interface()
psi.reset()
plate = None
for co in collision_objects:
wi.add_object(co)
if (co.id == goal.pickup_goal.collision_object_name):
plate = co
psi.reset()
ops = PoseStamped()
ops.header = copy.deepcopy(plate.header)
ops.pose = copy.deepcopy(plate.poses[0])
#the old pickup goal may have used base link to generate grasps
#i don't know what we do with things for which we need a point cloud
goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id),
object_pose_stamped=ops, collision_object_name=goal.pickup_goal.collision_object_name,
collision_support_surface_name=goal.pickup_goal.collision_support_surface_name,
desired_grasps=plate_grasps(ops, ops.header.frame_id))
rospy.loginfo('Teleop to initial state. Ready to go?')
raw_input()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal.execute = False
goal.planning_time = 200
goal.tries = 10000
goal.debug_level = 1
goal.do_pause = False
goal.goal_bias = 0.2
# goal.place_goal.place_locations[0].pose.position.x = 1
# goal.place_goal.place_locations[0].pose.position.y = 1
# goal.place_goal.place_locations[0].pose.position.z = 0.86
average_time = 0.0
rospy.loginfo('First place goal is\n'+str(goal.place_goal.place_locations[0]))
rospy.loginfo('File: '+sys.argv[1]+', restart after '+str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error '+str(result.error_code)+'. We had '+str(i)+
' successful trials with an average time of '+str(average_time/float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL '+str(i)+': '+str(result.planning_time))
rospy.loginfo(str(NTRIALS)+' averaged '+str(average_time/float(NTRIALS))+' seconds to solve')
class Planning:
def __init__(self):
self.client = SimpleActionClient('/move_base', MoveBaseAction)
self.goal = MoveBaseGoal()
self.current_goal = 'home'
self.goals = {
'home': np.array([0.0, 0.0]),
'pos1': np.array([1.0, 0.0]),
'pos2': np.array([1.0, 1.0]),
}
def sendGoal(self, x_pos, y_pos, yaw):
if abs(x_pos) <= 0.1:
x_pos = 0.0
if abs(y_pos) <= 0.1:
y_pos = 0.0
# quaternion = transformations.quaternion_from_euler(0, 0, yaw)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = x_pos
goal.target_pose.pose.position.y = y_pos
goal.target_pose.pose.position.z = 0.0
# goal.target_pose.pose.orientation.x = quaternion[0]
# goal.target_pose.pose.orientation.y = quaternion[1]
# goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = 1.0 #quaternion[3]
self.client.send_goal(goal)
self.client.wait_for_result()
print('Reached ' + self.current_goal)
print(self.client.get_result())
def computeNewGoal(self):
if self.current_goal == 'pos2':
self.current_goal = 'home'
elif self.current_goal == 'pos1':
self.current_goal = 'pos2'
elif self.current_goal == 'home':
self.current_goal = 'pos1'
else:
self.current_goal = 'home'
self.sendGoal(self.goals[self.current_goal][0],
self.goals[self.current_goal][1], 0)
def loop(self, rate):
self.sendGoal(self.goals['home'][0], self.goals['home'][1], 0)
while not rospy.is_shutdown():
self.computeNewGoal()
rate.sleep()
class DropService():
def __init__(self):
rospy.init_node(NAME + 'server' , anonymous=True)
self.client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
self.client.wait_for_server()
self.gripper_client.wait_for_server()
self.service = rospy.Service('drop_service', ArmDropService, self.handle_drop)
rospy.loginfo("%s: Ready for Dropping", NAME)
#drop it like it's hot
def handle_drop(self, req):
rospy.loginfo("told to drop")
success = False
#this seems to be as far as it can open (both straight ahead), same vals anh uses
if self.move_gripper(0.2, -0.2):
success = True
return success
#stolen gripper code from Anh Tran's "block_stacking_actions.py" in wubble_blocks
def move_gripper(self, left_finger, right_finger):
goal = SmartArmGripperGoal()
goal.target_joints = [left_finger, right_finger]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_goal_to_finish()
result = self.gripper_client.get_result()
if result.success == False:
rospy.loginfo("Drop failed")
else:
rospy.loginfo("Object Released! Hopefully it isn't sticky")
class Gripper:
LEFT = "l"
RIGHT = "r"
def __init__(self, direction):
assert direction == Gripper.LEFT or direction == Gripper.RIGHT
self.direction = direction
name_space = "/{0}_gripper_controller/gripper_action".format(self.direction)
self.gripper_client = SimpleActionClient(name_space, GripperCommandAction)
self.gripper_client.wait_for_server()
def open_gripper(self, wait=False):
self.change_state(0.08, wait)
def close_gripper(self, wait=False):
self.change_state(0.0, wait)
def change_state(self, value, wait):
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = value
gripper_goal.command.max_effort = 30.0
self.gripper_client.send_goal(gripper_goal)
if wait:
self.gripper_client.wait_for_result()
if not self.gripper_client.get_result().reached_goal:
time.sleep(1)
def pickplace_main():
rospy.loginfo('Waiting for action')
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
wi = WorldInterface()
psi = get_planning_scene_interface()
add_map_tables(wi)
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal = DARRTGoal()
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.lift_distance = 0.3
goal.pickup_goal.arm_name = 'right_arm'
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
place_pose_stamped.pose.position.x = -1.3
place_pose_stamped.pose.position.y = 0.7
if 'graspable' in goal.pickup_goal.collision_object_name:
place_pose_stamped.pose.position.z = 0.8
else:
place_pose_stamped.pose.position.z = 0.75
place_pose_stamped.pose.orientation.w = 1.0
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name = 'serving_table',
collision_object_name = goal.pickup_goal.collision_object_name)
goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
goal.object_sampling_fraction = 0.3
goal.retreat_distance = 0.3
goal.debug_level = 2
goal.do_pause = False
goal.execute = False
goal.planning_time = 30
goal.tries = 10
goal.goal_bias = 0.2
average_time = 0.0
rospy.loginfo('First place goal is\n'+str(goal.place_goal.place_locations[0]))
rospy.loginfo('Restart after '+str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error '+str(result.error_code)+'. We had '+str(i)+
' successful trials with an average time of '+str(average_time/float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL '+str(i)+': '+str(result.planning_time))
rospy.loginfo(str(NTRIALS)+' averaged '+str(average_time/float(NTRIALS))+' seconds to solve')
class Arm:
def __init__(self, arm):
if arm == 'r':
self.arm_client = SAC('r_arm_controller/joint_trajectory_action', JointTrajectoryAction)
self.arm = arm
elif arm == 'l':
self.arm_client = SAC('l_arm_controller/joint_trajectory_action', JointTrajectoryAction)
self.arm = arm
self.arm_client.wait_for_server()
rospy.loginfo('Waiting for server...')
def move(self, angles, time, blocking):
angles = [angles]
times = [time]
timeout=times[-1] + 1.0
goal = JointTrajectoryGoal()
goal.trajectory.joint_names =self.get_joint_names(self.arm)
for (ang, t) in zip(angles, times):
point = JointTrajectoryPoint()
point.positions = ang
point.time_from_start = rospy.Duration(t)
goal.trajectory.points.append(point)
goal.trajectory.header.stamp = rospy.Time.now()
self.arm_client.send_goal(goal)
rospy.sleep(.1)
rospy.loginfo("command sent to client")
status = 0
if blocking: #XXX why isn't this perfect?
end_time = rospy.Time.now() + rospy.Duration(timeout+ .1)
while (
(not rospy.is_shutdown()) and\
(rospy.Time.now() < end_time) and\
(status < gs.SUCCEEDED) and\
(type(self.arm_client.action_client.last_status_msg) != type(None))):
status = self.arm_client.action_client.last_status_msg.status_list[-1].status #XXX get to 80
rospy.Rate(10).sleep()
if status >gs.SUCCEEDED:
rospy.loginfo("goal status achieved. exiting")
else:
rospy.loginfo("ending due to timeout")
result = self.arm_client.get_result()
return result
def get_joint_names(self, char):
return [char+'_shoulder_pan_joint',
char+'_shoulder_lift_joint',
char+'_upper_arm_roll_joint',
char+'_elbow_flex_joint',
char+'_forearm_roll_joint',
char+'_wrist_flex_joint',
char+'_wrist_roll_joint' ]
class TriggeredImageTopic():
"""
This nodes provides allows to transform a trigger topic to an image topic using GrabImageAction.
Whenever a signal is published on ~trigger a sensor_msg.msg.Image is published on the output topic.
The interface is restricted to a rosparam for exposure with a low gain
"""
def __init__(self):
self.camera_name = rospy.get_param('~camera_name', '')
self.output_topic_name = rospy.get_param('~triggered_image_topic',
'triggered_images')
if not self.camera_name:
rospy.logwarn(
"No camera name given! Assuming 'pylon_camera_node' as"
" camera name")
self.camera_name = '/pylon_camera_node'
else:
rospy.loginfo('Camera name is: ' + self.camera_name)
self._grab_imgs_rect_ac = SimpleActionClient(
'{}/grab_images_rect'.format(self.camera_name), GrabImagesAction)
if self._grab_imgs_rect_ac.wait_for_server(rospy.Duration(10.0)):
rospy.loginfo('Found action server at '
'{}/grab_images_raw'.format(self.camera_name))
else:
rospy.logerr('Could not connect to action server at '
'{}/grab_images_raw'.format(self.camera_name))
self.pub = rospy.Publisher(self.output_topic_name,
Image,
queue_size=10)
self.subscriber = rospy.Subscriber("~trigger", Empty, self.trigger_cb)
def trigger_cb(self, msg):
grab_imgs_goal = GrabImagesGoal()
grab_imgs_goal.exposure_given = True
grab_imgs_goal.exposure_times = [
rospy.get_param('~exposure_time', 20000.0)
]
grab_imgs_goal.gain_given = True
grab_imgs_goal.gain_values = [0.2]
grab_imgs_goal.gain_auto = False
self._grab_imgs_rect_ac.send_goal(grab_imgs_goal)
self._grab_imgs_rect_ac.wait_for_result(rospy.Duration.from_sec(10.0))
grab_imgs_result = self._grab_imgs_rect_ac.get_result()
rospy.loginfo("Got image")
if len(grab_imgs_result.images) > 0:
rospy.loginfo("publish image")
self.pub.publish(grab_imgs_result.images[0])
def spin(self):
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
class AsyncSimpleActionClient:
def __init__(self, ns, action_spec):
self._client = SimpleActionClient(ns, action_spec)
self._client.wait_for_server()
async def send_goal(self, goal):
self._client.send_goal(goal)
loop = asyncio.get_event_loop()
await loop.run_in_executor(None, self._client.wait_for_result)
return self._client.get_state(), self._client.get_result()
class fibonacci_client:
def __init__(self, name):
self.action_client = SimpleActionClient('fibonacci_server',
FibonacciAction)
self.action_client.wait_for_server()
goal = FibonacciGoal(order=20)
self.action_client.send_goal(goal)
self.action_client.wait_for_result()
result = self.action_client.get_result()
rospy.loginfo('[Fibonacci Client] Result: {}'.format(', '.join(
[str(n) for n in result.sequence])))
def tiago_play_motion(motion, wait_duration=60.0):
rospy.loginfo('Doing PlayMotion: ' + motion)
pm = SimpleActionClient('/play_motion', PlayMotionAction)
pm.wait_for_server()
pmg = PlayMotionGoal()
pmg.motion_name = motion
pm.send_goal(pmg)
if pm.wait_for_result(rospy.Duration.from_sec(wait_duration)):
pm_result = pm.get_result()
rospy.loginfo(pm_result)
return pm_result
else:
rospy.logwarn("play motion timed out")
return False
class GUI(object):
""" Communication with the operator."""
def __init__(self, verbose=False):
self.verbose = verbose
self.gui_result = None
self.client = Client(topics.gui_validation, ValidateVictimGUIAction)
def cancel_all_goals(self):
log.debug('Waiting for the GUI action server...')
self.client.wait_for_server()
log.info('Canceling all goals on GUI.')
self.client.cancel_all_goals()
sleep(3)
def send_request(self, target):
""" Sends a validation request to the robot operator.
:param :target A target to be validated.
"""
goal = ValidateVictimGUIGoal()
goal.victimId = target.id
goal.victimFoundx = target.victimPose.pose.position.x
goal.victimFoundy = target.victimPose.pose.position.y
goal.probability = target.probability
goal.sensorIDsFound = target.sensors
log.debug('Waiting for the GUI action server.')
self.client.wait_for_server()
log.info('Sending validation request.')
if self.verbose:
log.debug(target)
self.client.send_goal(goal)
log.info('Waiting for response.')
self.client.wait_for_result()
status = self.client.get_state()
verbose_status = ACTION_STATES[status]
if status == GoalStatus.SUCCEEDED:
log.info('Validation request succeded!')
self.gui_result = self.client.get_result()
return True
else:
log.error('Validation request failed with %s.', verbose_status)
return False
def result(self):
return self.gui_result
def main():
[result, goal, collision_objects,
robot_state] = pickle.load(open(sys.argv[1], 'r'))
wi = WorldInterface()
#set the planning scene up correctly
wi.reset(repopulate=False)
for co in collision_objects:
#for running easy version
if ('table' in co.id or co.id == 'plate'):
wi.add_object(co)
psi = get_planning_scene_interface()
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
average_time = 0
goal.planning_time = 30
goal.tries = 10000
goal.debug_level = 0
goal.do_pause = False
goal.goal_bias = 0.2
#i think that this does not work
#goal.robot_state = robot_state
goal.execute = False
rospy.loginfo('First place goal is\n' +
str(goal.place_goal.place_locations[0]))
rospy.loginfo('File: ' + sys.argv[1] + ', restart after ' +
str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error ' +
str(result.error_code) + '. We had ' + str(i) +
' successful trials with an average time of ' +
str(average_time / float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL ' + str(i) + ': ' + str(result.planning_time))
rospy.loginfo(
str(NTRIALS) + ' averaged ' + str(average_time / float(NTRIALS)) +
' seconds to solve')
def main():
rospy.init_node("send_reference")
ref_args = sys.argv[1:]
if not ref_args:
rospy.logwarn("Usage: send_reference JOINT_NAME_1 POSITION_1 [ JOINT_NAME_2 POSITION 2 ... ]")
return 1
ac_joint_traj = SimpleActionClient("/amigo/body/joint_trajectory_action", FollowJointTrajectoryAction)
while not ac_joint_traj.wait_for_server(timeout=rospy.Duration(1.0)) and not rospy.is_shutdown():
rospy.logwarn("Waiting for server...")
if rospy.is_shutdown():
rospy.logwarn("No reference sent")
return 1
rospy.loginfo("Connected to server")
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Even items in ref_args are joint_names
joint_names = ref_args[0::2]
# Odd items in ref_args are set points
ref_positions = [float(i) for i in ref_args[1::2]]
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
points = [JointTrajectoryPoint(positions=ref_positions, time_from_start=rospy.Duration(0))]
joint_trajectory = JointTrajectory(joint_names=joint_names, points=points)
goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory, goal_time_tolerance=rospy.Duration(1000))
ac_joint_traj.send_goal(goal)
ac_joint_traj.wait_for_result(rospy.Duration(1000))
res = ac_joint_traj.get_result()
if res.error_string:
rospy.logerr(res.error_string)
def robot_move(self, map_node):
client = SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = map_node['frame_id']
goal.target_pose.header.stamp = Time.now()
goal.target_pose.pose.position.x = map_node['position']['x']
goal.target_pose.pose.position.y = map_node['position']['y']
goal.target_pose.pose.position.z = map_node['position']['z']
goal.target_pose.pose.orientation.x = map_node['orientation']['x']
goal.target_pose.pose.orientation.y = map_node['orientation']['y']
goal.target_pose.pose.orientation.z = map_node['orientation']['z']
goal.target_pose.pose.orientation.w = map_node['orientation']['w']
client.send_goal(goal)
client.wait_for_result()
return client.get_result()
class MoveTray(smach.State):
def __init__(self,
direction,
timeout=120.0,
action_server='/mdr_actions/move_tray_server'):
smach.State.__init__(self,
outcomes=['up', 'down', 'failed'],
input_keys=['command'])
self.timeout = timeout
self.move_tray_client = SimpleActionClient(action_server,
MoveTrayAction)
self.move_tray_client.wait_for_server()
def execute(self, userdata):
direction = None
command = userdata.command
if 'up' in command:
direction = 'up'
elif 'down' in command:
direction = 'down'
else:
rospy.logerr('Received unknown move tray direction')
return 'failed'
goal = MoveTrayGoal()
goal.direction = direction
rospy.loginfo('Calling move_tray action with direction %s' % direction)
self.move_tray_client.send_goal(goal)
duration = rospy.Duration.from_sec(self.timeout)
self.move_tray_client.wait_for_result(duration)
rospy.loginfo('move_tray call completed')
res = self.move_tray_client.get_result()
if res and res.success:
return direction
else:
return 'failed'
class ObjectSwat:
def __init__(self, file):
self.fileOut = file
rospy.init_node("object_swatter", anonymous=True)
self.all_objects = []
self.all_object_names = []
self.current_obj_idx = -1
self.swat_time = rospy.Time.now()
self.model_data = None
self.blockToSwat = ""
self.min_front_dist = 10.0
self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist)
self.allPoints = []
self.inProgress = False
# rospy.Subscriber('overhead_objects', PoseArray, self.update_object_positions)
rospy.Subscriber("gazebo/model_states", ModelStates, self.read_model)
# self.sh_pan_speed_srv = rospy.ServiceProxy('shoulder_pan_controller/set_speed', SetSpeed)
self.base_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
rospy.Subscriber("tilt_laser/scan", LaserScan, self.filter_scan)
# rospy.wait_for_service('shoulder_pan_controller/set_speed')
self.base_client.wait_for_server()
self.arm_client.wait_for_server()
self.gripper_client.wait_for_server()
rospy.loginfo("Connected to all action servers")
def setBlockToSwat(self, name):
self.blockToSwat = name
if name != "":
self.allPoints = []
self.inProgress = False
def read_model(self, data):
self.model_data = data
self.all_objects = []
self.all_object_names = []
sequence = self.model_data.name
for i, n in zip(range(len(sequence)), sequence):
if re.search("_swat_", n):
self.all_objects.append(self.model_data.pose[i])
self.all_object_names.append(n)
if not self.inProgress:
return
if re.search(n, self.blockToSwat):
t = rospy.Time.now()
if (
len(self.allPoints) == 0
or t.secs > (self.allPoints[-1])["time1"]
or t.nsecs > (self.allPoints[-1])["time2"] + 0.05
):
xx = self.model_data.pose[i].position.x
yy = self.model_data.pose[i].position.y
self.allPoints.append({"time1": t.secs, "time2": t.nsecs, "x": xx, "y": yy})
if self.fileOut != None:
self.fileOut.write(str(t.secs) + "." + str(t.nsecs) + "," + str(xx) + "," + str(yy) + "\n")
# self.all_objects = msg.poses
def getPoints(self):
return self.allPoints
def tuck_arm_for_navigation(self):
goal = SmartArmGripperGoal()
goal.target_joints = [0.15, -0.15]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
result = self.gripper_client.get_result()
if result.success:
print "Gripper in position"
else:
print "Gripper positioning failed"
goal = SmartArmGoal()
goal.target_joints = [0.0, 1.77, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
print "Arm tucked"
return True
else:
print "Failed to tuck arm"
return False
def go_to_object(self, idx):
self.current_obj_idx = idx
goal = ErraticBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "/map"
goal.target_pose.pose = self.all_objects[idx]
goal.vicinity_range = 0.3
self.base_client.send_goal(goal)
self.base_client.wait_for_result()
if self.base_client.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo("Robot is in front of the object")
return True
elif self.base_client.get_state() == GoalStatus.PREEMPTED:
rospy.loginfo("Action pre-empted")
return False
else:
rospy.loginfo("Action failed")
return False
def position_self(self):
max_speed = 0.1
cmd_vel = Twist()
rospy.loginfo("LIDAR reports object is %f m away...", self.min_front_dist)
# rospy.loginfo('HERE!!!!!!!!!!!!')
# move in closer
if self.min_front_dist > 0.24 and self.min_front_dist <= 0.50:
range = abs(self.min_front_dist - 0.24)
rospy.loginfo("Object is too far away, closing in %f m", range)
cmd_vel.linear.x = max_speed
cmd_vel.angular.z = 0.0
r = rospy.Rate(15)
et = rospy.Time.now() + rospy.Duration.from_sec(range / max_speed)
while rospy.Time.now() < et:
self.cmd_vel_pub.publish(cmd_vel)
r.sleep()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = 0.0
self.cmd_vel_pub.publish(cmd_vel)
# back away
elif self.min_front_dist < 0.22: # was 0.22
range = abs(self.min_front_dist - 0.22)
rospy.loginfo("Object is too close, backing away %f m", range)
cmd_vel.linear.x = -max_speed
cmd_vel.angular.z = 0.0
r = rospy.Rate(15)
et = rospy.Time.now() + rospy.Duration.from_sec(range / max_speed)
while rospy.Time.now() < et:
self.cmd_vel_pub.publish(cmd_vel)
r.sleep()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = 0.0
self.cmd_vel_pub.publish(cmd_vel)
# quit, navigation failed?
elif self.min_front_dist > 0.50:
rospy.logwarn("Object is way out there, navigation should take care of traversing great distances")
else:
rospy.loginfo("Object is within reach, no need to move")
def filter_scan(self, scan):
center_scan_index = (len(scan.ranges) + 1) / 2
min_idx = center_scan_index - 50
max_idx = center_scan_index + 50
for dist in scan.ranges[min_idx:max_idx]:
if dist < self.min_front_dist and dist > 0.05:
self.min_front_dist = dist
def swat_object(self):
goal = SmartArmGripperGoal()
goal.target_joints = [0.15, -0.15]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
result = self.gripper_client.get_result()
if result.success:
print "Gripper in position"
else:
print "Gripper positioning failed"
goal = SmartArmGoal()
goal.target_joints = [0.0, 1.77, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
print "Arm UP"
else:
print "Failed to raise arm"
goal.target_joints = [0.93, -0.63, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
print "Arm ready to swat"
else:
print "Failed to position the arm for swatting"
t = rospy.Time.now()
if self.fileOut != None:
self.fileOut.write("Swatting action at " + str(t.secs) + "." + str(t.nsecs) + "\n")
# self.sh_pan_speed_srv(3.0);
self.inProgress = True
goal.target_joints = [-0.93, -0.63, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
print "Arm just swatted"
else:
print "Arm failed to swat"
class InterfaceWrapper(object):
def __init__(self, sim=False, move=True):
# rospy.init_node('tests')
# rospy.set_param(DummyInterfaceNodeName + '/initial_beliefstate', 'package://refills_perception_interface/owl/muh.owl')
# rospy.set_param(DummyInterfaceNodeName + '/initial_beliefstate',
# 'package://refills_rooming_in/data/augsburg_rooming_in_map_2018-11-08_10-17-17.owl')
# rospy.set_param(DummyInterfaceNodeName + '/initial_beliefstate',
# 'package://refills_rooming_in/data/rooming_in_map_2018-11-29_16-59-38.owl')
# rospy.set_param(DummyInterfaceNodeName + '/path_to_json',
# 'package://refills_rooming_in/data/augsburg_rooming_in_map_2018-11-08_10-17-17.json')
self.query_shelf_systems_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_shelf_systems', QueryShelfSystems)
self.query_shelf_layers_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_shelf_layers', QueryShelfLayers)
self.query_facings_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_facings', QueryFacings)
self.finish_perception_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/finish_perception', FinishPerception)
self.query_reset_belief_state_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/reset_beliefstate', Trigger)
self.query_shelf_detection_path_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_detect_shelf_layers_path',
QueryDetectShelfLayersPath)
self.query_facing_detection_path_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_detect_facings_path',
QueryDetectFacingsPath)
self.query_product_counting_path_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_count_products_posture',
QueryCountProductsPosture)
self.detect_shelves_ac = SimpleActionClient(
DummyInterfaceNodeName + '/detect_shelf_layers',
DetectShelfLayersAction)
self.detect_facings_ac = SimpleActionClient(
DummyInterfaceNodeName + '/detect_facings', DetectFacingsAction)
self.count_products_ac = SimpleActionClient(
DummyInterfaceNodeName + '/count_products', CountProductsAction)
# self.simple_base_goal_pub = rospy.Publisher('move_base_simple/goal', PoseStamped)
self.simple_joint_goal = rospy.ServiceProxy(
'refills_bot/set_joint_states', SetJointState)
self.sleep = sim
self.sleep_amount = 0
self.move = move
self.giskard = MoveArm(avoid_self_collisinon=True)
# self.base = MoveBase()
rospy.sleep(.5)
def reset(self):
# self.query_shelf_systems()
# self.query_shelf_systems()
self.cancel_detect_shelf_layers()
self.cancel_detect_facings()
self.cancel_count_products()
# self.query_reset_belief_state()
# -------------------------------------------------------------other------------------------------------------------
def query_shelf_systems(self):
r = self.query_shelf_systems_srv.call(
QueryShelfSystemsRequest()) # type: QueryShelfSystemsResponse
assert len(r.ids) > 0
return r.ids
def finish_perception(self,
expected_error=FinishPerceptionResponse.SUCCESS):
r = self.finish_perception_srv.call(FinishPerceptionRequest())
assert r.error == expected_error
return r
def query_detect_shelf_layers_path(
self,
shelf_id,
expected_error=QueryDetectShelfLayersPathResponse.SUCCESS):
r = self.query_shelf_detection_path_srv.call(
QueryDetectShelfLayersPathRequest(id=shelf_id))
assert r.error == expected_error
# if expected_error == QueryDetectShelfLayersPathResponse.SUCCESS:
# assert len(r.path.postures) > 0
# number_of_joints = -1
# for posture in r.path.postures:
# assert len(posture.joints) > 0
# if number_of_joints == -1: # all postures should have the same number of joints
# number_of_joints = len(posture.joints)
# assert len(posture.joints) == number_of_joints
return r.path
def query_detect_facings_path(
self,
layer_id,
expected_error=QueryDetectFacingsPathResponse.SUCCESS):
r = self.query_facing_detection_path_srv.call(
QueryDetectFacingsPathRequest(id=layer_id))
assert r.error == expected_error
# if expected_error == QueryDetectFacingsPathResponse.SUCCESS:
# assert len(r.path.postures) > 0
# number_of_joints = -1
# for posture in r.path.postures:
# assert len(posture.joints) > 0
# if number_of_joints == -1: # all postures should have the same number of joints
# number_of_joints = len(posture.joints)
# assert len(posture.joints) == number_of_joints
return r.path
def query_count_products_posture(
self,
facing_id,
expected_error=QueryCountProductsPostureResponse.SUCCESS):
"""
:param facing_id:
:param expected_error:
:rtype: FullBodyPosture
"""
r = self.query_product_counting_path_srv.call(
QueryCountProductsPostureRequest(id=facing_id))
assert r.error == expected_error
# if expected_error == QueryCountProductsPostureResponse.SUCCESS:
# assert len(r.posture.joints) > 0
return r.posture
def query_reset_belief_state(self):
assert self.query_reset_belief_state_srv.call(TriggerRequest())
# -------------------------------------------------------layer------------------------------------------------------
def query_shelf_layers(self,
shelf_id,
expected_error=QueryShelfLayersResponse.SUCCESS):
"""
:param shelf_id: str
:rtype: QueryShelfLayersResponse
"""
r = self.query_shelf_layers_srv.call(
QueryShelfLayersRequest(id=shelf_id))
assert r.error == expected_error
return r.ids
def start_detect_shelf_layers(self, shelf_id):
goal = DetectShelfLayersGoal()
goal.id = shelf_id
self.detect_shelves_ac.send_goal(goal)
rospy.sleep(.5)
def get_detect_shelf_layers_result(
self,
expected_state=GoalStatus.SUCCEEDED,
expected_error=DetectShelfLayersResult.SUCCESS):
self.detect_shelves_ac.wait_for_result()
assert self.detect_shelves_ac.get_state() == expected_state
r = self.detect_shelves_ac.get_result()
assert r.error == expected_error
return r
def detect_shelf_layers(self, shelf_system_id, path):
self.start_detect_shelf_layers(shelf_system_id)
if self.sleep:
rospy.sleep(self.sleep_amount)
else:
if path is not None:
self.execute_full_body_path(path)
self.finish_perception()
r = self.get_detect_shelf_layers_result()
assert len(r.ids) > 0
return r.ids
def cancel_detect_shelf_layers(self):
self.detect_shelves_ac.cancel_all_goals()
# ------------------------------------------------------facing------------------------------------------------------
def start_detect_facings(self, layer_id):
goal = DetectFacingsGoal()
goal.id = layer_id
rospy.sleep(.5)
self.detect_facings_ac.send_goal(goal)
rospy.sleep(.5)
def get_detect_facings_result(self,
expected_state=GoalStatus.SUCCEEDED,
expected_error=DetectFacingsResult.SUCCESS):
self.detect_facings_ac.wait_for_result()
assert self.detect_facings_ac.get_state() == expected_state
r = self.detect_facings_ac.get_result()
assert r.error == expected_error
return r
def query_facings(self,
layer_id,
expected_error=QueryFacingsResponse.SUCCESS):
r = self.query_facings_srv.call(QueryFacingsRequest(id=layer_id))
assert r.error == expected_error
return r.ids
def detect_facings(self, layer_id, path):
self.start_detect_facings(layer_id)
if self.sleep:
rospy.sleep(self.sleep_amount)
else:
self.execute_full_body_path(path)
self.finish_perception()
r = self.get_detect_facings_result()
assert len(r.ids) > 0
return r.ids
def cancel_detect_facings(self):
self.detect_facings_ac.cancel_all_goals()
# ------------------------------------------------------counting----------------------------------------------------
def start_count_products(self, facing_id):
goal = CountProductsGoal()
goal.id = facing_id
self.count_products_ac.send_goal(goal)
rospy.sleep(.5)
def get_count_products_result(self,
expected_state=GoalStatus.SUCCEEDED,
expected_error=CountProductsResult.SUCCESS):
self.count_products_ac.wait_for_result()
assert self.count_products_ac.get_state() == expected_state
r = self.count_products_ac.get_result()
assert r.error == expected_error
return r
def count_products(self, facing_id):
if self.move:
rospy.sleep(.5)
self.start_count_products(facing_id)
# self.finish_perception()
r = self.get_count_products_result()
return r.count
def cancel_count_products(self):
self.count_products_ac.cancel_all_goals()
def to_real_joint_name(self, joint_name):
d = {
'Torso_lin1': 'torso_lin_1',
'Torso_lin2': 'torso_lin_2',
'Torso_rot_1': 'torso_rot_1'
}
return d[joint_name]
def execute_full_body_path(self, fbp, sleep=0):
"""
:type fbp: FullBodyPath
"""
if self.move:
for posture in fbp.postures: # type: FullBodyPosture
self.execute_full_body_posture(posture)
rospy.sleep(sleep)
def move_camera_footprint(self, goal_pose):
if self.move:
self.giskard.move_other_frame(goal_pose)
def move_base(self, goal_pose):
if self.move:
self.giskard.move_absolute(goal_pose)
def execute_full_body_posture(self, posture):
"""
:type fbp: FullBodyPath
"""
if self.move:
if posture.type == posture.NO_TYPE:
assert False, '{} has no type'.format(posture)
if posture.type == posture.BASE:
self.move_base(posture.base_pos)
if posture.type == posture.CAMERA or posture.type == posture.BOTH:
self.giskard.set_and_send_cartesian_goal(posture.camera_pos)
if posture.type == posture.JOINT:
self.giskard.set_and_send_joint_goal(posture.goal_joint_state)
if posture.type == posture.CAM_FOOTPRINT or posture.type == posture.BOTH:
self.move_camera_footprint(posture.base_pos)
class PickAruco(object):
def __init__(self):
rospy.loginfo("Initalizing...")
self.bridge = CvBridge()
self.tfBuffer = tf2_ros.Buffer()
self.tf_l = tf2_ros.TransformListener(self.tfBuffer)
rospy.loginfo("Waiting for /pickup_pose AS...")
self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction)
time.sleep(1.0)
if not self.pick_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /pickup_pose AS")
exit()
rospy.loginfo("Waiting for /place_pose AS...")
self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction)
self.place_as.wait_for_server()
rospy.loginfo("Setting publishers to torso and head controller...")
self.torso_cmd = rospy.Publisher('/torso_controller/command',
JointTrajectory,
queue_size=1)
self.head_cmd = rospy.Publisher('/head_controller/command',
JointTrajectory,
queue_size=1)
self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose',
PoseStamped,
queue_size=1,
latch=True)
rospy.loginfo("Waiting for '/play_motion' AS...")
self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
if not self.play_m_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /play_motion AS")
exit()
rospy.loginfo("Connected!")
rospy.sleep(1.0)
rospy.loginfo("Done initializing PickAruco.")
def strip_leading_slash(self, s):
return s[1:] if s.startswith("/") else s
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 lift_torso(self):
rospy.loginfo("Moving torso up")
jt = JointTrajectory()
jt.joint_names = ['torso_lift_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.34]
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
self.torso_cmd.publish(jt)
def lower_head(self):
rospy.loginfo("Moving head down")
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.0, -0.75]
jtp.time_from_start = rospy.Duration(2.0)
jt.points.append(jtp)
self.head_cmd.publish(jt)
rospy.loginfo("Done.")
def prepare_robot(self):
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.lower_head()
rospy.loginfo("Robot prepared.")
class ShowTime:
def __init__(self, script_path):
rospy.init_node('show time')
rospy.on_shutdown(self.cleanup)
# Initialize a number of parameters and variables for nav locations
# Set the default TTS voice to use
self.voice = rospy.get_param("~voice", "voice_don_diphone")
self.robot = rospy.get_param("~robot", "robbie")
self.room_locations = (('start', (Pose(Point(4.245, -1.588, 0.0), Quaternion(0.0, 0.0, -0.411, 0.911)))),
('left', (Pose(Point(4.245, -1.588, 0.0), Quaternion(0.0, 0.0, -0.411, 0.911)))),
('right', (Pose(Point(4.245, -1.588, 0.0), Quaternion(0.0, 0.0, -0.411, 0.911)))),
('auto_dock', (Pose(Point(4.245, -1.588, 0.0), Quaternion(0.0, 0.0, -0.411, 0.911)))))
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
# Create the sound client object
self.soundhandle = SoundClient()
# Wait a moment to let the client connect to the
# sound_play server
rospy.sleep(2)
# Make sure any lingering sound_play processes are stopped.
self.soundhandle.stopAll()
# Set the wave file path if used
self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds")
#define afternoon and morning
self.noon = strftime("%p:", localtime())
if self.noon == "AM:":
self.noon1 = "Goood Morning "
else:
self.noon1 ="Good After Noon "
self.local = strftime("%H:%M:", localtime())
#local time
self.local = strftime("%H:%M:", localtime())
self.soundhandle.say(self.noon1 + self.robot +" is on line" + " the time is " + self.local, self.voice)
rospy.sleep(2)
self.move_to("start")
#self.soundhandle.say("" + self.local, self.voice)
self.soundhandle.say("Welcome to SHOW time. This is where I get to demonstrate my capabilities" + self.local, self.voice)
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = self.room_locations[location]
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
self.client.wait_for_result()
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
def cleanup(self):
self.soundhandle.stopAll()
rospy.loginfo("Shutting down show time node...")
class Gripper():
""" Wrapper for all the action client stuff """
def __init__(self):
""" Instantiate action clients and wait for communication with servers """
self.client_gripper = SimpleActionClient("r_gripper_sensor_controller/gripper_action", Pr2GripperCommandAction)
self.client_contact = SimpleActionClient("r_gripper_sensor_controller/find_contact", PR2GripperFindContactAction)
self.client_force = SimpleActionClient("r_gripper_sensor_controller/force_servo", PR2GripperForceServoAction)
self.client_gripper.wait_for_server(rospy.Duration(10.0))
self.client_contact.wait_for_server(rospy.Duration(10.0))
self.client_force.wait_for_server(rospy.Duration(10.0))
rospy.loginfo('All servers ready!')
rospy.Subscriber('r_gripper_pressure_balance', String, self.store_balance)
def store_balance(self, balance):
""" Stores gripper balance. """
self.balance = balance.data
def open(self):
""" Open the gripper """
goal_open = Pr2GripperCommandGoal()
goal_open.command.position = 0.09
goal_open.command.max_effort = -1.0
rospy.loginfo("Sending goal: OPEN")
self.client_gripper.send_goal_and_wait(goal_open)
if (self.client_gripper.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. The gripper opened!')
else:
rospy.loginfo('FAILURE. The gripper failed to open.')
def open_manual(self, position):
""" Open the gripper """
goal_open = Pr2GripperCommandGoal()
goal_open.command.position = position
goal_open.command.max_effort = -1.0
rospy.loginfo("Sending goal: OPEN")
self.client_gripper.send_goal_and_wait(goal_open)
if (self.client_gripper.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. The gripper opened!')
else:
rospy.loginfo('FAILURE. The gripper failed to open.')
def close(self):
""" Close the gripper """
goal_close = Pr2GripperCommandGoal()
goal_close.command.position = 0.00
goal_close.command.max_effort = 20.0
rospy.loginfo("Sending goal: CLOSE")
self.client_gripper.send_goal_and_wait(goal_close)
if (self.client_gripper.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. The gripper closed!')
else:
rospy.loginfo('FAILURE. The gripper failed to close.')
def hold(self, holdForce):
""" Hold something with a specified grip force """
goal_squeeze = PR2GripperForceServoGoal()
goal_squeeze.command.fingertip_force = holdForce
rospy.loginfo("Sending goal: HOLD with {0}N of force.".format(holdForce))
self.client_force.send_goal_and_wait( goal_squeeze )
if (self.client_force.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. Stable force achieved!')
else:
rospy.loginfo('FAILURE. Stable force was NOT achieved.')
def findTwoContacts(self):
""" Find two contacts and go into force control mode """
goal_contact = PR2GripperFindContactGoal()
goal_contact.command.contact_conditions = goal_contact.command.BOTH # close until both fingers contact
goal_contact.command.zero_fingertip_sensors = True # zero fingertip sensors before moving
rospy.loginfo("Sending goal: FIND TWO CONTACTS")
self.client_contact.send_goal_and_wait(goal_contact)
if (self.client_contact.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. Contact found at left: {0} and right: {1}'.format(self.client_contact.get_result().data.left_fingertip_pad_contact,
self.client_contact.get_result().data.right_fingertip_pad_contact))
rospy.loginfo('Contact force for left: {0} and right: {1}'.format(self.client_contact.get_result().data.left_fingertip_pad_force,
self.client_contact.get_result().data.right_fingertip_pad_force))
else:
rospy.loginfo('FAILURE. No contact found or force not able to be maintained')
class ObjectSwat:
def __init__(self):
self.all_objects = []
self.current_obj_idx = -1
self.swat_time = rospy.Time.now()
self.min_dist = 10
self.front_dist = 10
self.avg_front_dist = 10
self.min_front_dist = 10
rospy.Subscriber('overhead_objects', PoseArray, self.update_object_positions)
rospy.Subscriber('tilt_laser/scan', LaserScan, self.filter_scan)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
self.sh_pan_speed_srv = rospy.ServiceProxy('shoulder_pan_controller/set_speed', SetSpeed)
self.classify_obj_srv = rospy.ServiceProxy('classify_object', ClassifyObject)
self.base_client = SimpleActionClient('erratic_base_action', ErraticBaseAction)
self.arm_client = SimpleActionClient('smart_arm_action', SmartArmAction)
self.gripper_client = SimpleActionClient('smart_arm_gripper_action', SmartArmGripperAction)
rospy.wait_for_service('shoulder_pan_controller/set_speed')
rospy.wait_for_service('classify_object')
self.base_client.wait_for_server()
self.arm_client.wait_for_server()
self.gripper_client.wait_for_server()
rospy.loginfo('Connected to all action servers')
def update_object_positions(self, msg):
self.all_objects = msg.poses
def filter_scan(self, scan):
center_scan_index = (len(scan.ranges) + 1) / 2
min_idx = center_scan_index - 50
max_idx = center_scan_index + 50
for dist in scan.ranges[min_idx:max_idx]:
if dist < self.min_front_dist and dist > 0.05:
self.min_front_dist = dist
def tuck_arm_for_navigation(self):
goal = SmartArmGripperGoal()
goal.target_joints = [0.15, -0.15]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
result = self.gripper_client.get_result()
if result.success:
rospy.loginfo('Gripper in position')
else:
rospy.loginfo('Gripper positioning failed')
goal = SmartArmGoal()
goal.target_joints = [0.0, 1.77, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
rospy.loginfo('Arm tucked')
return True
else:
rospy.loginfo('Failed to tuck arm')
return False
def go_to_object(self, idx):
self.current_obj_idx = idx
goal = ErraticBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = '/map'
goal.target_pose.pose = self.all_objects[idx]
goal.vicinity_range = 0.3
self.base_client.send_goal(goal)
self.base_client.wait_for_result()
if self.base_client.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Robot is in front of the object')
return True
elif self.base_client.get_state() == GoalStatus.PREEMPTED:
rospy.loginfo('Action pre-empted')
return False
else:
rospy.loginfo('Action failed')
return False
def position_self(self):
max_speed = 0.1
cmd_vel = Twist()
rospy.loginfo('LIDAR reports object is %f m away...', self.min_front_dist)
# move in closer
if self.min_front_dist > 0.24:
range = abs(self.min_front_dist - 0.24)
rospy.loginfo('Object is too far away, closing in %f m', range)
cmd_vel.linear.x = max_speed
cmd_vel.angular.z = 0.0
r = rospy.Rate(15)
et = rospy.Time.now() + rospy.Duration.from_sec(range / max_speed)
while rospy.Time.now() < et:
self.cmd_vel_pub.publish(cmd_vel)
r.sleep()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = 0.0
self.cmd_vel_pub.publish(cmd_vel)
# back away
elif self.min_front_dist < 0.22:
range = abs(self.min_front_dist - 0.22)
rospy.loginfo('Object is too close, backing away %f m', range)
cmd_vel.linear.x = -max_speed
cmd_vel.angular.z = 0.0
r = rospy.Rate(15)
et = rospy.Time.now() + rospy.Duration.from_sec(range / max_speed)
while rospy.Time.now() < et:
self.cmd_vel_pub.publish(cmd_vel)
r.sleep()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = 0.0
self.cmd_vel_pub.publish(cmd_vel)
# quit, navigation failed?
elif self.min_front_dist > 0.50:
rospy.logwarn('Object is way out there, navigation should take care of traversing great distances')
else:
rospy.loginfo('Object is within reach, no need to move')
def swat_object(self):
self.tuck_arm_for_navigation()
goal = SmartArmGoal()
goal.target_joints = [0.93, -0.63, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
rospy.loginfo('Arm ready to swat')
else:
rospy.loginfo('Failed to position the arm for swatting')
self.swat_time = rospy.Time.now()
rospy.loginfo('Swatting action at ' + str(self.swat_time.secs) + '.' + str(self.swat_time.nsecs))
self.sh_pan_speed_srv(3.0)
goal.target_joints = [-0.93, -0.63, 0.0, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
result = self.arm_client.get_result()
if result.success:
rospy.loginfo('Arm just swatted')
else:
rospy.loginfo('Arm failed to swat')
self.sh_pan_speed_srv(1.17)
self.tuck_arm_for_navigation()
rospy.sleep(5)
def classify_object(self):
resp = self.classify_obj_srv(self.current_obj_idx, self.swat_time)
rospy.loginfo('The object is %s', resp.category)
class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("moveit_demo")
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher("planning_scene", PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher("gripper_pose", PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# move_base action
self.fridge = Pose(Point(X_FRIDGE, Y_FRIDGE, 0.0), Quaternion(0.0, 0.0, 0, 1)) # location of the beer
self.person = Pose(Point(X_PERSON, Y_PERSON, 0.0), Quaternion(0.0, 0.0, 0, 1)) # person requesting the beer
self.station = Pose(Point(0.5, 0.0, 0.0), Quaternion(0.0, 0.0, 0, 1)) # person requesting the beer
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
left_arm = MoveGroupCommander("left_arm")
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 10 seconds per planning attempt
right_arm.set_planning_time(10)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 10
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = "table"
box1_id = "box1"
box2_id = "box2"
target_id = "target"
tool_id = "tool"
person1_id = "person1"
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
scene.remove_world_object(person1_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target("right_start")
right_arm.go()
left_arm.set_named_target("left_start")
left_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.65
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
person1_size = [0.3, 0.7, 0.01]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = X_FRIDGE + 0.55
table_pose.pose.position.y = Y_FRIDGE + 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = X_FRIDGE + 0.55
box1_pose.pose.position.y = Y_FRIDGE + -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = X_FRIDGE + 0.54
box2_pose.pose.position.y = Y_FRIDGE + 0.13
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# add the person to the scene
person1_pose = PoseStamped()
person1_pose.header.frame_id = REFERENCE_FRAME
person1_pose.pose.position.x = X_PERSON + 0.54
person1_pose.pose.position.y = Y_PERSON + 0.13
person1_pose.pose.position.z = table_ground + table_size[2] + person1_size[2] / 2.0
person1_pose.pose.orientation.w = 1.0
scene.add_box(person1_id, person1_pose, person1_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = X_FRIDGE + 0.50
target_pose.pose.position.y = Y_FRIDGE + 0.0
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
self.setColor(person1_id, 0.8, 0, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = X_PERSON + 0.50
place_pose.pose.position.y = Y_PERSON + -0.25
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# move to target
self.move_to(self.fridge)
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = right_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# _------------------------now we move to the other table__________-------------------------------------------
right_arm.set_named_target("r_travel")
right_arm.go()
self.move_to(self.person)
# _------------------------now we move to the other table__________-------------------------------------------
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target("right_start")
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# move to station
self.move_to(self.station)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
# move to location
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = location
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
# self.client.wait_for_result()
self.client.wait_for_result(rospy.Duration.from_sec(40.0))
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
# Get the gripper posture as a JointTrajectory
def make_gripper_posture(self, joint_positions):
# Initialize the joint trajectory for the gripper joints
t = JointTrajectory()
# Set the joint names to the gripper joint names
t.joint_names = GRIPPER_JOINT_NAMES
# Initialize a joint trajectory point to represent the goal
tp = JointTrajectoryPoint()
# Assign the trajectory joint positions to the input positions
tp.positions = joint_positions
# Set the gripper effort
tp.effort = GRIPPER_EFFORT
tp.time_from_start = rospy.Duration(1.0)
# Append the goal point to the trajectory points
t.points.append(tp)
# Return the joint trajectory
return t
# Generate a gripper translation in the direction given by vector
def make_gripper_translation(self, min_dist, desired, vector):
# Initialize the gripper translation object
g = GripperTranslation()
# Set the direction vector components to the input
g.direction.vector.x = vector[0]
g.direction.vector.y = vector[1]
g.direction.vector.z = vector[2]
# The vector is relative to the gripper frame
g.direction.header.frame_id = GRIPPER_FRAME
# Assign the min and desired distances from the input
g.min_distance = min_dist
g.desired_distance = desired
return g
# Generate a list of possible grasps
def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
# Initialize the grasp object
g = Grasp()
# Set the pre-grasp and grasp postures appropriately
g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
# Set the approach and retreat parameters as desired
g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [1.0, 0.0, 0.0])
g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, -1.0, 1.0])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
# Pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]
# Yaw angles to try
yaw_vals = [0]
# A list to hold the grasps
grasps = []
# Generate a grasp for each pitch and yaw angle
for y in yaw_vals:
for p in pitch_vals:
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, p, y)
# Set the grasp pose orientation accordingly
g.grasp_pose.pose.orientation.x = q[0]
g.grasp_pose.pose.orientation.y = q[1]
g.grasp_pose.pose.orientation.z = q[2]
g.grasp_pose.pose.orientation.w = q[3]
# Set and id for this grasp (simply needs to be unique)
g.id = str(len(grasps))
# Set the allowed touch objects to the input list
g.allowed_touch_objects = allowed_touch_objects
# Don't restrict contact force
g.max_contact_force = 0
# Degrade grasp quality for increasing pitch angles
g.grasp_quality = 1.0 - abs(p)
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
# Generate a list of possible place poses
def make_places(self, init_pose):
# Initialize the place location as a PoseStamped message
place = PoseStamped()
# Start with the input place pose
place = init_pose
# A list of x shifts (meters) to try
x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
pitch_vals = [0]
# A list of yaw angles to try
yaw_vals = [0]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for y in yaw_vals:
for p in pitch_vals:
for y in y_vals:
for x in x_vals:
place.pose.position.x = init_pose.pose.position.x + x
place.pose.position.y = init_pose.pose.position.y + y
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, p, y)
# Set the place pose orientation accordingly
place.pose.orientation.x = q[0]
place.pose.orientation.y = q[1]
place.pose.orientation.z = q[2]
place.pose.orientation.w = q[3]
# Append this place pose to the list
places.append(deepcopy(place))
# 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)
class ShowTime:
def __init__(self, script_path):
rospy.init_node('show_time')
rospy.on_shutdown(self.cleanup)
# Initialize a number of parameters and variables for nav locations
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Initialize the move group for the arms
right_arm = MoveGroupCommander('right_arm')
left_arm = MoveGroupCommander('left_arm')
right_gripper = MoveGroupCommander('right_gripper')
left_gripper = MoveGroupCommander('left_gripper')
# Set the default TTS voice to use
self.voice = rospy.get_param("~voice", "voice_don_diphone")
self.robot = rospy.get_param("~robot", "robbie")
self.start =(Pose(Point(1.7, 0, 0.0), Quaternion(0.0, 0.0, 1.000, 0.018)))
self.left =(Pose(Point(1.960, -1.854, 0.0), Quaternion(0.0, 0.0, 0.916, 0.401)))
self.right =(Pose(Point(2.123, 1.592, 0.0), Quaternion(0.0, 0.0, 0.877, -0.480)))
self.auto_dock =(Pose(Point(0.5, 0, 0.0), Quaternion(0.0, 0.0, 0.002, 1.000)))
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
# Create the sound client object
self.soundhandle = SoundClient()
# Wait a moment to let the client connect to the
# sound_play server
rospy.sleep(2)
# Make sure any lingering sound_play processes are stopped.
self.soundhandle.stopAll()
# Set the wave file path if used
self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds")
#define afternoon and morning
self.noon = strftime("%p:", localtime())
if self.noon == "AM:":
self.noon1 = "Goood Morning "
else:
self.noon1 ="Good After Noon "
self.local = strftime("%H:%M:", localtime())
#local time
self.local = strftime("%H:%M:", localtime())
self.soundhandle.say(self.noon1 + self.robot +" is on line" + " the time is " + self.local, self.voice)
right_arm.set_named_target('r_travel')
right_arm.go()
left_arm.set_named_target('l_travel')
left_arm.go()
rospy.sleep(5)
self.move_to(self.start)
self.soundhandle.say("Welcome to SHOW time. This is where I get to demonstrate my capabilities" , self.voice)
rospy.sleep(6)
self.soundhandle.say("I can move to my left", self.voice)
rospy.sleep(2)
self.move_to(self.left)
self.soundhandle.say("now back to the start position", self.voice)
rospy.sleep(2)
self.move_to(self.start)
self.soundhandle.say("I can move to my right", self.voice)
rospy.sleep(2)
self.move_to(self.right)
self.soundhandle.say("And again back to the start position", self.voice)
rospy.sleep(2)
self.move_to(self.start)
self.soundhandle.say("I once caught a fish this big", self.voice)
right_arm.set_named_target('r_fish')
right_arm.go()
left_arm.set_named_target('l_fish')
left_arm.go()
#rospy.sleep(2)
self.soundhandle.say("Thank you for your time ", self.voice)
right_arm.set_named_target('right_start')
right_arm.go()
left_arm.set_named_target('left_start')
left_arm.go()
#rospy.sleep(2)
#rospy.sleep(2)
self.move_to(self.auto_dock)
# add auto dock sequence here
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = location
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration.from_sec(40.0))
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
def cleanup(self):
self.soundhandle.stopAll()
rospy.loginfo("Shutting down show time node...")
class ActionTasks:
def __init__(self, script_path):
rospy.init_node('task_cordinator')
rospy.on_shutdown(self.cleanup)
# Initialize a number of parameters and variables for nav locations
setup_task_environment(self)
# Set the default TTS voice to use
self.voice = rospy.get_param("~voice", "voice_en1_mbrola")
self.robot = rospy.get_param("~robot", "robbie")
#self.NavPublisher = rospy.Publisher("goto", String)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
# Create the sound client object
self.soundhandle = SoundClient()
# Set the wave file path if used
self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds")
#define afternoon and morning
self.noon = strftime("%p:", localtime())
if self.noon == "AM:":
self.noon1 = "Goood Morning "
else:
self.noon1 ="Good After Noon "
self.local = strftime("%H:%M:", localtime())
#local time
self.local = strftime("%H:%M:", localtime())
# Wait a moment to let the client connect to the
# sound_play server
rospy.sleep(1)
# Make sure any lingering sound_play processes are stopped.
self.soundhandle.stopAll()
self.soundhandle.say(self.noon1 + self.robot +" is on line" + " the time is " + self.local, self.voice)
rospy.sleep(2)
# Subscribe to the recognizer output and set the callback function
rospy.Subscriber('/nltk_interpret', String, self.commands)
self.pub = rospy.Publisher('auto_dock', String, queue_size=1)
# here we do our main text prosessing
def commands(self,text):
task = text.data
ar = task.split(':')
rospy.logwarn(ar)
driver = Driver()
if ar[0] == "stop":
self.soundhandle.say("stopping", self.voice)
self.move_base.cancel_all_goals()
self.cmd_vel_pub.publish(Twist())
if ar[0] == "go":
loc = ar[6]+ar[9]
self.soundhandle.say(loc, self.voice)
self.move_to(loc)
if ar[0] == "pick":
self.soundhandle.say(str(PickUp(task)), self.voice)
if ar[0] == "run"and ar[5] == "demo":
self.soundhandle.say("ok running show time", self.voice)
ShowTime()
if ar[0] == "get" and ar[9] == "beer":
self.soundhandle.say("ok get a beer for you", self.voice)
if ar[0] == "turn" and ar[5] == "left":
self.soundhandle.say("ok turning left", self.voice)
driver.turn(angle = -0.5 * math.pi, angularSpeed = 0.5);#90 degrees left turn
if ar[0] == "turn" and ar[5] == "right":
driver.turn(angle = 0.5 * math.pi, angularSpeed = 0.5);#90 degrees right turn
self.soundhandle.say("ok turning right", self.voice)
if ar[0] == "move" and ar[5] == "forward":
driver.driveX(distance = 1.0, speed = 0.15)
self.soundhandle.say("ok, moving forward.", self.voice)
if ar[0] == "look":
self.soundhandle.say("ok, looking "+ str(ar[5]), self.voice)
Move_Head(task)
if ar[1] == "what"and ar[5] == "this":
self.soundhandle.say(" I have no idea. My vision system is not on line.", self.voice)
if ar[1] == "what"and ar[5] == "time":
local = strftime("%H:%M:", localtime())
self.soundhandle.say(" the time is " + local , self.voice)
if ar[1] == "what"and ar[5] == "weather":
self.soundhandle.say(str(Weather()) , self.voice)
if ar[5] == "charge":
self.soundhandle.say("Starting auto dock sequence", self.voice)
self.move_base.cancel_all_goals()
self.cmd_vel_pub.publish(Twist())
self.pub.publish("dock")
if ar[11] != "":
#self.soundhandle.say(str(Hello()), self.voice)
self.soundhandle.say("hello", self.voice)
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = self.room_locations[location]
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration.from_sec(40.0))
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
def cleanup(self):
self.soundhandle.stopAll()
self.cmd_vel_pub.publish(Twist())
rospy.loginfo("Shutting down talk node...")
class ManipulateAruco(object):
def __init__(self):
rospy.loginfo("Initalizing ManipulateAruco...")
self.aruco_pose_top = rospy.get_param(rospy.get_name() + '/marker_pose_topic')
self.pickup_pose_top = rospy.get_param(rospy.get_name() + '/pickup_marker_pose')
self.place_pose_top = rospy.get_param(rospy.get_name() + '/place_marker_pose')
self.bridge = CvBridge()
self.tfBuffer = tf2_ros.Buffer()
self.tf_l = tf2_ros.TransformListener(self.tfBuffer)
rospy.loginfo("Waiting for /pickup_pose AS...")
self.pick_as = SimpleActionClient(self.pickup_pose_top, PickUpPoseAction)
self.pick_as.wait_for_server()
rospy.loginfo("Waiting for /place_pose AS...")
self.place_as = SimpleActionClient(self.place_pose_top, PickUpPoseAction)
self.place_as.wait_for_server()
rospy.loginfo("Waiting for '/play_motion' AS...")
self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
if not self.play_m_as.wait_for_server(rospy.Duration(300)):
rospy.logerr("Could not connect to /play_motion AS")
exit()
rospy.loginfo("Connected!")
rospy.sleep(1.0)
rospy.loginfo("Setting publishers to torso and head controller...")
self.torso_cmd = rospy.Publisher(
'/torso_controller/command', JointTrajectory, queue_size=1)
self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose',
PoseStamped,
queue_size=1,
latch=True)
self.aruco_pose_rcv = False
self.aruco_pose_subs = rospy.Subscriber(self.aruco_pose_top, PoseStamped, self.aruco_pose_cb)
self.pick_g = PickUpPoseGoal()
rospy.loginfo("Done initializing ManipulateAruco.")
def strip_leading_slash(self, s):
return s[1:] if s.startswith("/") else s
def pick_and_place_aruco(self, string_operation):
success = False
if string_operation == "pick":
self.prepare_robot_pandp()
rospy.sleep(2.0)
while not rospy.is_shutdown() and self.aruco_pose_rcv == False:
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection...")
rospy.sleep(1.0)
aruco_pose = self.aruco_pose
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)
rospy.loginfo("Setting cube pose based on Aruco detection")
self.pick_g.object_pose.pose.position = 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")
success = False
else:
success = True
self.prepare_robot_nav()
return success
if string_operation == "place":
# Place the object on table in front
rospy.loginfo("Placing aruco marker")
self.place_as.send_goal_and_wait(self.pick_g)
rospy.loginfo("Done!")
result = self.place_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to place, not trying further")
success = False
else:
success = True
return success
def move_torso(self, string_operation):
jt = JointTrajectory()
jt.joint_names = ['torso_lift_joint']
jtp = JointTrajectoryPoint()
if string_operation == "lift":
jtp.positions = [0.34]
elif string_operation == "lower":
jtp.positions = [0.15]
else:
return
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
rospy.loginfo("Moving torso " + string_operation)
self.torso_cmd.publish(jt)
def prepare_robot_pandp(self):
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.")
rospy.loginfo("Robot prepared.")
def prepare_robot_nav(self):
# Move torso to its maximum height
self.move_torso("lift")
# 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.")
def aruco_pose_cb(self, aruco_pose_msg):
self.aruco_pose = aruco_pose_msg
self.aruco_pose_rcv = True
class ActionTasks:
def __init__(self, script_path):
rospy.init_node('task_cordinator')
rospy.on_shutdown(self.cleanup)
# Initialize a number of parameters and variables for nav locations
setup_task_environment(self)
# Set the default TTS voice to use
self.voice = rospy.get_param("~voice", "voice_don_diphone")
self.robot = rospy.get_param("~robot", "robbie")
self.NavPublisher = rospy.Publisher("goto", String)
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
# Create the sound client object
self.soundhandle = SoundClient()
# Wait a moment to let the client connect to the
# sound_play server
rospy.sleep(1)
# Make sure any lingering sound_play processes are stopped.
self.soundhandle.stopAll()
# Subscribe to the recognizer output and set the callback function
rospy.Subscriber('/nltk_interpret', String, self.commands)
# here we do our main text prosessing
def commands(self,text):
task = text.data
ar = task.split(':')
if ar[0] == "go":
self.move_to(ar[9].replace(" ", ""))
if ar[0] == "get":
pass
if ar[0] == "pick":
self.move_to(ar[9])
if ar[1] == "what":
pass
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = self.room_locations[location]
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
self.client.wait_for_result()
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
def cleanup(self):
self.soundhandle.stopAll()
rospy.loginfo("Shutting down talk node...")
goal = GenericExecuteGoal()
if len(sys.argv) > 1:
location = str(sys.argv[1]).upper()
else:
location = "PP01"
goal.parameters.append(KeyValue(key="location", value=location))
rospy.loginfo("Sending following goal to perceive cavity server")
rospy.loginfo(goal)
client.send_goal(goal)
timeout = 15.0
finished_within_time = client.wait_for_result(
rospy.Duration.from_sec(int(timeout)))
if not finished_within_time:
client.cancel_goal()
state = client.get_state()
result = client.get_result()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Action SUCCESS")
for kv in result.results:
rospy.loginfo(kv.value)
elif state == GoalStatus.ABORTED:
rospy.logerr("Action FAILED")
else:
rospy.logwarn("State: " + str(state))
rospy.loginfo(result)
class ShowTime:
def __init__(self, script_path):
rospy.init_node('show_time')
rospy.on_shutdown(self.cleanup)
# Initialize a number of parameters and variables for nav locations
# Set the default TTS voice to use
self.voice = rospy.get_param("~voice", "voice_en1_mbrola")
self.robot = rospy.get_param("~robot", "robbie")
self.start =(Pose(Point(1.7, 0, 0.0), Quaternion(0.0, 0.0, 1.000, 0.018)))
self.left =(Pose(Point(1.960, -1.854, 0.0), Quaternion(0.0, 0.0, 0.916, 0.401)))
self.right =(Pose(Point(2.123, 1.592, 0.0), Quaternion(0.0, 0.0, 0.877, -0.480)))
self.auto_dock =(Pose(Point(0.5, 0, 0.0), Quaternion(0.0, 0.0, 0.002, 1.000)))
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
# Create the sound client object
self.soundhandle = SoundClient()
# Wait a moment to let the client connect to the
# sound_play server
rospy.sleep(2)
# Make sure any lingering sound_play processes are stopped.
self.soundhandle.stopAll()
# Set the wave file path if used
self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds")
#define afternoon and morning
self.noon = strftime("%p:", localtime())
if self.noon == "AM:":
self.noon1 = "Goood Morning "
else:
self.noon1 ="Good After Noon "
self.local = strftime("%H:%M:", localtime())
#local time
self.local = strftime("%H:%M:", localtime())
self.soundhandle.say(self.noon1 + self.robot +" is on line" + " the time is " + self.local, self.voice)
rospy.sleep(6)
self.move_to(self.start)
self.soundhandle.say("Welcome to SHOW time. This is where I get to demonstrate my capabilities" , self.voice)
rospy.sleep(5)
self.soundhandle.say("I can move to my left", self.voice)
rospy.sleep(2)
self.move_to(self.left)
self.soundhandle.say("now back to the start position", self.voice)
rospy.sleep(2)
self.move_to(self.start)
self.soundhandle.say("I can move to my right", self.voice)
rospy.sleep(2)
self.move_to(self.right)
self.soundhandle.say("And again back to the start position", self.voice)
rospy.sleep(2)
self.move_to(self.start)
rospy.sleep(2)
self.soundhandle.say("Thank you for your time ", self.voice)
rospy.sleep(2)
#rospy.sleep(2)
self.move_to(self.auto_dock)
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = location
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration.from_sec(50.0))
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
def cleanup(self):
self.soundhandle.stopAll()
rospy.loginfo("Shutting down show time node...")
class bin_bot_base():
def __init__(self, desired_z, KpLin, KdLin, KpRot, KdRot, tol_z, tol_ang, maxLinSpeed, maxRotSpeed, max_z, min_z):
#global
self.speed = Twist() # variable to store velocity commands
self.tracked_blob = Twist() # variable to store pose of tracked blob
self.state = FSM_STATES.FSM_LOCALIZE # set initial state
self.obj_pose = MoveBaseGoal() # variable
self.goal = MoveBaseGoal()
self.blobs_3d = Blobs3d()
self.blobs_detected = False
self.miss_counter = 0
self.zStable = False # flag to signify desired z satisfied
self.angStable = False # flag to signify desired angle satisfied
self.arm_state = String
#params
self.desired_z = desired_z #0.001 #0.8
self.KpLin = KpLin #1000.0
self.KdLin = KdLin
self.KpRot = KpRot #0.1
self.KdRot = KdRot
self.tol_z = tol_z #0.00005 #0.05
self.tol_ang= tol_ang #0.5
self.maxLinSpeed = maxLinSpeed #0.2
self.maxRotSpeed = maxRotSpeed
self.max_z = max_z #3.0
self.min_z= min_z #0.0005 #0.2
# move_base Action Client
self._move_base = SimpleActionClient('move_base', MoveBaseAction)
self._move_base.wait_for_server()
#subscribtions
rospy.Subscriber('/blobs_3d', Blobs3d, self.blob_callback)
rospy.Subscriber('/bin_bot_base/goal', PoseStamped, self.goal_callback)
rospy.Subscriber('/arm_node/state', String, self.arm_state_callback)
# publications
self.speed_pub = rospy.Publisher('/navigation_velocity_smoother/raw_cmd_vel', Twist) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity
self.state_pub = rospy.Publisher('/bin_bot_base/state', String, queue_size=10) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity
#################
# goal_callback : Run everytime '\arm_bot_base\goal' topic receives message.
# # Send a goal to the Action Server upon callback
#################
def goal_callback(self, pose_stamped):
self.goal.target_pose = pose_stamped
if(self.goal):
self._move_base.send_goal(self.goal)
rospy.loginfo("Just passed a goal")
#################
# blob_callback : Run everytime \blobs_3d receives message.
#################
def blob_callback(self, blobs_3d):
self.blobs_3d = blobs_3d
self.blob_area = 0
if(len(self.blobs_3d.blobs)):
self.blobs_detected = True
self.miss_counter = 0
for blob in blobs_3d.blobs:
if (blob.area>self.blob_area):
self.blob_area = blob.area
self.tracked_blob.linear = self.getPose(blob)
else:
if(self.miss_counter>=5): # Allow for 5 callbacks before signaling non-detection
self.blobs_detected = False
self.miss_counter += 1
######################
# arm_state_callback : Run everytime the arm updates it's state.
######################
def arm_state_callback(self, arm_state):
self.arm_state = arm_state
#################
# speed_callback : Pubishes velocity and handles state transitions
#################
def speed_callback(self):
self.moveDuration = rospy.Duration(3) # duration of forward movement
# Print state
print ("\n")
rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state))
# FSM_LOCALIZE STATE
# =============>
if self.state == FSM_STATES.FSM_LOCALIZE:
# Spin
self.speed.linear.x = 0.0
self.speed.angular.z = 0.5
# Next state transition
# if-else statement to ensure error-less view of goal status
if(self.blobs_detected): # if a blob is seen
rospy.loginfo("Arm Bot detected! Approaching robot...")
self.state = FSM_STATES.FSM_OPTIMIZE
elif (self._move_base.simple_state == 2):
self.goal_state = 'No goals received'
rospy.loginfo("Goal Status: %s" %self.goal_state)
self.state = FSM_STATES.FSM_LOCALIZE
else: # if a goal is detected
self.goal_state = self._move_base.get_state()
rospy.loginfo("Goal Status: %f" %self.goal_state)
self.state = FSM_STATES.FSM_NAVIGATE
# FSM_NAVIGATE STATE
# =============>
elif self.state == FSM_STATES.FSM_NAVIGATE:
# No action required, simply allow path planner to navigate
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
# Next state transitions
if(self.blobs_detected): # and (self.tracked_blob.linear.z < 3.0): # if a blob is seen (and closer than 3 meters)
self._move_base.cancel_all_goals() # cancell all nav goals
rospy.loginfo("Goal Status: %f" %self._move_base.get_state())
rospy.loginfo("Arm Bot detected! Aborting goal and approaching robot...")
self.state = FSM_STATES.FSM_OPTIMIZE
else:
if(self._move_base.get_state() != 3.0):
rospy.loginfo("Navigating to Arm Bot at pose (%f, %f, %f)..." %(self.goal.target_pose.pose.position.x, self.goal.target_pose.pose.position.y, self.goal.target_pose.pose.position.z))
else:
rospy.loginfo("Waiting for goal...")
rospy.loginfo("Goal Status: %f" %self._move_base.get_state())
self.state = FSM_STATES.FSM_NAVIGATE
# FSM_OPTIMIZE STATE (PID/PD controller)
# =================>
elif self.state == FSM_STATES.FSM_OPTIMIZE :
rospy.loginfo("Blob detected at x:%f, y:%f, z:%f "%(self.tracked_blob.linear.x, self.tracked_blob.linear.y, self.tracked_blob.linear.z))
#self.LinDerivator = 0.0
self.RotDerivator = 0.0
# if statement to check Arm Bot is with range (TO BE DELETED/AMENDED!)
if (self.tracked_blob.linear.z > self.max_z or self.tracked_blob.linear.z < self.min_z):
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
rospy.loginfo("Detected object is either too close or too far away.")
else:
# Set linear speed to 0
self.speed.linear.x = 0.0
# Compute PD angular error
self.angError = -math.atan2(self.tracked_blob.linear.x, self.tracked_blob.linear.y);
self.pRotError = self.KpRot*self.angError
self.dRotError = self.KdRot*(self.angError-self.RotDerivator)
self.RotDerivator = self.angError
self.anglePID = self.pRotError + self.dRotError;
# Cap maximum rotation speed
if(math.fabs(self.anglePID)>self.maxRotSpeed):
self.anglePID=self.maxRotSpeed*math.fabs(self.anglePID)/self.anglePID;
# Set tolerance in angle to signify stable orientation
if(math.fabs(self.angError) < self.tol_ang):
self.speed.angular.z = 0.0;
RotDerivator = 0.0
self.angStable = True
else:
self.speed.angular.z = self.anglePID;
self.angStable = False
# Debug messages
rospy.loginfo("distance: %f" %(self.tracked_blob.linear.z))
rospy.loginfo("angError: %f, pRotError: %f, dRotError: %f" %(self.angError, self.pRotError, self.dRotError))
# Next state transitions
if (self.angStable): # if stable position and orientation have been achieved
self.state = FSM_STATES.FSM_APPROACH_OBJ # move to stable state
self.moveStartTime = rospy.Time.now()
elif(self.blobs_detected): # else if (while) blobs are being detected
self.state = FSM_STATES.FSM_OPTIMIZE # loop in same state
else:
self.state = FSM_STATES.FSM_LOCALIZE # else move to nav state
# FSM_APPROACH_OBJ STATE
# =============>
elif self.state == FSM_STATES.FSM_APPROACH_OBJ:
self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame'
self.obj_pose.target_pose.header.stamp = rospy.Time.now()
self.obj_pose.target_pose.pose.orientation.w = 1.0
# if-else statements to perform corrections in target pose
# if distance is higher than 2.20m
if ( self.tracked_blob.linear.z > 0.0022):
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.002) # set goal to (distance - 2m)
self._move_base.send_goal(self.obj_pose) # send goal
rospy.loginfo('Performing approach to 2m. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
rospy.loginfo('Waiting for result...')
self._move_base.wait_for_result()
if self._move_base.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal Reached!')
else:
rospy.loginfo('Goal aborted!')
self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle
# else if distance is less than 2m and more than 1.20m
elif (self.tracked_blob.linear.z > 0.0012):
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.001) # set goal to (distance - 1m)
self._move_base.send_goal(self.obj_pose) # send goal
rospy.loginfo('Performing approach to 1m. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
rospy.loginfo('Waiting for result...')
self._move_base.wait_for_result()
if self._move_base.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal Reached!')
else:
rospy.loginfo('Goal aborted!')
rospy.loginfo(self._move_base.get_result())
self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle
# else if distance is less than 1m
else:
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.4 # set goal to final target
self._move_base.send_goal(self.obj_pose) # send goal
rospy.loginfo('Performing final approach. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
rospy.loginfo('Waiting for result...')
self._move_base.wait_for_result()
if self._move_base.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal Reached!')
else:
rospy.loginfo('Goal aborted!')
self.moveStartTime = rospy.Time.now()
self.state = FSM_STATES.FSM_SPIN # move to FSM_SPIN
# FSM_SPIN STATE
# =============>
elif (self.state == FSM_STATES.FSM_SPIN):
# Spin
if (rospy.Time.now() < self.moveStartTime + self.moveDuration):
self.speed.linear.x = 0.0
self.speed.angular.z = 0.5
self.state = FSM_STATES.FSM_SPIN
else:
self.state = FSM_STATES.FSM_WAIT_FOR_DROP
# FSM_WAIT_FOR_DROP STATE
# =============>
elif (self.state == FSM_STATES.FSM_WAIT_FOR_DROP):
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
# Next state transitions
if (self.arm_state.data == 'OBJ_DROPPED'): # if arm signifies that object has been dropped
self.state = FSM_STATES.FSM_MOVE_TO_BASE # change to MOVE_TO_BASE state
else: # if object not dropped
self.state = FSM_STATES.FSM_WAIT_FOR_DROP # loop back
# FSM_MOVE_TO_BASE STATE
# =============>
elif (self.state == FSM_STATES.FSM_MOVE_TO_BASE):
# **TEST** wait for coordinates of base to be passed and then move to base
if (self._move_base.simple_state == 2):
rospy.loginfo("Waiting for base coordinates...")
self.state = FSM_STATES.FSM_MOVE_TO_BASE
else: # if a goal is detected
rospy.loginfo("Moving back to base")
rospy.loginfo("Goal Status: %f" %self._move_base.get_state())
self._move_base.wait_for_result()
if self._move_base.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal Reached!')
else:
rospy.loginfo('Goal aborted!')
self.state = FSM_STATES.FSM_NAVIGATE
# Print general info
self.printINFO()
# Publish /arm_bot_base/state
self.state_pub.publish(String(FSM_STATES.__geitem__(self.state)))
# Only publish velocity when no goal is being pursued or expected
if (self.state != FSM_STATES.FSM_NAVIGATE and self.state != FSM_STATES.FSM_MOVE_TO_BASE and self.state != FSM_STATES.FSM_APPROACH_OBJ):
self.speed_pub.publish(self.speed)
############
# getPose(): Returns pose of received blob relative to the camera.
############
def getPose(self, blob):
self.pose = Vector3()
self.pose.x = blob.center.x
self.pose.y = blob.center.y
self.pose.z = blob.center.z
return self.pose
def printINFO(self):
# Print debug info about linear and angular speeds
rospy.loginfo("speedPID: %f" %(self.speed.linear.x))
rospy.loginfo("anglePID: %f" %(self.speed.angular.z))
if(self.blobs_detected):
rospy.loginfo("Blob distance: %f" %(self.tracked_blob.linear.z))
rospy.loginfo("miss_counter = %f" %(self.miss_counter))
else:
rospy.loginfo("No blobs detected")
rospy.loginfo("miss_counter = %f" %(self.miss_counter))
def spin(self):
rate = rospy.Rate(RATE)
while not rospy.is_shutdown():
self.speed_callback()
rate.sleep()
class PointArmService():
def __init__(self):
rospy.init_node(NAME + 'server' , anonymous=True)
self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
# self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
# self.move_client.wait_for_server()
self.tf = tf.TransformListener()
self.arm_client.wait_for_server()
self.gripper_client.wait_for_server()
self.service = rospy.Service('point_arm_service', ArmPointService, self.handle_point)
rospy.loginfo("%s: Ready for pointing", NAME)
#handles receiving a PointStamped. Not handling preempts - does client do this? remember to check code
def handle_point(self, req):
success = False
#print info about request
rospy.loginfo("%s: recieved position %s %s %s in %s", \
NAME, req.position.point.x, req.position.point.y, req.position.point.z, req.position.header.frame_id);
if self.point_at(req.position.header.frame_id, req.position.point.x, req.position.point.y, req.position.point.z):
success = True
return success
#(almost) blatent copy / past from wubble_head_action.py. Err, it's going to the wrong position
def transform_target_point(self, point):
# Cannot use arm_shoulder_pan_link & arm_shoulder_tilt_link as target frame_id
# because the angles used by the low-level motors are absolute, and fixed to
# the arm_base_link coordinate (not relative to the coordinates of pan & tilt)
target_frame = 'arm_base_link'
#rospy.loginfo("%s: got %s %s %s %s", NAME, point.header.frame_id, point.point.x, point.point.y, point.point.z)
# Wait for tf info (time-out in 5 seconds)
self.tf.waitForTransform(target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(2.0))
# Transform target point & retrieve the pan angle
pan_target = self.tf.transformPoint(target_frame, point)
pan_angle = math.atan2(pan_target.point.y, pan_target.point.x)
#rospy.loginfo("%s: Pan transformed to <%s, %s, %s> => angle %s", \
# NAME, pan_target.point.x, pan_target.point.y, pan_target.point.z, pan_angle)
# Transform target point & retrieve the tilt angle
tilt_target = self.tf.transformPoint(target_frame, point)
tilt_angle = math.atan2(tilt_target.point.z,
math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2)))
#rospy.loginfo("%s: Tilt transformed to <%s, %s, %s> => angle %s", \
# NAME, tilt_target.point.x, tilt_target.point.y, tilt_target.point.z, tilt_angle)
return [pan_angle, tilt_angle]
#point! modified move_arm + reach_at from move_arm_demo
def point_at(self, frame_id, x, y, z):
goal = SmartArmGoal()
goal.target_point = PointStamped()
goal.target_point.header.frame_id = frame_id
goal.target_point.point.x = x
goal.target_point.point.y = y
goal.target_point.point.z = z
self.move_gripper (-2,2)
target_shoulder = list()
target_shoulder = self.transform_target_point(goal.target_point)
if (target_shoulder[0] < 1.2 and target_shoulder[0] > -1.2):
#bend arm a bit
goal.target_joints = [target_shoulder[0], target_shoulder[1] + .2, .3, .5]
# Sends the goal to the action server.
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result(rospy.Duration(2.0))
#straighten arm
goal.target_joints = [target_shoulder[0], target_shoulder[1], 0, 0]
# Sends the goal to the action server.
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
# Return result
#something broke here, returning True fix is temporary
return self.arm_client.get_result()
#motors are too slow in the demo to actually wait for arm to go to cobra position before moving
self.go_to_cobra()
rospy.sleep(2)
position = Point(x, y, z)
orientation = Quaternion(w=1.0)
# self.move_to('/map', position, orientation, 99)
return self.point_at(frame_id, x, y, z)
#I stole this from somewhere
# goal = ErraticBaseGoal()
# goal.target_pose.header.stamp = rospy.Time.now()
# goal.target_pose.header.frame_id = frame_id
# goal.target_pose.pose.position = position
# goal.target_pose.pose.orientation = orientation
# goal.vicinity_range = vicinity
# self.move_client.send_goal(goal)
# self.move_client.wait_for_result()
def go_to_cobra(self):
goal = SmartArmGoal()
goal.target_point = PointStamped()
goal.target_point.header.frame_id = 'arm_base_link'
goal.target_point.point.x = 0
goal.target_point.point.y = 0
goal.target_point.point.z = 0
goal.target_joints = [0.0, 1.972222, -1.972222, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
return self.arm_client.get_result()
#stolen gripper code from Anh Tran's "block_stacking_actions.py" in wubble_blocks
#would be cool if could point with a single claw rather than both (or with 'thumb'?)
def move_gripper(self, left_finger, right_finger):
success = False
goal = SmartArmGripperGoal()
goal.target_joints = [left_finger, right_finger]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
if self.gripper_client.get_result():
success = True
return success
class SimpleExerciser(unittest.TestCase):
def setUp(self):
self.default_wait = rospy.Duration(60.0)
self.client = SimpleActionClient('test_request_action', TestRequestAction)
self.assert_(self.client.wait_for_server(self.default_wait))
def test_just_succeed(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
def test_just_abort(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_ABORTED,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
def test_just_preempt(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
delay_terminate = rospy.Duration(100000),
the_result = 42)
self.client.send_goal(goal)
# Ensure that the action server got the goal, before continuing
timeout_time = rospy.Time.now() + self.default_wait
while rospy.Time.now() < timeout_time:
if (self.client.get_state() != GoalStatus.PENDING):
break
self.client.cancel_goal()
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
# Should print out errors about not setting a terminal status in the action server.
def test_drop(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_DROP,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
self.assertEqual(0, self.client.get_result().the_result)
# Should print out errors about throwing an exception
def test_exception(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_EXCEPTION,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
self.assertEqual(0, self.client.get_result().the_result)
def test_ignore_cancel_and_succeed(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
delay_terminate = rospy.Duration(2.0),
ignore_cancel = True,
the_result = 42)
self.client.send_goal(goal)
# Ensure that the action server got the goal, before continuing
timeout_time = rospy.Time.now() + self.default_wait
while rospy.Time.now() < timeout_time:
if (self.client.get_state() != GoalStatus.PENDING):
break
self.client.cancel_goal()
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
def test_lose(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_LOSE,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.LOST, self.client.get_state())
import roslib; roslib.load_manifest('wubble2_robot')
import rospy
from actionlib import SimpleActionClient
from wubble2_robot.msg import WubbleLaserTiltAction
from wubble2_robot.msg import WubbleLaserTiltGoal
if __name__ == '__main__':
try:
rospy.init_node('tilt_neck_laser', anonymous=True)
client = SimpleActionClient('hokuyo_laser_tilt_action', WubbleLaserTiltAction)
client.wait_for_server()
goal = WubbleLaserTiltGoal()
goal.amplitude = 1.5
goal.offset = 0.0
goal.duration = 7.0
rospy.loginfo('Amplitude: %f, offset: %f, duration: %f' % (goal.amplitude, goal.offset, goal.duration))
client.send_goal(goal)
client.wait_for_result()
rospy.loginfo("%s" % str(client.get_result()))
except rospy.ROSInterruptException:
pass
class TestMoveActionCancelDrop(unittest.TestCase):
def setUp(self):
# create a action client of move group
self._move_client = SimpleActionClient('move_group', MoveGroupAction)
self._move_client.wait_for_server()
moveit_commander.roscpp_initialize(sys.argv)
group_name = moveit_commander.RobotCommander().get_group_names()[0]
group = moveit_commander.MoveGroupCommander(group_name)
# prepare a joint goal
self._goal = MoveGroupGoal()
self._goal.request.group_name = group_name
self._goal.request.max_velocity_scaling_factor = 0.1
self._goal.request.max_acceleration_scaling_factor = 0.1
self._goal.request.start_state.is_diff = True
goal_constraint = Constraints()
joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
joint_names = group.get_active_joints()
for i in range(len(joint_names)):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_names[i]
joint_constraint.position = joint_values[i]
joint_constraint.weight = 1.0
goal_constraint.joint_constraints.append(joint_constraint)
self._goal.request.goal_constraints.append(goal_constraint)
self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True
def test_cancel_drop_plan_execution(self):
# send the goal
self._move_client.send_goal(self._goal)
# cancel the goal
self._move_client.cancel_goal()
# wait for result
self._move_client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = self._move_client.get_result()
rospy.sleep(0.1)
# check the error code in result
# error code is 0 if the server ends with RECALLED status
self.assertTrue(result.error_code.val == MoveItErrorCodes.PREEMPTED or result.error_code.val == 0)
def test_cancel_drop_plan_only(self):
# set the plan only flag
self._goal.planning_options.plan_only = True
# send the goal
self._move_client.send_goal(self._goal)
# cancel the goal
self._move_client.cancel_goal()
# wait for result
self._move_client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = self._move_client.get_result()
rospy.sleep(0.1)
# check the error code in result
# error code is 0 if the server ends with RECALLED status
self.assertTrue(result.error_code.val == MoveItErrorCodes.PREEMPTED or result.error_code.val == 0)
def test_cancel_resend(self):
# send the goal
self._move_client.send_goal(self._goal)
# cancel the goal
self._move_client.cancel_goal()
# send the goal again
self._move_client.send_goal(self._goal)
# wait for result
self._move_client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = self._move_client.get_result()
rospy.sleep(0.1)
# check the error code in result
self.assertEqual(result.error_code.val, MoveItErrorCodes.SUCCESS)
class arm_bot_base():
def __init__(self, desired_z, KpLin, KdLin, KpRot, KdRot, tol_z, tol_ang, maxLinSpeed, maxRotSpeed, max_z, min_z):
#global
self.speed = Twist()
self.tracked_blob = Twist()
self.state = FSM_STATES.FSM_LOCALIZE
self.obj_pose = MoveBaseGoal()
self.goal = MoveBaseGoal()
self.blobs_3d = Blobs3d()
#params
self.desired_z = desired_z #0.001 #0.8
self.KpLin = KpLin #1000.0
self.KdLin = KdLin
self.KpRot = KpRot #0.1
self.KdRot = KdRot
self.tol_z = tol_z #0.00005 #0.05
self.tol_ang= tol_ang #0.5
self.maxLinSpeed = maxLinSpeed #0.2
self.maxRotSpeed = maxRotSpeed
self.max_z = max_z #3.0
self.min_z= min_z #0.0005 #0.2
# move_base Action Client
self._move_base = SimpleActionClient('move_base', MoveBaseAction)
#subscribtions
rospy.Subscriber('/blobs_3d', Blobs3d, self.blob_callback)
rospy.Subscriber('/arm_bot_base/goal', PoseStamped, self.goal_callback)
# publications
self.speed_pub = rospy.Publisher('/navigation_velocity_smoother/raw_cmd_vel', Twist) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity
self.state_pub = rospy.Publisher('/arm_bot_base/state', String, queue_size=10) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity
#################
# goal_callback : Run everytime '\arm_bot_base\goal' topic receives message.
# # Send a goal to the Action Server upon callback
#################
def goal_callback(self, pose_stamped):
self.goal.target_pose = pose_stamped
if(self.goal):
self._move_base.send_goal(self.goal)
rospy.loginfo("Just passed a goal")
#################
# blob_callback : Run everytime \blobs_3d receives message.
# # Used to set rate of node and calls speed_callback() on every run
#################
def blob_callback(self, blobs_3d):
self.blobs_3d = blobs_3d
self.blob_area = 0
if(len(blobs_3d.blobs)):
for blob in blobs_3d.blobs:
if (blob.area>self.blob_area):
self.blob_area = blob.area
self.tracked_blob.linear = self.getPose(blob)
rospy.loginfo("Blob distance: %f" %(self.tracked_blob.linear.z))
#self.state = FSM_STATES.FSM_OPTIMIZE
else:
rospy.loginfo("No blobs detected")
#self.state = FSM_STATES.FSM_SEARCH
#################
# speed_callback : Pubishes velocity and handles state transitions
#################
def speed_callback(self):
self.zStable = False # flag to signify desired z satisfied
self.angStable = False # flag to signify desired angle satisfied
self.moveDuration = rospy.Duration(3) # duration of forward movement
print ("\n")
rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state))
# FSM_LOCALIZE STATE
# =============>
if self.state == FSM_STATES.FSM_LOCALIZE:
# Spin
self.speed.linear.x = 0.0
self.speed.angular.z = 0.5
# Next state transition
# if-else statement to ensure error-less view of goal status
if (self._move_base.simple_state == 2):
self.goal_state = 'No goals received'
rospy.loginfo("Goal Status: %s" %self.goal_state)
self.state = FSM_STATES.FSM_LOCALIZE
elif(len(self.blobs_3d.blobs)): # if a blob is seen
rospy.loginfo("Object detected! Approaching object...")
self.state = FSM_STATES.FSM_OPTIMIZE
else: # if a goal is detected
self.goal_state = self._move_base.get_state()
rospy.loginfo("Goal Status: %f" %self.goal_state)
self.state = FSM_STATES.FSM_NAVIGATE
# FSM_NAVIGATE STATE
# =============>
elif self.state == FSM_STATES.FSM_NAVIGATE:
# No action required, simply allow path planner to navigate
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
# Next state transitions
if(len(self.blobs_3d.blobs) ): # if a blob is seen
self._move_base.cancel_all_goals() # cancell all nav goals
rospy.loginfo("Goal Status: %f" %self._move_base.get_state())
rospy.loginfo("Object detected! Aborting goal and approaching object...")
self.state = FSM_STATES.FSM_OPTIMIZE
else:
self.state = FSM_STATES.FSM_NAVIGATE
# FSM_SEARCH STATE
# ===============>
elif self.state == FSM_STATES.FSM_SEARCH:
# Spin to search for object
self.speed.linear.x = 0.0
self.speed.angular.z = 0.2
# Next state transitions
if (len(self.blobs_3d.blobs)):
self.state = FSM_STATES.FSM_OPTIMIZE
else:
self.state = FSM_STATES.FSM_SEARCH
# FSM_OPTIMIZE STATE (PID/PD controller)
# =================>
elif self.state == FSM_STATES.FSM_OPTIMIZE :
rospy.loginfo("Blob detected at x:%f, y:%f, z:%f "%(self.tracked_blob.linear.x, self.tracked_blob.linear.y, self.tracked_blob.linear.z))
#self.LinDerivator = 0.0
self.RotDerivator = 0.0
# if statement to check object is with range (TO BE DELETED/AMENDED!)
if (self.tracked_blob.linear.z > self.max_z or self.tracked_blob.linear.z < self.min_z):
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
rospy.loginfo("Detected object is either too close or too far away.")
else:
# Compute distance PD error
#self.zError = self.tracked_blob.linear.z - self.desired_z
#self.pLinError = self.zError*self.KpLin
#self.dLinError = self.KdLin*(self.zError-self.LinDerivator)
#self.LinDerivator = self.zError
#self.speedPID = self.pLinError + self.dLinError
# Cap maximum linear speed
#if(math.fabs(self.speedPID)>self.maxLinSpeed):
# self.speedPID=self.maxLinSpeed*math.fabs(self.speedPID)/self.speedPID;
# Set tolerance in z to signify stable position
#if(math.fabs(self.zError)<self.tol_z):
# self.speed.linear.x = 0.0
# LinDerivator = 0.0
# self.zStable = True
#else:
# self.speed.linear.x = self.speedPID
# self.zStable = False
# Set linear speed to 0
self.speed.linear.x = 0.0
# Compute PD angular error
self.angError = -math.atan2(self.tracked_blob.linear.x, self.tracked_blob.linear.y);
self.pRotError = self.KpRot*self.angError
self.dRotError = self.KdRot*(self.angError-self.RotDerivator)
self.RotDerivator = self.angError
self.anglePID = self.pRotError + self.dRotError;
# Cap maximum rotation speed
if(math.fabs(self.anglePID)>self.maxRotSpeed):
self.anglePID=self.maxRotSpeed*math.fabs(self.anglePID)/self.anglePID;
# Set tolerance in angle to signify stable orientation
if(math.fabs(self.angError) < self.tol_ang):
self.speed.angular.z = 0.0;
RotDerivator = 0.0
self.angStable = True
else:
self.speed.angular.z = self.anglePID;
self.angStable = False
# Debug messages
rospy.loginfo("distance: %f" %(self.tracked_blob.linear.z))
rospy.loginfo("angError: %f, pRotError: %f, dRotError: %f" %(self.angError, self.pRotError, self.dRotError))
# Next state transitions
if (self.angStable): # if stable position and orientation have been achieved
self.state = FSM_STATES.FSM_APPROACH_OBJ # move to stable state
self.moveStartTime = rospy.Time.now()
elif(len(self.blobs_3d.blobs)): # else if (while) blobs are being detected
self.state = FSM_STATES.FSM_OPTIMIZE # loop in same state
else:
self.state = FSM_STATES.FSM_LOCALIZE # else move to nav state
# FSM_APPROACH_OBJ STATE
# =============>
elif self.state == FSM_STATES.FSM_APPROACH_OBJ:
self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame'
self.obj_pose.target_pose.header.stamp = rospy.Time.now()
self.obj_pose.target_pose.pose.orientation.w = 1.0
# if-else statements to perform corrections in target pose
# if distance is higher than 2.20m (4.5 m gazebo)
if ( self.tracked_blob.linear.z > 0.0045):
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.004) # set goal to (distance - 2m)
self._move_base.send_goal(self.obj_pose)
rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
self._move_base.wait_for_result()
rospy.loginfo(self._move_base.get_result())
self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle
# else if distance is less than 2m and more than 1.20m (2.5 m gazebo)
elif (self.tracked_blob.linear.z > 0.0025):
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.002) # set goal to (distance - 1m)
self._move_base.send_goal(self.obj_pose)
rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
self._move_base.wait_for_result()
rospy.loginfo(self._move_base.get_result())
self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle
# else if distance is less than 1m
else:
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.4 # set goal to final target
self._move_base.send_goal(self.obj_pose)
rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
self._move_base.wait_for_result()
rospy.loginfo(self._move_base.get_result())
self.state = FSM_STATES.FSM_SPIN # move to FSM_SPIN
# if ( self.tracked_blob.linear.z > 0.004):
# while(self.tracked_blob.linear.z > 0.003):
# pass
# self._move_base.cancel_all_goals() # cancell all nav goals
# self.state = FSM_STATES.FSM_OPTIMIZE
# elif ( self.tracked_blob.linear.z > 0.001):
# while(self.tracked_blob.linear.z > 0.001):
# pass
# self._move_base.cancel_all_goals() # cancell all nav goals
# self.state = FSM_STATES.FSM_OPTIMIZE
# else:
# self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame'
# self.obj_pose.target_pose.header.stamp = rospy.Time.now()
# self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.4
# self.obj_pose.target_pose.pose.orientation.w = 1.0
# self._move_base.send_goal(self.obj_pose)
# rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
# self._move_base.wait_for_result()
# rospy.loginfo(self._move_base.get_result())
# self.state= FSM_STATES.FSM_SPIN
# FSM_STABLE STATE
# ===============>
elif (self.state == FSM_STATES.FSM_STABLE):
# Move forward for desired duration to approach target object
if (rospy.Time.now() < self.moveStartTime + self.moveDuration):
self.speed.linear.x = 0.2
self.state = FSM_STATES.FSM_STABLE
# Once duration has elapsed move to spin state
else:
self.speed.linear.x = 0.0
self.state = FSM_STATES.FSM_SPIN
# FSM_SPIN STATE
# =============>
elif (self.state == FSM_STATES.FSM_SPIN):
# Spin
self.speed.linear.x = 0.0
self.speed.angular.z = 0.5
# Next state transitions
# -Until arm sees object-!
self.state_pub.publish(String(FSM_STATES.__geitem__(self.state)))
# Print debug info about linear and angular speeds
rospy.loginfo("speedPID: %f" %(self.speed.linear.x))
rospy.loginfo("anglePID: %f" %(self.speed.angular.z))
#Only publish velocity when no goal is being pursued or expected
if (self.state != FSM_STATES.FSM_NAVIGATE):
self.speed_pub.publish(self.speed)
############
# getPose(): Returns pose of received blob relative to the camera.
############
def getPose(self, blob):
self.pose = Vector3()
self.pose.x = blob.center.x
self.pose.y = blob.center.y
self.pose.z = blob.center.z
return self.pose
def spin(self):
rate = rospy.Rate(RATE)
while not rospy.is_shutdown():
self.speed_callback()
rate.sleep()
class GraspObjectServer:
def __init__(self, name):
# stuff for grasp planning
self.tf_listener = tf.TransformListener()
self.tf_broadcaster = tf.TransformBroadcaster()
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_objects = RecognizedObjectArray()
#rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback)
self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback)
self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
#pickup_ac.wait_for_server() # needed?
rospy.loginfo("Connecting to grasp generator AS")
self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
#grasps_ac.wait_for_server() # needed?
#planning scene for motion planning
self.scene = PlanningSceneInterface()
# blocking action server
self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False)
self.feedback = GraspObjectFeedback()
self.result = GraspObjectResult()
self.current_goal = None
self.grasp_obj_as.start()
def objects_callback(self, data):
rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects))
self.last_objects = data
def goal_callback(self, goal):
if self.current_goal:
goal.set_rejected() # "Server busy"
return
elif len(self.last_objects.objects) - 1 < goal.get_goal().target_id:
goal.set_rejected() # "No objects to grasp were received on the objects topic."
return
else:
#store and accept new goal
self.current_goal = goal
self.current_goal.set_accepted()
#run grasping state machine
self.grasping_sm()
#finished, get rid of goal
self.current_goal = None
def cancel_callback(self, goal):
#TODO stop motions?
self.current_goal.set_canceled()
def grasping_sm(self):
if self.current_goal:
self.update_feedback("running clustering")
(object_points, obj_bbox_dims,
object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(self.last_objects.objects[self.current_goal.get_goal().target_id].point_clouds[0])
#TODO visualize bbox
#TODO publish filtered pointcloud?
print obj_bbox_dims
########
self.update_feedback("check reachability")
########
self.update_feedback("generate grasps")
#transform pose to base_link
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(5))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
#HACK remove orientation -> pose is aligned with parent(base_link)
object_pose.pose.orientation.w = 1.0
object_pose.pose.orientation.x = 0.0
object_pose.pose.orientation.y = 0.0
object_pose.pose.orientation.z = 0.0
# shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
# TODO remove this hack, fix it in table filtering
object_pose.pose.position.z += obj_bbox_dims[2]/2.0
grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x
# check if there are grasps, if not, abort
if len(grasp_list) == 0:
self.update_aborted("no grasps received")
return
self.publish_grasps_as_poses(grasp_list)
self.feedback.grasps = grasp_list
self.current_goal.publish_feedback(self.feedback)
self.result.grasps = grasp_list
########
self.update_feedback("setup planning scene")
#remove old objects
self.scene.remove_world_object("object_to_grasp")
# add object to grasp to planning scene
self.scene.add_box("object_to_grasp", object_pose,
(obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
self.result.object_scene_name = "object_to_grasp"
########
if self.current_goal.get_goal().execute_grasp:
self.update_feedback("execute grasps")
pug = self.createPickupGoal("object_to_grasp", grasp_list)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(pug)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.loginfo("Result is:")
print result
rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
########
self.update_feedback("finished")
self.result.object_pose = object_pose
#bounding box in a point message
self.result.bounding_box = Point()
self.result.bounding_box.x = obj_bbox_dims[0]
self.result.bounding_box.y = obj_bbox_dims[1]
self.result.bounding_box.z = obj_bbox_dims[2]
self.current_goal.set_succeeded(result = self.result)
#self.current_goal.set_aborted()
def update_feedback(self, text):
self.feedback.last_state = text
self.current_goal.publish_feedback(self.feedback)
def update_aborted(self, text = ""):
self.update_feedback("aborted." + text)
self.current_goal.set_aborted()
def generate_grasps(self, pose, width):
#send request to block grasp generator service
if not self.grasps_ac.wait_for_server(rospy.Duration(3.0)):
return []
rospy.loginfo("Successfully connected.")
goal = GenerateBlockGraspsGoal()
goal.pose = pose.pose
goal.width = width
self.grasps_ac.send_goal(goal)
rospy.loginfo("Sent goal, waiting:\n" + str(goal))
t_start = rospy.Time.now()
self.grasps_ac.wait_for_result()
t_end = rospy.Time.now()
t_total = t_end - t_start
rospy.loginfo("Result received in " + str(t_total.to_sec()))
grasp_list = self.grasps_ac.get_result().grasps
return grasp_list
def publish_grasps_as_poses(self, grasps):
rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
self.grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def createPickupGoal(self, target, possible_grasps, group="right_arm_torso_grasping"):
""" Create a PickupGoal with the provided data"""
pug = PickupGoal()
pug.target_name = target
pug.group_name = group
pug.possible_grasps.extend(possible_grasps)
pug.allowed_planning_time = 5.0
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
pug.planning_options.replan = True
pug.planning_options.replan_attempts = 10
pug.attached_object_touch_links = ['arm_right_5_link', "arm_right_grasping_frame"]
pug.allowed_touch_objects.append(target)
#pug.attached_object_touch_links.append('all')
return pug
class CokeCanPickAndPlace:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~arm', 'arm')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.6)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_coke_can(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y + 0.02
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
def _add_table(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.22
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (0.5, 0.4, 0.02))
return p.pose
def _add_coke_can(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.25
p.pose.position.y = 0
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.01, 0.01, 0.009))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle)
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame()
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = -1
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame()
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 1
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
class ObjectManipulationAS:
def __init__(self, name):
# stuff for grasp planning
rospy.loginfo("Getting a TransformListener...")
self.tf_listener = tf.TransformListener()
rospy.loginfo("Getting a TransformBroadcaster...")
self.tf_broadcaster = tf.TransformBroadcaster()
rospy.loginfo("Initializing a ClusterBoundingBoxFinder...")
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_clusters = None
rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...")
self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback)
if DEBUG_MODE:
self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped)
rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...")
self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...")
self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...")
self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction)
self.grasps_ac.wait_for_server()
rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...")
self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty)
self.depth_service.wait_for_service()
rospy.loginfo("Getting a PlanningSceneInterface instance...")
self.scene = PlanningSceneInterface()
# blocking action server
rospy.loginfo("Creating Action Server '" + name + "'...")
self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False)
self.as_feedback = ObjectManipulationFeedback()
self.as_result = ObjectManipulationActionResult()
self.current_goal = None
# Take care of left and right arm grasped stuff
self.right_hand_object = None
self.left_hand_object = None
self.current_side = 'right'
rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!")
self.grasp_obj_as.start()
def objects_callback(self, data):
rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects))
self.last_clusters = data
def goal_callback(self, goal):
if self.current_goal:
goal.set_rejected() # "Server busy"
return
# TODO: Check if pose is not empty, if it is, reject
# elif len(self.last_clusters.objects) - 1 < goal.get_goal().target_id:
# goal.set_rejected() # "No objects to grasp were received on the objects topic."
# return
else:
#store and accept new goal
self.current_goal = goal
self.current_goal.set_accepted()
#run the corresponding operation
if self.current_goal.get_goal().operation == ObjectManipulationGoal.PICK:
rospy.loginfo("ObjectManipulation: PICK!")
self.pick_operation()
elif self.current_goal.get_goal().operation == ObjectManipulationGoal.PLACE:
rospy.loginfo("ObjectManipulation: PLACE!")
self.place_operation()
else:
rospy.logerr("ObjectManipulation: Erroneous operation!")
#finished, get rid of goal
self.current_goal = None
def cancel_callback(self, goal):
#TODO stop motions?
self.current_goal.set_canceled()
def pick_operation(self):
"""Execute pick operation"""
if self.message_fields_ok():
self.as_result = ObjectManipulationResult()
goal_message_field = self.current_goal.get_goal()
self.update_feedback("Checking hand to use")
# Check which arm group was requested and if it's currently free of objects
if 'right' in goal_message_field.group:
if self.right_hand_object: # Something already in the hand
self.update_aborted("Right hand already contains an object.")
return # necessary?
self.current_side = 'right'
elif 'left' in goal_message_field.group:
if self.left_hand_object:
self.update_aborted("Left hand already contains an object.")
return
self.current_side = 'left'
# Publish pose of goal position
if DEBUG_MODE:
self.to_grasp_object_pose_pub.publish(goal_message_field.target_pose)
self.update_feedback("Detecting clusters")
if not self.wait_for_recognized_array(wait_time=5, timeout_time=10): # wait until we get clusters published
self.update_aborted("Failed detecting clusters")
# Search closer cluster
# transform pose to base_link if needed
if goal_message_field.target_pose.header.frame_id != "base_link":
self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5))
object_to_grasp_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose)
else:
object_to_grasp_pose = goal_message_field.target_pose.pose
self.update_feedback("Searching closer cluster while clustering")
(closest_cluster_id, (object_points, obj_bbox_dims, object_bounding_box, object_pose)) = self.get_id_of_closest_cluster_to_pose(object_to_grasp_pose)
rospy.logdebug("in AS: Closest cluster id is: " + str(closest_cluster_id))
#TODO visualize bbox
#TODO publish filtered pointcloud?
rospy.logdebug("BBOX: " + str(obj_bbox_dims))
########
self.update_feedback("Check reachability")
########
self.update_feedback("Generating grasps")
rospy.logdebug("Object pose before tf thing is: " + str(object_pose))
#transform pose to base_link, IS THIS NECESSARY?? should be a function in any case
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
#HACK remove orientation -> pose is aligned with parent(base_link)
object_pose.pose.orientation.w = 1.0
object_pose.pose.orientation.x = 0.0
object_pose.pose.orientation.y = 0.0
object_pose.pose.orientation.z = 0.0
# shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
# TODO remove this hack, fix it in table filtering
object_pose.pose.position.z += obj_bbox_dims[2] / 2.0
grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x
# check if there are grasps, if not, abort
if len(grasp_list) == 0:
self.update_aborted("No grasps received")
return
if DEBUG_MODE: # TODO: change to dynamic param
publish_grasps_as_poses(grasp_list)
self.current_goal.publish_feedback(self.as_feedback)
########
self.update_feedback("Setup planning scene")
# Add object to grasp to planning scene
self.scene.add_box(self.current_side + "_hand_object", object_pose,
(obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
self.as_result.object_scene_name = self.current_side + "_hand_object"
########
self.update_feedback("Execute grasps")
pug = createPickupGoal(self.current_side + "_hand_object", grasp_list, group=goal_message_field.group)
rospy.loginfo("Sending pickup goal")
self.pickup_ac.send_goal(pug)
rospy.loginfo("Waiting for result...")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
########
self.update_feedback("finished")
self.as_result.object_pose = object_pose
self.as_result.error_code = result.error_code
self.as_result.error_message = str(moveit_error_dict[result.error_code.val])
if result.error_code.val == MoveItErrorCodes.SUCCESS:
if self.current_side == 'right':
self.right_hand_object = self.current_side + "_hand_object"
elif self.current_side == 'left':
self.left_hand_object = self.current_side + "_hand_object"
self.current_goal.set_succeeded(result=self.as_result)
else:
# Remove object as it has not been picked
self.scene.remove_world_object(self.current_side + "_hand_object")
self.update_aborted(text="MoveIt pick failed", manipulation_result=self.as_result)
else:
self.update_aborted("Goal fields not correctly fulfilled")
def place_operation(self):
"""Execute the place operation"""
if self.message_fields_ok():
self.as_result = ObjectManipulationResult()
goal_message_field = self.current_goal.get_goal()
self.update_feedback("Checking arm to use")
# Check which arm group was requested and if it currently has an object
if 'right' in goal_message_field.group:
if not self.right_hand_object: # Something already in the hand
self.update_aborted("Right hand does not contain an object.")
return # necessary?
self.current_side = 'right'
current_target = self.right_hand_object
elif 'left' in goal_message_field.group:
if not self.left_hand_object:
self.update_aborted("Left hand does not contain an object.")
return
self.current_side = 'left'
current_target = self.left_hand_object
# transform pose to base_link if needed
if goal_message_field.target_pose.header.frame_id != "base_link":
self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5))
placing_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose)
else:
placing_pose = goal_message_field.target_pose.pose
#### TODO: ADD HERE LOGIC ABOUT SEARCHING GOOD PLACE POSE ####
self.update_feedback("Sending place order to MoveIt!")
placing_pose = PoseStamped(header=Header(frame_id="base_link"), pose=placing_pose)
goal = createPlaceGoal(placing_pose, group=goal_message_field.group, target=current_target)
rospy.loginfo("Sending place 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("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
self.as_result.object_pose = placing_pose
self.as_result.error_code = result.error_code
self.as_result.error_message = str(moveit_error_dict[result.error_code.val])
if result.error_code.val == MoveItErrorCodes.SUCCESS:
# Emptying hand
self.update_feedback("Emptying hand")
if self.current_side == 'right':
self.as_result.object_scene_name = current_target
self.right_hand_object = None
elif self.current_side == 'left':
self.as_result.object_scene_name = current_target
self.left_hand_object = None
# Removing object from the collision world
self.scene.remove_world_object(current_target)
self.current_goal.set_succeeded(result=self.as_result)
else:
self.update_aborted(text="MoveIt place failed", manipulation_result=self.as_result)
else:
self.update_aborted("Goal fields not correctly fulfilled")
def message_fields_ok(self):
"""Check if the minimal fields for the message are fulfilled"""
if self.current_goal:
goal_message_field = self.current_goal.get_goal()
if len(goal_message_field.group) == 0:
rospy.logwarn("Group field empty.")
return False
if goal_message_field.operation != ObjectManipulationGoal.PICK and goal_message_field.operation != ObjectManipulationGoal.PLACE:
rospy.logwarn("Asked for an operation different from PICK or PLACE: " + str(goal_message_field.operation))
return False
if len(goal_message_field.target_pose.header.frame_id) == 0:
rospy.logwarn("Target pose frame_id is empty")
return False
return True
def update_feedback(self, text):
"""Publish feedback with given text"""
self.as_feedback.last_state = text
self.current_goal.publish_feedback(self.as_feedback)
def update_aborted(self, text="", manipulation_result=None):
"""Publish feedback and abort with the text give and optionally an ObjectManipulationResult already fulfilled"""
self.update_feedback("aborted." + text)
if manipulation_result == None:
aborted_result = ObjectManipulationResult()
aborted_result.error_message = text
self.current_goal.set_aborted(result=aborted_result)
else:
self.current_goal.set_aborted(result=manipulation_result)
def generate_grasps(self, pose, width):
"""Send request to block grasp generator service"""
goal = GenerateGraspsGoal()
goal.pose = pose.pose
goal.width = width
grasp_options = GraspGeneratorOptions()
grasp_options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Y
grasp_options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_DOWN
grasp_options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_HALF
goal.options.append(grasp_options)
self.grasps_ac.send_goal(goal)
if DEBUG_MODE:
rospy.loginfo("Sent goal, waiting:\n" + str(goal))
self.grasps_ac.wait_for_result()
grasp_list = self.grasps_ac.get_result().grasps
return grasp_list
def get_id_of_closest_cluster_to_pose(self, input_pose):
"""Returns the id of the closest cluster to the pose provided (in Pose or PoseStamped)
and all the associated clustering information"""
current_id = 0
closest_pose = None
closest_object_points = None
closest_id = 0
closest_bbox = None
closest_bbox_dims = None
closest_distance = 99999.9
for myobject in self.last_clusters.objects:
(object_points, obj_bbox_dims, object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(myobject.point_clouds[0])
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
rospy.loginfo("id: " + str(current_id) + "\n pose:\n" + str(object_pose))
if closest_pose == None: # first loop iteration
closest_object_points = object_points
closest_pose = object_pose
closest_bbox = object_bounding_box
closest_bbox_dims = obj_bbox_dims
closest_distance = dist_between_poses(closest_pose, input_pose)
else:
if dist_between_poses(object_pose, input_pose) < closest_distance:
closest_object_points = object_points
closest_distance = dist_between_poses(object_pose, input_pose)
closest_pose = object_pose
closest_bbox = object_bounding_box
closest_bbox_dims = obj_bbox_dims
closest_id = current_id
current_id += 1
rospy.loginfo("Closest id is: " + str(closest_id) + " at " + str(closest_pose))
return closest_id, (closest_object_points, closest_bbox_dims, closest_bbox_dims, closest_pose)
def wait_for_recognized_array(self, wait_time=6, timeout_time=10):
"""Ask for depth images until we get a recognized array
wait for wait_time between depth throtle calls
stop if timeout_time is achieved
If we dont find it in the correspondent time return false, true otherwise"""
initial_time = rospy.Time.now()
self.last_clusters = None
count = 0
num_calls = 1
self.depth_service.call(EmptyRequest())
rospy.loginfo("Depth throtle server call #" + str(num_calls))
rospy.loginfo("Waiting for a recognized array...")
while rospy.Time.now() - initial_time < rospy.Duration(timeout_time) and self.last_clusters == None:
rospy.sleep(0.1)
count += 1
if count >= wait_time / 10:
self.depth_service.call(EmptyRequest())
num_calls += 1
rospy.loginfo("Depth throtle server call #" + str(num_calls))
if self.last_clusters == None:
return False
else:
return True
class GiskardWrapper(object):
def __init__(self, node_name=u'giskard'):
giskard_topic = u'{}/command'.format(node_name)
if giskard_topic is not None:
self._client = SimpleActionClient(giskard_topic, MoveAction)
self._update_world_srv = rospy.ServiceProxy(
u'{}/update_world'.format(node_name), UpdateWorld)
self._get_object_names_srv = rospy.ServiceProxy(
u'{}/get_object_names'.format(node_name), GetObjectNames)
self._get_object_info_srv = rospy.ServiceProxy(
u'{}/get_object_info'.format(node_name), GetObjectInfo)
self._update_rviz_markers_srv = rospy.ServiceProxy(
u'{}/update_rviz_markers'.format(node_name), UpdateRvizMarkers)
self._get_attached_objects_srv = rospy.ServiceProxy(
u'{}/get_attached_objects'.format(node_name),
GetAttachedObjects)
self._marker_pub = rospy.Publisher(u'visualization_marker_array',
MarkerArray,
queue_size=10)
rospy.wait_for_service(u'{}/update_world'.format(node_name))
self._client.wait_for_server()
self.robot_urdf = URDFObject(rospy.get_param(u'robot_description'))
self.collisions = []
self.clear_cmds()
self._object_js_topics = {}
rospy.sleep(.3)
def get_robot_name(self):
"""
:rtype: str
"""
return self.robot_urdf.get_name()
def get_root(self):
"""
Returns the name of the robot's root link
:rtype: str
"""
return self.robot_urdf.get_root()
def get_robot_links(self):
"""
Returns a list of the robots links
:rtype: dict
"""
return self.robot_urdf.get_link_names()
def get_joint_states(self, topic=u'joint_states', timeout=1):
"""
Returns a dictionary of all joints (key) and their position (value)
:param topic: joint_state topic
:param timeout: duration to wait for JointState msg
:return: OrderedDict[str, float]
"""
try:
msg = rospy.wait_for_message(topic, JointState,
rospy.Duration(timeout))
return to_joint_state_position_dict(msg)
except rospy.ROSException:
rospy.logwarn("get_joint_states: wait_for_message timeout")
return {}
def set_cart_goal(self,
root_link,
tip_link,
goal_pose,
max_linear_velocity=None,
max_angular_velocity=None,
weight=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link into the goal pose
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose
:type goal_pose: PoseStamped
:param max_linear_velocity: m/s, default 0.1
:type max_linear_velocity: float
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
self.set_translation_goal(root_link,
tip_link,
goal_pose,
max_velocity=max_linear_velocity,
weight=weight)
self.set_rotation_goal(root_link,
tip_link,
goal_pose,
max_velocity=max_angular_velocity,
weight=weight)
def set_translation_goal(self,
root_link,
tip_link,
goal_pose,
weight=None,
max_velocity=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link into the goal position
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose, orientation will be ignored
:type goal_pose: PoseStamped
:param max_velocity: m/s, default 0.1
:type max_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
if not max_velocity and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.TRANSLATION_3D
constraint.root_link = str(root_link)
constraint.tip_link = str(tip_link)
constraint.goal = goal_pose
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianPosition'
params = {}
params[u'root_link'] = root_link
params[u'tip_link'] = tip_link
params[u'goal'] = convert_ros_message_to_dictionary(goal_pose)
if max_velocity:
params[u'max_velocity'] = max_velocity
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_rotation_goal(self,
root_link,
tip_link,
goal_pose,
weight=None,
max_velocity=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link into the goal orientation
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose, position will be ignored
:type goal_pose: PoseStamped
:param max_velocity: rad/s, default 0.5
:type max_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
if not max_velocity and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.ROTATION_3D
constraint.root_link = str(root_link)
constraint.tip_link = str(tip_link)
constraint.goal = goal_pose
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianOrientationSlerp'
params = {}
params[u'root_link'] = root_link
params[u'tip_link'] = tip_link
params[u'goal'] = convert_ros_message_to_dictionary(goal_pose)
if max_velocity:
params[u'max_velocity'] = max_velocity
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_joint_goal(self, goal_state, weight=None, max_velocity=None):
"""
This goal will move the robots joint to the desired configuration.
:param goal_state: Can either be a joint state messages or a dict mapping joint name to position.
:type goal_state: Union[JointState, dict]
:param weight: default WEIGHT_BELOW_CA
:type weight: float
:param max_velocity: default is the default of the added joint goals
:type max_velocity: float
"""
if weight is None and max_velocity is None:
constraint = JointConstraint()
constraint.type = JointConstraint.JOINT
if isinstance(goal_state, JointState):
constraint.goal_state = goal_state
else:
for joint_name, joint_position in goal_state.items():
constraint.goal_state.name.append(joint_name)
constraint.goal_state.position.append(joint_position)
self.cmd_seq[-1].joint_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = JointConstraint.JOINT
if isinstance(goal_state, JointState):
goal_state = goal_state
else:
goal_state2 = JointState()
for joint_name, joint_position in goal_state.items():
goal_state2.name.append(joint_name)
goal_state2.position.append(joint_position)
goal_state = goal_state2
params = {}
params[u'goal_state'] = convert_ros_message_to_dictionary(
goal_state)
if weight is not None:
params[u'weight'] = weight
if max_velocity is not None:
params[u'max_velocity'] = max_velocity
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def align_planes(self,
tip_link,
tip_normal,
root_link=None,
root_normal=None,
max_angular_velocity=None,
weight=WEIGHT_ABOVE_CA):
"""
This Goal will use the kinematic chain between tip and root normal to align both
:param root_link: name of the root link for the kinematic chain, default robot root link
:type root_link: str
:param tip_link: name of the tip link for the kinematic chain
:type tip_link: str
:param tip_normal: normal at the tip of the kin chain, default is z axis of robot root link
:type tip_normal: Vector3Stamped
:param root_normal: normal at the root of the kin chain
:type root_normal: Vector3Stamped
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param weight: default WEIGHT_BELOW_CA
:type weight: float
"""
if root_link is None:
root_link = self.get_root()
if root_normal is None:
root_normal = Vector3Stamped()
root_normal.header.frame_id = self.get_root()
root_normal.vector.z = 1
params = {
u'tip_link': tip_link,
u'tip_normal': tip_normal,
u'root_link': root_link,
u'root_normal': root_normal
}
if weight is not None:
params[u'weight'] = weight
if max_angular_velocity is not None:
params[u'max_angular_velocity'] = max_angular_velocity
self.set_json_goal(u'AlignPlanes', **params)
def avoid_joint_limits(self, percentage=15, weight=WEIGHT_BELOW_CA):
"""
This goal will push joints away from their position limits
:param percentage: default 15, if limits are 0-100, the constraint will push into the 15-85 range
:type percentage: float
:param weight: default WEIGHT_BELOW_CA
:type weight: float
"""
self.set_json_goal(u'AvoidJointLimits',
percentage=percentage,
weight=weight)
def limit_cartesian_velocity(self,
root_link,
tip_link,
weight=WEIGHT_ABOVE_CA,
max_linear_velocity=0.1,
max_angular_velocity=0.5,
hard=True):
"""
This goal will limit the cartesian velocity of the tip link relative to root link
:param root_link: root link of the kin chain
:type root_link: str
:param tip_link: tip link of the kin chain
:type tip_link: str
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
:param max_linear_velocity: m/s, default 0.1
:type max_linear_velocity: float
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param hard: default True, will turn this into a hard constraint, that will always be satisfied, can could
make some goal combination infeasible
:type hard: bool
"""
self.set_json_goal(u'CartesianVelocityLimit',
root_link=root_link,
tip_link=tip_link,
weight=weight,
max_linear_velocity=max_linear_velocity,
max_angular_velocity=max_angular_velocity,
hard=hard)
def grasp_bar(self,
root_link,
tip_link,
tip_grasp_axis,
bar_center,
bar_axis,
bar_length,
max_linear_velocity=0.1,
max_angular_velocity=0.5,
weight=WEIGHT_ABOVE_CA):
"""
This goal can be used to grasp bars. It's like a cartesian goal with some freedom along one axis.
:param root_link: root link of the kin chain
:type root_link: str
:param tip_link: tip link of the kin chain
:type tip_link: str
:param tip_grasp_axis: this axis of the tip will be aligned with bar_axis
:type tip_grasp_axis: Vector3Stamped
:param bar_center: center of the bar
:type bar_center: PointStamped
:param bar_axis: tip_grasp_axis will be aligned with this vector
:type bar_axis: Vector3Stamped
:param bar_length: length of the bar
:type bar_length: float
:param max_linear_velocity: m/s, default 0.1
:type max_linear_velocity: float
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
self.set_json_goal(u'GraspBar',
root_link=root_link,
tip_link=tip_link,
tip_grasp_axis=tip_grasp_axis,
bar_center=bar_center,
bar_axis=bar_axis,
bar_length=bar_length,
max_linear_velocity=max_linear_velocity,
max_angular_velocity=max_angular_velocity,
weight=weight)
def set_pull_door_goal(self,
tip_link,
object_name_prefix,
object_link_name,
angle_goal,
weight=WEIGHT_ABOVE_CA):
"""
:type tip_link: str
:param tip_link: tip of manipulator (gripper) which is used
:type object_name_prefix: object name link prefix
:param object_name_prefix: string
:type object_link_name str
:param object_link_name name of the object link name
:type object_link_name str
:param object_link_name handle to grasp
:type angle_goal: float
:param angle_goal: how far to open
:type weight float
:param weight Default = WEIGHT_ABOVE_CA
"""
self.set_json_goal(u'OpenDoor',
tip_link=tip_link,
object_name=object_name_prefix,
object_link_name=object_link_name,
angle_goal=angle_goal,
weight=weight)
def update_god_map(self, updates):
"""
don't use, it's only for hacks :)
"""
self.set_json_goal(u'UpdateGodMap', updates=updates)
def pointing(self,
tip_link,
goal_point,
root_link=None,
pointing_axis=None,
weight=None):
"""
Uses the kinematic chain from root_link to tip_link to move the pointing axis, such that it points to the goal point.
:param tip_link: name of the tip of the kin chain
:type tip_link: str
:param goal_point: where the pointing_axis will point towards
:type goal_point: PointStamped
:param root_link: name of the root of the kin chain
:type root_link: str
:param pointing_axis: default is z axis, this axis will point towards the goal_point
:type pointing_axis: Vector3Stamped
:param weight: default WEIGHT_BELOW_CA
:type weight: float
"""
kwargs = {u'tip_link': tip_link, u'goal_point': goal_point}
if root_link is not None:
kwargs[u'root_link'] = root_link
if pointing_axis is not None:
kwargs[u'pointing_axis'] = pointing_axis
if weight is not None:
kwargs[u'weight'] = weight
kwargs[u'goal_point'] = goal_point
self.set_json_goal(u'Pointing', **kwargs)
def set_json_goal(self, constraint_type, **kwargs):
"""
Set a goal for any of the goals defined in Constraints.py
:param constraint_type: Name of the Goal
:type constraint_type: str
:param kwargs: maps constraint parameter names to values. Values should be float, str or ros messages.
:type kwargs: dict
"""
constraint = Constraint()
constraint.type = constraint_type
for k, v in kwargs.items():
if isinstance(v, Message):
kwargs[k] = convert_ros_message_to_dictionary(v)
constraint.parameter_value_pair = json.dumps(kwargs)
self.cmd_seq[-1].constraints.append(constraint)
def set_collision_entries(self, collisions):
"""
Adds collision entries to the current goal
:param collisions: list of CollisionEntry
:type collisions: list
"""
self.cmd_seq[-1].collisions.extend(collisions)
def allow_collision(self,
robot_links=(CollisionEntry.ALL, ),
body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL, )):
"""
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def avoid_collision(self,
min_dist,
robot_links=(CollisionEntry.ALL, ),
body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL, )):
"""
:param min_dist: the distance giskard is trying to keep between specified links
:type min_dist: float
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.min_dist = min_dist
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def allow_all_collisions(self):
"""
Allows all collisions for next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def allow_self_collision(self):
"""
Allows the collision with itself for the next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def avoid_self_collision(self):
"""
Avoid collisions with itself for the next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def avoid_all_collisions(self, distance=0.05):
"""
Avoids all collisions for next goal. The distance will override anything from the config file.
If you don't want to override the distance, don't call this function. Avoid all is the default, if you don't
add any collision entries.
:param distance: the distance that giskard is trying to keep from all objects
:type distance: float
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
collision_entry.min_dist = distance
self.set_collision_entries([collision_entry])
def add_cmd(self):
"""
Adds another command to the goal sequence. Any set goal calls will be added the this new goal.
This is used, if you want Giskard to plan multiple goals in succession.
"""
move_cmd = MoveCmd()
self.cmd_seq.append(move_cmd)
def clear_cmds(self):
"""
Removes all move commands from the current goal, collision entries are left untouched.
"""
self.cmd_seq = []
self.add_cmd()
def plan_and_execute(self, wait=True):
"""
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
return self.send_goal(MoveGoal.PLAN_AND_EXECUTE, wait)
def check_reachability(self, wait=True):
"""
Not implemented
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
return self.send_goal(MoveGoal.CHECK_REACHABILITY, wait)
def plan(self, wait=True):
"""
Plans, but doesn't execute the goal. Useful, if you just want to look at the planning ghost.
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
return self.send_goal(MoveGoal.PLAN_ONLY, wait)
def send_goal(self, goal_type, wait=True):
goal = self._get_goal()
goal.type = goal_type
if wait:
self._client.send_goal_and_wait(goal)
return self._client.get_result()
else:
self._client.send_goal(goal)
def get_collision_entries(self):
return self.cmd_seq
def _get_goal(self):
goal = MoveGoal()
goal.cmd_seq = self.cmd_seq
goal.type = MoveGoal.PLAN_AND_EXECUTE
self.clear_cmds()
return goal
def interrupt(self):
"""
Stops any goal that Giskard is processing.
"""
self._client.cancel_goal()
def get_result(self, timeout=rospy.Duration()):
"""
Waits for giskardpy result and returns it. Only used when plan_and_execute was called with wait=False
:type timeout: rospy.Duration
:rtype: MoveResult
"""
self._client.wait_for_result(timeout)
return self._client.get_result()
def clear_world(self):
"""
Removes any objects and attached objects from Giskard's world and reverts the robots urdf to what it got from
the parameter server.
:rtype: UpdateWorldResponse
"""
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE_ALL, WorldBody(),
False, PoseStamped())
return self._update_world_srv.call(req)
def remove_object(self, name):
"""
:param name:
:type name: str
:return:
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.name = str(name)
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE, object, False,
PoseStamped())
return self._update_world_srv.call(req)
def add_box(self,
name=u'box',
size=(1, 1, 1),
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param size: (x length, y length, z length) in m
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose)
return self._update_world_srv.call(req)
def add_sphere(self,
name=u'sphere',
size=1,
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param size: radius in m
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.SPHERE
object.shape.dimensions.append(size)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self._update_world_srv.call(req)
def add_mesh(self,
name=u'mesh',
mesh=u'',
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param mesh: path to the meshes location. e.g. package://giskardpy/test/urdfs/meshes/bowl_21.obj
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.type = WorldBody.MESH_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.mesh = mesh
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self._update_world_srv.call(req)
def add_cylinder(self,
name=u'cylinder',
height=1,
radius=1,
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param height: in m
:type height: float
:param radius: in m
:type radius: float
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.CYLINDER
object.shape.dimensions = [0, 0]
object.shape.dimensions[SolidPrimitive.CYLINDER_HEIGHT] = height
object.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS] = radius
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self._update_world_srv.call(req)
def attach_box(self,
name=u'box',
size=None,
frame_id=None,
position=None,
orientation=None,
pose=None):
"""
Add a box to the world and attach it to the robot at frame_id.
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:param pose: pose of the box
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(
frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(
*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(
*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose)
return self._update_world_srv.call(req)
def attach_cylinder(self,
name=u'cylinder',
height=1,
radius=1,
frame_id=None,
position=None,
orientation=None):
"""
Add a cylinder to the world and attach it to the robot at frame_id.
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:type height: int
:param height: height of the cylinder. Default = 1
:type radius: int
:param radius: radius of the cylinder. Default = 1
:type frame_id: str
:type position: list
:type orientation: list
:rtype: UpdateWorldResponse
"""
cylinder = make_world_body_cylinder(name, height, radius)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(
frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(
*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(
*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, cylinder, True, pose)
return self._update_world_srv.call(req)
def attach_object(self, name, link_frame_id):
"""
Attach an already existing object at link_frame_id of the robot.
:type name: str
:param link_frame_id: name of a robot link
:type link_frame_id: str
:return: UpdateWorldResponse
"""
req = UpdateWorldRequest()
req.rigidly_attached = True
req.body.name = name
req.pose.header.frame_id = link_frame_id
req.operation = UpdateWorldRequest.ADD
return self._update_world_srv.call(req)
def detach_object(self, object_name):
"""
Detach an object from the robot and add it back to the world.
Careful though, you could amputate an arm be accident!
:type object_name: str
:return: UpdateWorldResponse
"""
req = UpdateWorldRequest()
req.body.name = object_name
req.operation = req.DETACH
return self._update_world_srv.call(req)
def add_urdf(self, name, urdf, pose, js_topic=u'', set_js_topic=None):
"""
Adds a urdf to the world
:param name: name it will have in the world
:type name: str
:param urdf: urdf as string, no path
:type urdf: str
:type pose: PoseStamped
:param js_topic: Giskard will listen on that topic for joint states and update the urdf accordingly
:type js_topic: str
:param set_js_topic: A topic that the python wrapper will use to set the urdf joint state.
If None, set_js_topic == js_topic
:type set_js_topic: str
:return: UpdateWorldResponse
"""
if set_js_topic is None:
set_js_topic = js_topic
urdf_body = WorldBody()
urdf_body.name = str(name)
urdf_body.type = WorldBody.URDF_BODY
urdf_body.urdf = str(urdf)
urdf_body.joint_state_topic = str(js_topic)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, urdf_body, False,
pose)
if js_topic:
# FIXME publisher has to be removed, when object gets deleted
# FIXME there could be sync error, if objects get added/removed by something else
self._object_js_topics[name] = rospy.Publisher(set_js_topic,
JointState,
queue_size=10)
return self._update_world_srv.call(req)
def set_object_joint_state(self, object_name, joint_states):
"""
:type object_name: str
:param joint_states: joint state message or a dict that maps joint name to position
:type joint_states: Union[JointState, dict]
:return: UpdateWorldResponse
"""
if isinstance(joint_states, dict):
joint_states = position_dict_to_joint_states(joint_states)
self._object_js_topics[object_name].publish(joint_states)
def get_object_names(self):
"""
returns the names of every object in the world
:rtype: GetObjectNamesResponse
"""
return self._get_object_names_srv()
def get_object_info(self, name):
"""
returns the joint state, joint state topic and pose of the object with the given name
:type name: str
:rtype: GetObjectInfoResponse
"""
return self._get_object_info_srv(name)
def update_rviz_markers(self, object_names):
"""
republishes visualization markers for rviz
:type object_names: list
:rtype: UpdateRvizMarkersResponse
"""
return self._update_rviz_markers_srv(object_names)
def get_attached_objects(self):
"""
returns a list of all objects that are attached to the robot and the respective attachement points
:rtype: GetAttachedObjectsResponse
"""
return self._get_attached_objects_srv()
class Dance:
"""Move Katana arm around in a given fashion"""
def __init__(self):
self.poses= [
[-0.07, 0.21, -0.14, -1.03, -2.96, 0.27, 0.27],
[-1.35, 0.15, -0.25, -0.89, -2.96, 0.27, 0.27],
[-1.3, 0.96, -0.09, 0.33, -2.96, 0.27, 0.27],
[-0.07, 0.92, 0.1, 0.16, -2.96, 0.27, 0.27],
[1.13, 0.93, 0.02, 0.16, -2.96, 0.27, 0.27],
[1.64, 0.05, 0.26, -0.19, -2.96, 0.27, 0.27],
# ... what a nice dance ;)
]
self.i= -1
self.hopping= False
self.client= SimpleActionClient('katana_arm_controller/joint_movement_action', JointMovementAction)
self.client.wait_for_server()
def hop(self, pose= None, noreset= False):
"""Hop to next stance of the dance
pose - JointState, optional. If given, this pose will be taken instead of the next one
noreset - Boolean, optional. If True, don't reset the PoseBuffer during movement
"""
if pose == None:
if self.i+1 >= len(self.poses):
self.i= -1
raise LastHopReachedException()
self.i= (self.i+1) % len(self.poses)
goal= JointMovementGoal( jointGoal= JointState(name=JOINTS, position=self.poses[self.i]) )
else:
goal= JointMovementGoal(jointGoal= pose)
self.hopping= True
if not noreset:
transform.reset()
self.client.send_goal(goal)
self.client.wait_for_result()
self.hopping= False
if not noreset:
transform.reset()
return self.client.get_result()
def setup(self):
"""Get as safe as possible to the first stance"""
try:
jointmsg= rospy.wait_for_message('/katana_joint_states', JointState, 2.0)
self.old_pose= []
for n in JOINTS:
self.old_pose.append(jointmsg.position[jointmsg.name.index(n)])
except Exception, e:
raise NoJointStatesFoundException(e)
self.hop(JointState(
name= ['katana_motor3_lift_joint', 'katana_motor4_lift_joint', 'katana_motor5_wrist_roll_joint', 'katana_r_finger_joint', 'katana_l_finger_joint'],
position= [-2.18, -2.02, -2.96, 0.28, 0.28]
), noreset= True)
self.hop( JointState(name= ['katana_motor2_lift_joint'], position= [2.16]), noreset= True )
self.hop( JointState(name= ['katana_motor1_pan_joint'], position= [0.00]), noreset= True )
self.hop(noreset= True)
class PathExecutor:
goal_index = 0
reached_all_nodes = True
def __init__(self):
rospy.loginfo('__init__ start')
# create a node
rospy.init_node(NODE)
# Action server to receive goals
self.path_server = SimpleActionServer('/path_executor/execute_path', ExecutePathAction,
self.handle_path, auto_start=False)
self.path_server.start()
# publishers & clients
self.visualization_publisher = rospy.Publisher('/path_executor/current_path', Path)
# get parameters from launch file
self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True)
# action server based on use_obstacle_avoidance
if self.use_obstacle_avoidance == False:
self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction)
else:
self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction)
self.goal_client.wait_for_server()
# other fields
self.goal_index = 0
self.executePathGoal = None
self.executePathResult = ExecutePathResult()
def handle_path(self, paramExecutePathGoal):
'''
Action server callback to handle following path in succession
'''
rospy.loginfo('handle_path')
self.goal_index = 0
self.executePathGoal = paramExecutePathGoal
self.executePathResult = ExecutePathResult()
if self.executePathGoal is not None:
self.visualization_publisher.publish(self.executePathGoal.path)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
if not self.path_server.is_active():
return
if self.path_server.is_preempt_requested():
rospy.loginfo('Preempt requested...')
# Stop bug2
self.goal_client.cancel_goal()
# Stop path_server
self.path_server.set_preempted()
return
if self.goal_index < len(self.executePathGoal.path.poses):
moveto_goal = MoveToGoal()
moveto_goal.target_pose = self.executePathGoal.path.poses[self.goal_index]
self.goal_client.send_goal(moveto_goal, done_cb=self.handle_goal,
feedback_cb=self.handle_goal_preempt)
# Wait for result
while self.goal_client.get_result() is None:
if self.path_server.is_preempt_requested():
self.goal_client.cancel_goal()
else:
rospy.loginfo('Done processing goals')
self.goal_client.cancel_goal()
self.path_server.set_succeeded(self.executePathResult, 'Done processing goals')
return
rate.sleep()
self.path_server.set_aborted(self.executePathResult, 'Aborted. The node has been killed.')
def handle_goal(self, state, result):
'''
Handle goal events (succeeded, preempted, aborted, unreachable, ...)
Send feedback message
'''
feedback = ExecutePathFeedback()
feedback.pose = self.executePathGoal.path.poses[self.goal_index]
# state is GoalStatus message as shown here:
# http://docs.ros.org/diamondback/api/actionlib_msgs/html/msg/GoalStatus.html
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Succeeded finding goal %d", self.goal_index + 1)
self.executePathResult.visited.append(True)
feedback.reached = True
else:
rospy.loginfo("Failed finding goal %d", self.goal_index + 1)
self.executePathResult.visited.append(False)
feedback.reached = False
if not self.executePathGoal.skip_unreachable:
rospy.loginfo('Failed finding goal %d, not skipping...', self.goal_index + 1)
# Stop bug2
self.goal_client.cancel_goal()
# Stop path_server
self.path_server.set_succeeded(self.executePathResult,
'Unreachable goal in path. Done processing goals.')
#self.path_server.set_preempted()
#return
self.path_server.publish_feedback(feedback)
self.goal_index = self.goal_index + 1
def handle_goal_preempt(self, state):
'''
Callback for goal_client to check for preemption from path_server
'''
if self.path_server.is_preempt_requested():
self.goal_client.cancel_goal()
class Communication(object):
def __init__(self):
self.read_registers_ac = None
self.write_registers_ac = None
self.modbus_action_lock = Lock()
def connect_to_device(self):
self.read_registers_ac = SimpleActionClient('/teachmate_comms/read_registers', ReadRegistersAction)
read_connected = self.read_registers_ac.wait_for_server(rospy.Duration(3.0))
self.write_registers_ac = SimpleActionClient('/teachmate_comms/write_registers', WriteRegistersAction)
write_connected = self.write_registers_ac.wait_for_server(rospy.Duration(3.0))
return read_connected and write_connected
def disconnect_from_device(self):
""" Close connection """
self.__log.err('Disconnecting Robotiq')
self.stop_action_client(self.read_registers_ac)
self.read_registers_ac = None
self.stop_action_client(self.write_registers_ac)
self.write_registers_ac = None
@staticmethod
def stop_action_client(ac):
ac.cancel_all_goals()
ac.action_client.pub_goal.unregister()
ac.action_client.pub_cancel.unregister()
def send_command(self, data):
"""
Send a command to the Gripper - the method takes a list of uint8 as an argument.
The meaning of each variable depends on the Gripper model (see support.robotiq.com for more details)
"""
# make sure data has an even number of elements
if len(data) % 2 == 1:
data.append(0)
# Initiate message as an empty list
message = []
# Fill message by combining two bytes in one register
for i in range(0, len(data) / 2):
message.append((data[2 * i] << 8) + data[2 * i + 1])
if not self.write_registers_ac.wait_for_server(rospy.Duration(3.0)):
self.__log.err('Teachmate Modbus Communications could not be contacted!')
return False
request = WriteRegistersGoal()
request.slave_id = ROBOTIQ_ID
request.first_register = 0x03E8
request.values = message
with self.modbus_action_lock:
self.write_registers_ac.send_goal(request)
if not self.write_registers_ac.wait_for_result(rospy.Duration(3.0)):
self.__log.err('Timed Out While Sending Command')
return False
resp = self.write_registers_ac.get_result()
if resp.result != resp.SUCCESS:
self.__log.warn('Error Writing Registers')
return False
return True
def get_status(self, num_bytes):
"""
Sends a request to read, wait for the response and returns the Gripper status.
The method gets the number of bytes to read as an argument
"""
num_regs = int(ceil(num_bytes / 2.0))
if not self.read_registers_ac.wait_for_server(rospy.Duration(3.0)):
self.__log.err('Teachmate Modbus Communications could not be contacted!')
return None
if self.read_registers_ac.get_state == GoalStatus.ACTIVE:
self.__log.warn('Previous goal is stick executing: skipping request')
return None
request = ReadRegistersGoal()
request.slave_id = ROBOTIQ_ID
request.first_register = 0x07D0
request.num_registers = num_regs
with self.modbus_action_lock:
self.read_registers_ac.send_goal(request)
if not self.read_registers_ac.wait_for_result(rospy.Duration(3.0)):
self.__log.err('Timed Out on Response')
return None
resp = self.read_registers_ac.get_result()
if resp.result != resp.SUCCESS:
self.__log.warn('Error Reading Registers')
return None
# Instantiate output as an empty list
output = []
# Fill the output with the bytes in the appropriate order
for i in xrange(len(resp.values)):
output.append((resp.values[i] & 0xFF00) >> 8)
output.append(resp.values[i] & 0x00FF)
# Output the result
return output
class Pr2LookAtFace(Action):
def __init__(self):
super(Pr2LookAtFace, self).__init__()
self._client = SimpleActionClient('face_detector_action',FaceDetectorAction)
self._head_client = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)
self._timer = None
self._executing = False
self._pending_face_goal = False
self._pending_head_goal = False
def set_duration(self, duration):
duration = max(duration, 1.5)
super(Pr2LookAtFace, self).set_duration(duration)
def to_string(self):
return 'look_at_face()'
def execute(self):
super(Pr2LookAtFace, self).execute()
print('Pr2LookAtFace.execute() %d' % self.get_duration())
# delay execution to not run nested in the current stack context
self._timer = rospy.Timer(rospy.Duration.from_sec(0.001), self._execute, oneshot=True)
def _execute(self, event):
self._executing = True
self._timer = rospy.Timer(rospy.Duration.from_sec(self.get_duration()), self._preempt, oneshot=True)
hgoal = None
connected = self._client.wait_for_server(rospy.Duration(1.0))
if connected:
while self._executing:
fgoal = FaceDetectorGoal()
self._pending_face_goal = True
self._client.send_goal(fgoal)
self._client.wait_for_result()
self._pending_face_goal = False
f = self._client.get_result()
if len(f.face_positions) > 0:
closest = -1
closest_dist = 1000
for i in range(len(f.face_positions)):
dist = f.face_positions[i].pos.x*f.face_positions[i].pos.x + f.face_positions[i].pos.y*f.face_positions[i].pos.y\
+ f.face_positions[i].pos.z*f.face_positions[i].pos.z
if dist < closest_dist:
closest = i
closest_dist = dist
if closest > -1:
hgoal = PointHeadGoal()
hgoal.target.header.frame_id = f.face_positions[closest].header.frame_id
hgoal.target.point.x = f.face_positions[closest].pos.x
hgoal.target.point.y = f.face_positions[closest].pos.y
hgoal.target.point.z = f.face_positions[closest].pos.z
hgoal.min_duration = rospy.Duration(1.0)
if self._executing:
hconnected = self._head_client.wait_for_server(rospy.Duration(1.0))
if hconnected and self._executing:
self._pending_head_goal = True
self._head_client.send_goal(hgoal)
# self._head_client.wait_for_result()
# self._pending_head_goal = False
else:
hgoal = PointHeadGoal()
hgoal.target.header.frame_id = "base_footprint";
hgoal.target.point.x = 2.0
hgoal.target.point.y = -2.0
hgoal.target.point.z = 1.2
hgoal.min_duration = rospy.Duration(1.0)
if self._executing:
hconnected = self._head_client.wait_for_server(rospy.Duration(1.0))
if hconnected and self._executing:
self._pending_head_goal = True
self._head_client.send_goal(hgoal)
self._head_client.wait_for_result()
self._pending_head_goal = False
if self._executing:
time.sleep(1.0)
self._finished_finding_face()
def _preempt(self, event):
self._executing = False
if self._pending_face_goal:
self._client.cancel_goal()
if self._pending_head_goal:
self._head_client.cancel_goal()
self._execute_finished()
def _finished_finding_face(self):
if self._executing:
self._executing = False
self._timer.shutdown()
self._execute_finished()
class PigeonPickAndPlace:
#Class constructor (parecido com java, inicializa o que foi instanciado)
def __init__(self):
# Retrieve params:
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016)
self._arm_group = rospy.get_param('~arm', 'arm_move_group')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
# Create planning scene where we will add the objects etc.
self._scene = PlanningSceneInterface()
# Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene (remove the old objects:
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
# TODO get the position of the detected object
self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name)
rospy.sleep(1.0)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Pick object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
def __del__(self):
# Clean the scene
self._scene.remove_world_object(self._grasp_object_name)
def _add_object_grasp(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame() #pose
p.header.stamp = rospy.Time.now()
rospy.loginfo(self._robot.get_planning_frame())
p.pose.position.x = 0.11 # 0.26599 # antigo = 0.75 - 0.01
p.pose.position.y = -0.31 # -0.030892 #antigo = 0.25 - 0.01
p.pose.position.z = 0.41339 #antigo = 1.00 + (0.3 + 0.03) / 2.0
#p.pose.orientation.x = 0.0028925
#p.pose.orientation.y = -0.0019311
#p.pose.orientation.z = -0.71058
#p.pose.orientation.w = 0.70361
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/,
# using the measure tape tool from meshlab.
# The box is the bounding box of the lego cylinder.
# The values are taken from the cylinder base diameter and height.
# the size is length (x),thickness(y),height(z)
# I don't know if the detector return this values of object
self._scene.add_box(name, p, (0.032, 0.016, 0.020))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
#rospy.logerr('Generated: %f grasps:' % grasps.size)
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
#goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 5.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 10
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_object_grasp, 0.016)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
class Pick_Place:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
#
# self._pose_table = self._scene.get_object_poses(self._table_object_name)
# self._pose_coke_can = self._scene.get_object_poses(self._grasp_object_name)
# self.setup()
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
rospy.sleep(1.0)
self._scene.remove_world_object(self._grasp_object_name)
rospy.sleep(1.0)
# # Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
rospy.sleep(2.0)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
rospy.sleep(2.0)
# rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y - 0.06
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
# while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
# rospy.logwarn('Place failed! Retrying ...')
# rospy.sleep(1.0)
#
# rospy.loginfo('Place successfully')
def setup():
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_attached_object()
def _add_table(self, name):
rospy.loginfo("entered table")
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.2
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (9 , 9, 0.02))
return p.pose
def _add_grasp_block_(self, name):
rospy.loginfo("entered box grabber")
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.05
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.1, 0.1, 0.1))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle )
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame()
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = 0.2
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame()
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 0.2
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
def _add_objects(self):
"""
Here add all the objects that you want to add to the _scene
"""
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
class Api(object):
def __init__(self, name):
'''
Wrap the actionlib interface with the API
'''
self._client = SimpleActionClient(name, QueryAction)
rospy.loginfo('waiting for "%s" server', name)
self._client.wait_for_server()
self._feedback = False
self.last_talker_id = ""
def _send_query(self, description, spec, choices):
goal = queryToROS(description, spec, choices)
state = self._client.send_goal(goal, feedback_cb=self._feedback_callback)
def _feedback_callback(self, feedback):
rospy.loginfo("Received feedback")
self._feedback = True
def _wait_for_result_and_get(self, timeout=None):
execute_timeout = rospy.Duration(timeout) if timeout else rospy.Duration(10)
preempt_timeout = rospy.Duration(1)
while not self._client.wait_for_result(execute_timeout):
if not self._feedback:
# preempt action
rospy.logdebug("Canceling goal")
self._client.cancel_goal()
if self._client.wait_for_result(preempt_timeout):
rospy.loginfo("Preempt finished within specified preempt_timeout [%.2f]", preempt_timeout.to_sec());
else:
rospy.logwarn("Preempt didn't finish specified preempt_timeout [%.2f]", preempt_timeout.to_sec());
break
else:
self._feedback = False
rospy.loginfo("I received feedback, let's wait another %.2f seconds" % execute_timeout.to_sec())
state = self._client.get_state()
if state != GoalStatus.SUCCEEDED:
if state == GoalStatus.PREEMPTED:
# Timeout
_print_timeout()
raise TimeoutException("Goal did not succeed within the time limit")
else:
_print_generic_failure()
raise Exception("Goal did not succeed, it was: %s" % GoalStatus.to_string(state))
return self._client.get_result()
def query(self, description, spec, choices, timeout=10):
'''
Perform a HMI query, returns a dict of {choicename: value}
'''
rospy.loginfo('Question: %s, spec: %s', description, _truncate(spec))
_print_example(spec, choices)
self._send_query(description, spec, choices)
answer = self._wait_for_result_and_get(timeout=timeout)
self.last_talker_id = answer.talker_id # Keep track of the last talker_id
_print_answer(answer)
return resultFromROS(answer)
def query_raw(self, description, spec, timeout=10):
'''
Perform a HMI query without choices, returns a string
'''
rospy.loginfo('Question: %s, spec: %s', description, _truncate(spec))
_print_example(spec, {})
self._send_query(description, spec, {})
answer = self._wait_for_result_and_get(timeout=timeout)
self.last_talker_id = answer.talker_id # Keep track of the last talker_id
_print_answer(answer)
return answer.raw_result
def old_query(self, spec, choices, timeout=10):
'''
Convert old queryies to a HMI query
'''
rospy.loginfo('spec: %s', _truncate(spec))
_print_example(spec, choices)
self._send_query('', spec, choices)
try:
answer = self._wait_for_result_and_get(timeout=timeout)
except TimeoutException:
return GetSpeechResponse(result="")
except:
return None
else:
# so we've got an answer
self.last_talker_id = answer.talker_id # Keep track of the last talker_id
_print_answer(answer)
# convert it to the old message
choices = resultFromROS(answer)
result = GetSpeechResponse(result=answer.raw_result)
result.choices = choices
return result
def set_description(self, description):
pass
def set_grammar(self, spec):
pass
def wait_for_grammar_set(self, spec):
pass
