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'