def test_no_look(self):
# parameters
x_coordinate = 2.1
y_coordinate = 3.7
z_coordinate = 0
frame_id = "/map"
robot = Mockbot()
arm = ArmDesignator(robot, {}, name="arm_designator")
frame = Designator(frame_stamped(frame_id, x_coordinate, y_coordinate, z_coordinate), name="frame_designator")
pc, oc = arms_reach_constraint(frame, arm, look=False)
# verify positionconstraint
self.assertEqual(pc.frame, frame_id)
publicarm = arm.resolve()
radius = math.hypot(publicarm.base_offset.x(), publicarm.base_offset.y())
margin = 0.075
ro = "(x-{})^2+(y-{})^2 < {}^2".format(x_coordinate, y_coordinate, radius + margin)
ri = "(x-{})^2+(y-{})^2 > {}^2".format(x_coordinate, y_coordinate, radius - margin)
verification_string = ri + " and " + ro
equal, msg = constraint_strings_equal(pc.constraint, verification_string)
self.assertTrue(equal, msg)
# verify orientationconstraint
self.assertIsNone(oc)
def _grab_handle(ud):
robot.rightArm.wait_for_motion_done()
robot.speech.speak('I hope this goes right!', block=False)
fs = frame_stamped("dishwasher", 0.42, 0, 0.8, roll=math.pi / 2, pitch=0, yaw=math.pi)
robot.rightArm.send_goal(fs.projectToFrame(robot.robot_name + "/base_link", robot.tf_listener))
robot.rightArm.send_gripper_goal("close")
return 'done'
def execute(self, userdata=None):
""" Does the actual work
:param userdata:
:return:
"""
self._robot.head.reset()
rospy.sleep(1)
self._robot.speech.speak("I'm waiting for a waving person")
waving_persons = []
while not rospy.is_shutdown() and not waving_persons:
rospy.sleep(1 / self.rate)
for person in self.people_received.people:
if {'RWave', 'LWave'}.intersection(set(person.tags)):
waving_persons.append(person)
if not waving_persons:
return 'aborted'
rospy.loginfo('waving persons: %s', waving_persons)
if len(waving_persons) > 1:
rospy.logwarn('using the first person')
header = self.people_received.header
point = waving_persons[0].position
pose = frame_stamped(header.frame_id, point.x, point.y, point.z)
rospy.loginfo('update customer position to %s', pose)
self._robot.ed.update_entity(id=self._caller_id,
frame_stamped=pose,
type="waypoint")
# customer_point_msg_stamped_camera = PointStamped()
# customer_point_msg_stamped_camera = self.people_received.header
# customer_point_msg_stamped_camera = waving_persons[0].position
# self.robot.tf_listener.
# look at the barman
kitchen_entity = self._kitchen_designator.resolve()
target_pose = kitchen_entity._pose
head_target_kdl = target_pose * kdl.Vector(20.0, 0.0, 0.0)
head_target = VectorStamped(x=head_target_kdl.x(),
y=head_target_kdl.y(),
z=head_target_kdl.z(),
frame_id="/map")
# pose = kitchen_entity.pose.extractVectorStamped()
# pose.vector[2] = 1.5
# self._robot.head.look_at_point(head_target)
self._robot.speech.speak(
"I have seen a waving person, should I continue?")
if self._confirm():
self._robot.head.cancel_goal()
return 'succeeded'
else:
self._robot.head.cancel_goal()
return 'rejected'
def execute(self, userdata):
self._robot.speech.speak("I'm waiting for a customer")
rospy.loginfo("You can click in rviz")
rate = rospy.Rate(self.rate)
self._point = False
while not rospy.is_shutdown() and not self._point:
rate.sleep()
if not self._point:
return 'aborted'
# TODO, get data from point into ED
pose = frame_stamped("map", self._point.point.x, self._point.point.y, 0.0)
self._robot.ed.update_entity(id=self._caller_id, frame_stamped=pose, type="waypoint")
return 'succeeded'
def execute(self, userdata):
self._robot.speech.speak("I'm waiting for a customer")
rospy.loginfo("You can click in rviz")
rate = rospy.Rate(self.rate)
self._point = False
while not rospy.is_shutdown() and not self._point:
rate.sleep()
if not self._point:
return 'aborted'
# TODO, get data from point into ED
pose = frame_stamped("map", self._point.point.x, self._point.point.y,
0.0)
self._robot.ed.update_entity(id="customer",
frame_stamped=pose,
type="waypoint")
return 'succeeded'
def execute(self, userdata=None):
""" Does the actual work
:param userdata:
:return:
"""
self._robot.head.reset()
rospy.sleep(1)
while True:
self._robot.speech.speak("I'm looking for waving persons")
persons = self._robot.head.detect_waving_persons()
if not persons:
continue
self._robot.speech.speak("I found a waving person")
person = persons[0]
try:
point = self._robot.head.project_roi(person.roi,
frame_id="map")
break
except ValueError as e:
rospy.logerr('project failed: %s', e)
pose = frame_stamped("map", point.vector.x(), point.vector.y(), 0.0)
self._robot.speech.speak(
"I have seen a waving person, should I continue?")
if self._confirm():
self._robot.ed.update_entity(id="customer",
frame_stamped=pose,
type="waypoint")
return 'succeeded'
else:
return 'rejected'
def execute(self, userdata=None):
""" Does the actual work
:param userdata:
:return:
"""
self._robot.head.reset()
self._robot.head.wait_for_motion_done()
self._robot.speech.speak("I'm waiting for a waving person")
head_samples = 20
look_distance = 3.0
look_angles = [0,
0,
0,
10,
-10,
20,
-20]
self.clear_queue()
waving_persons = []
i = 0
while not rospy.is_shutdown() and not waving_persons:
header, waving_persons = self.wait_for_waving_person(head_samples=head_samples)
angle = look_angles[i % len(look_angles)]
rospy.loginfo('Still waiting... looking at %d degrees', angle)
angle = math.radians(angle)
head_goal = VectorStamped(x=look_distance * math.cos(angle),
y=look_distance * math.sin(angle), z=1.3,
frame_id="/%s/base_link" % self._robot.robot_name)
self._robot.head.look_at_point(head_goal)
i += 1
if not waving_persons:
return 'aborted'
rospy.loginfo('waving persons: %s', waving_persons)
if len(waving_persons) > 1:
rospy.logwarn('using the first person')
point = waving_persons[0].position
pose = frame_stamped(header.frame_id, point.x, point.y, point.z)
rospy.loginfo('update customer position to %s', pose)
self._robot.ed.update_entity(id=self._caller_id, frame_stamped=pose, type="waypoint")
# look at the barman
kitchen_entity = self._kitchen_designator.resolve()
target_pose = kitchen_entity._pose
head_target_kdl = target_pose * kdl.Vector(20.0, 0.0, 0.0)
head_target = VectorStamped(x=head_target_kdl.x(), y=head_target_kdl.y(), z=head_target_kdl.z(),
frame_id="/map")
# pose = kitchen_entity.pose.extractVectorStamped()
# pose.vector[2] = 1.5
self._robot.head.look_at_point(head_target)
self._robot.head.wait_for_motion_done()
return 'succeeded'
Make sure you have a (robot)-start running since a robot object is created for this.
"""
parser = argparse.ArgumentParser(
description="Example arms_reach_constaint() function")
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
rospy.init_node("example_arms_reach_constraints")
robot = get_robot(args.robot)
# get an armdesignator and a pose designator
arm_designator = ArmDesignator(robot, {}, name="arm_designator")
pose = frame_stamped("/map", 0, 0, 1.3)
pose_designator = Designator(pose, name="frame designator")
# basic functionality
rospy.loginfo("Example of the basic funtionality")
rospy.loginfo(
"Running: pc, oc = arms_reach_constraint(pose_designator, arm_designator)"
)
pc, oc = arms_reach_constraint(pose_designator, arm_designator)
rospy.loginfo("pc becomes : {}".format(pc))
rospy.loginfo("oc becomes : {}".format(oc))
# dont look at the frame
rospy.loginfo(
"Example where we don't require the robot to look at the pose")
rospy.loginfo(
def execute(self, userdata=None):
""" Does the actual work
:param userdata:
:return:
"""
self._robot.head.reset()
self._robot.head.wait_for_motion_done()
self._robot.speech.speak("I'm waiting for a waving person")
head_samples = 20
look_distance = 3.0
look_angles = [0, 0, 0, 10, -10, 20, -20]
self.clear_queue()
waving_persons = []
i = 0
while not rospy.is_shutdown() and not waving_persons:
header, waving_persons = self.wait_for_waving_person(
head_samples=head_samples)
angle = look_angles[i % len(look_angles)]
rospy.loginfo('Still waiting... looking at %d degrees', angle)
angle = math.radians(angle)
head_goal = VectorStamped(x=look_distance * math.cos(angle),
y=look_distance * math.sin(angle),
z=1.3,
frame_id="/%s/base_link" %
self._robot.robot_name)
self._robot.head.look_at_point(head_goal)
i += 1
if not waving_persons:
return 'aborted'
rospy.loginfo('waving persons: %s', waving_persons)
if len(waving_persons) > 1:
rospy.logwarn('using the first person')
point = waving_persons[0].position
pose = frame_stamped(header.frame_id, point.x, point.y, point.z)
rospy.loginfo('update customer position to %s', pose)
self._robot.ed.update_entity(id=self._caller_id,
frame_stamped=pose,
type="waypoint")
# look at the barman
kitchen_entity = self._kitchen_designator.resolve()
target_pose = kitchen_entity._pose
head_target_kdl = target_pose * kdl.Vector(20.0, 0.0, 0.0)
head_target = VectorStamped(x=head_target_kdl.x(),
y=head_target_kdl.y(),
z=head_target_kdl.z(),
frame_id="/map")
# pose = kitchen_entity.pose.extractVectorStamped()
# pose.vector[2] = 1.5
self._robot.head.look_at_point(head_target)
self._robot.head.wait_for_motion_done()
return 'succeeded'
def execute(self, ud):
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
# Make sure the torso and the arm are done
self.robot.torso.wait_for_motion_done(cancel=True)
arm.wait_for_motion_done(cancel=True)
# This is needed because the head is not entirely still when the
# look_at_point function finishes
rospy.sleep(rospy.Duration(0.5))
# Take the last available, not at detection timestamp
ud.position.header.stamp = rospy.Time(0)
goal_point_base_link = self.robot.tf_listener.transformPoint('/amigo/base_link', ud.position)
x = goal_point_base_link.point.x
y = goal_point_base_link.point.y
z = goal_point_base_link.point.z
goal_bl = frame_stamped(goal_point_base_link.header.frame_id, x, y, z, roll=0, pitch=0, yaw=-0.0463)
# Grasp
rospy.loginfo('Start grasping')
if not arm.send_goal(goal_bl, timeout=20, pre_grasp=True):
self.robot.speech.speak('I am sorry but I cannot move my arm to the object position', block=False)
rospy.logerr('Grasp failed')
arm.reset()
arm.send_gripper_goal('close', timeout=0.0)
return 'failed'
# Close gripper
arm.send_gripper_goal('close')
arm.occupied_by = None # TODO: what to do if we don't have an entity
# Lift
rospy.loginfo('Start lifting')
if arm.side == "left":
roll = 0.3
else:
roll = -0.3
goal_bl.frame.p.z(goal_bl.frame.p.z() + 0.05) # Add 5 cm
goal_bl.frame.M = kdl.Rotation.RPY(roll, 0, 0) # Update the roll
rospy.loginfo("Start lift")
if not arm.send_goal(goal_bl, timeout=20):
rospy.logerr('Failed lift')
# Retract
rospy.loginfo('Start retracting')
if arm.side == "left":
roll = 0.6
else:
roll = -0.6
goal_bl.frame.p.x(goal_bl.frame.p.x() - 0.1) # Retract 10 cm
goal_bl.frame.p.z(goal_bl.frame.p.z() + 0.05) # Go 5 cm higher
goal_bl.frame.M = kdl.Rotation.RPY(roll, 0.0, 0.0) # Update the roll
rospy.loginfo("Start retract")
if not arm.send_goal(goal_bl, timeout=0.0):
rospy.logerr('Failed retract')
self.robot.base.force_drive(-0.125, 0, 0, 2.0)
arm.wait_for_motion_done(cancel=True)
# Carrying pose
rospy.loginfo('start moving to carrying pose')
arm.send_joint_goal('carrying_pose', timeout=0.0)
# Reset head
self.robot.head.cancel_goal()
return 'succeeded'
def execute(self, ud):
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
# Make sure the torso and the arm are done
self.robot.torso.wait_for_motion_done(cancel=True)
arm.wait_for_motion_done(cancel=True)
# This is needed because the head is not entirely still when the
# look_at_point function finishes
rospy.sleep(rospy.Duration(0.5))
# Take the last available, not at detection timestamp
ud.position.header.stamp = rospy.Time(0)
goal_point_base_link = self.robot.tf_listener.transformPoint(
'/amigo/base_link', ud.position)
x = goal_point_base_link.point.x
y = goal_point_base_link.point.y
z = goal_point_base_link.point.z
goal_bl = frame_stamped(goal_point_base_link.header.frame_id,
x,
y,
z,
roll=0,
pitch=0,
yaw=-0.0463)
# Grasp
rospy.loginfo('Start grasping')
if not arm.send_goal(goal_bl, timeout=20, pre_grasp=True):
self.robot.speech.speak(
'I am sorry but I cannot move my arm to the object position',
block=False)
rospy.logerr('Grasp failed')
arm.reset()
arm.send_gripper_goal('close', timeout=0.0)
return 'failed'
# Close gripper
arm.send_gripper_goal('close')
arm.occupied_by = None # TODO: what to do if we don't have an entity
# Lift
rospy.loginfo('Start lifting')
if arm.side == "left":
roll = 0.3
else:
roll = -0.3
goal_bl.frame.p.z(goal_bl.frame.p.z() + 0.05) # Add 5 cm
goal_bl.frame.M = kdl.Rotation.RPY(roll, 0, 0) # Update the roll
rospy.loginfo("Start lift")
if not arm.send_goal(goal_bl, timeout=20):
rospy.logerr('Failed lift')
# Retract
rospy.loginfo('Start retracting')
if arm.side == "left":
roll = 0.6
else:
roll = -0.6
goal_bl.frame.p.x(goal_bl.frame.p.x() - 0.1) # Retract 10 cm
goal_bl.frame.p.z(goal_bl.frame.p.z() + 0.05) # Go 5 cm higher
goal_bl.frame.M = kdl.Rotation.RPY(roll, 0.0, 0.0) # Update the roll
rospy.loginfo("Start retract")
if not arm.send_goal(goal_bl, timeout=0.0):
rospy.logerr('Failed retract')
self.robot.base.force_drive(-0.125, 0, 0, 2.0)
arm.wait_for_motion_done(cancel=True)
# Carrying pose
rospy.loginfo('start moving to carrying pose')
arm.send_joint_goal('carrying_pose', timeout=0.0)
# Reset head
self.robot.head.cancel_goal()
return 'succeeded'