def test_delivery(robot): from robot_skills.util.kdl_conversions import kdl_frame_stamped_from_XYZRPY robot.ed.update_entity(id="one", kdl_frame_stamped=kdl_frame_stamped_from_XYZRPY( x=1.0, y=0, frame_id="/map"), type="waypoint") robot.ed.update_entity(id="two", kdl_frame_stamped=kdl_frame_stamped_from_XYZRPY( x=-1.2, y=0.0, frame_id="/map"), type="waypoint") robot.ed.update_entity(id="three", kdl_frame_stamped=kdl_frame_stamped_from_XYZRPY( x=1.950, y=1.551, frame_id="/map"), type="waypoint") global ORDERS ORDERS = { "beverage": { "name": "coke", "location": "one" }, "combo": { "name": "pringles and chocolate", "location": "two" } } deliver = smach.StateMachine(outcomes=['done', 'aborted']) with deliver: smach.StateMachine.add('DELIVER_BEVERAGE', DeliverOrderWithBasket(robot, "beverage"), transitions={ 'succeeded': 'NAVIGATE_TO_KITCHEN', 'failed': 'NAVIGATE_TO_KITCHEN' }) smach.StateMachine.add('NAVIGATE_TO_KITCHEN', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id="kitchen"), radius=WAYPOINT_RADIUS), transitions={ 'arrived': 'DELIVER_COMBO', 'unreachable': 'DELIVER_COMBO', 'goal_not_defined': 'DELIVER_COMBO' }) smach.StateMachine.add('DELIVER_COMBO', DeliverOrderWithBasket(robot, "combo"), transitions={ 'succeeded': 'done', 'failed': 'aborted' }) deliver.execute(None)
def determine_points_of_interest(self, center_frame, z_max, convex_hull): """ Determine candidates for place poses :param center_frame: kdl.Frame, center of the Entity to place on top of :param z_max: float, height of the entity to place on, w.r.t. the entity :param convex_hull: [kdl.Vector], convex hull of the entity :return: [FrameStamped] of candidates for placing """ points = [] if len(convex_hull) == 0: rospy.logerr('determine_points_of_interest: Empty convex hull') return [] # Convert convex hull to map frame ch = offsetConvexHull(convex_hull, center_frame) # Loop over hulls self.marker_array.markers = [] for i in xrange(len(ch)): j = (i + 1) % len(ch) dx = ch[j].x() - ch[i].x() dy = ch[j].y() - ch[i].y() length = kdl.diff(ch[j], ch[i]).Norm() d = self._edge_distance while d < (length - self._edge_distance): # Point on edge xs = ch[i].x() + d / length * dx ys = ch[i].y() + d / length * dy # Shift point inwards and fill message fs = kdl_frame_stamped_from_XYZRPY( x=xs - dy / length * self._edge_distance, y=ys + dx / length * self._edge_distance, z=center_frame.p.z() + z_max, frame_id="/map") # It's nice to put an object on the middle of a long edge. In case of a cabinet, e.g., this might # prevent the robot from hitting the cabinet edges # print "Length: {}, edge score: {}".format(length, min(d, length-d)) setattr(fs, 'edge_score', min(d, length - d)) points += [fs] self.marker_array.markers.append( self.create_marker(fs.frame.p.x(), fs.frame.p.y(), fs.frame.p.z())) # ToDo: check if still within hull??? d += self._spacing self.marker_pub.publish(self.marker_array) return points
def determine_points_of_interest(self, center_frame, z_max, convex_hull): """ Determine candidates for place poses :param center_frame: kdl.Frame, center of the Entity to place on top of :param z_max: float, height of the entity to place on, w.r.t. the entity :param convex_hull: [kdl.Vector], convex hull of the entity :return: [FrameStamped] of candidates for placing """ points = [] if len(convex_hull) == 0: rospy.logerr('determine_points_of_interest: Empty convex hull') return [] # Convert convex hull to map frame ch = offsetConvexHull(convex_hull, center_frame) # Loop over hulls self.marker_array.markers = [] for i in xrange(len(ch)): j = (i + 1) % len(ch) dx = ch[j].x() - ch[i].x() dy = ch[j].y() - ch[i].y() length = kdl.diff(ch[j], ch[i]).Norm() d = self._edge_distance while d < (length - self._edge_distance): # Point on edge xs = ch[i].x() + d / length * dx ys = ch[i].y() + d / length * dy # Shift point inwards and fill message fs = kdl_frame_stamped_from_XYZRPY(x=xs - dy / length * self._edge_distance, y=ys + dx / length * self._edge_distance, z=center_frame.p.z() + z_max, frame_id="/map") # It's nice to put an object on the middle of a long edge. In case of a cabinet, e.g., this might # prevent the robot from hitting the cabinet edges # print "Length: {}, edge score: {}".format(length, min(d, length-d)) setattr(fs, 'edge_score', min(d, length-d)) points += [fs] self.marker_array.markers.append(self.create_marker(fs.frame.p.x(), fs.frame.p.y(), fs.frame.p.z())) # ToDo: check if still within hull??? d += self._spacing self.marker_pub.publish(self.marker_array) return points
def determine_points_of_interest_with_area(self, e, area): """ Determines the points of interest using an area :param e: :param area: :return: """ # Just to be sure, copy e e = self.robot.ed.get_entity(id=e.id) # We want to give it a convex hull using the designated area if area in e.volumes: box = e.volumes[area] else: rospy.logwarn("Entity {0} has no volume named {1}".format( e.id, area)) if not self._candidate_list_obj: for y in [ 0, self._spacing, -self._spacing, 2.0 * self._spacing, -2.0 * self._spacing ]: if y < box.min_corner.y() or y > box.max_corner.y(): rospy.logerr( "Spacing of empty spot designator is too large!!!") continue frame_stamped = kdl_frame_stamped_from_XYZRPY( x=box.max_corner.x() - self._edge_distance, y=y, z=box.min_corner.z() - 0.04, # 0.04 is the usual offset) frame_id=e.id) # e.convex_hull = [] # e.convex_hull.append(gm.Point(box['min']['x'], box['min']['y'], box['min']['z'])) # 1 # e.convex_hull.append(gm.Point(box['max']['x'], box['min']['y'], box['min']['z'])) # 2 # e.convex_hull.append(gm.Point(box['max']['x'], box['max']['y'], box['min']['z'])) # 3 # e.convex_hull.append(gm.Point(box['min']['x'], box['max']['y'], box['min']['z'])) # 4 # # # Make sure we overwrite the e.z_max # e.z_max = box['min']['z'] - 0.04 # 0.04 is the usual offset # return self.determinePointsOfInterest(e) self._candidate_list_obj.append(frame_stamped) # publish marker self.marker_array = MarkerArray() for frame_stamped in self._candidate_list_obj: self.marker_array.markers.append( self.create_marker(frame_stamped.frame.p.x(), frame_stamped.frame.p.y(), frame_stamped.frame.p.z(), e.id)) self.marker_pub.publish(self.marker_array) return self._candidate_list_obj
def determine_points_of_interest_with_area(self, e, area): """ Determines the points of interest using an area :param e: :param area: :return: """ # Just to be sure, copy e e = self.robot.ed.get_entity(id=e.id, parse=True) # We want to give it a convex hull using the designated area if area in e.volumes: box = e.volumes[area] else: rospy.logwarn("Entity {0} has no volume named {1}".format(e.id, area)) if not self._candidate_list_obj: for y in [0, self._spacing, -self._spacing, 2.0 * self._spacing, -2.0 * self._spacing]: if y < box.min_corner.y() or y > box.max_corner.y(): rospy.logerr("Spacing of empty spot designator is too large!!!") continue frame_stamped = kdl_frame_stamped_from_XYZRPY(x=box.max_corner.x() - self._edge_distance, y=y, z=box.min_corner.z() - 0.04, # 0.04 is the usual offset) frame_id=e.id) # e.convex_hull = [] # e.convex_hull.append(gm.Point(box['min']['x'], box['min']['y'], box['min']['z'])) # 1 # e.convex_hull.append(gm.Point(box['max']['x'], box['min']['y'], box['min']['z'])) # 2 # e.convex_hull.append(gm.Point(box['max']['x'], box['max']['y'], box['min']['z'])) # 3 # e.convex_hull.append(gm.Point(box['min']['x'], box['max']['y'], box['min']['z'])) # 4 # # # Make sure we overwrite the e.z_max # e.z_max = box['min']['z'] - 0.04 # 0.04 is the usual offset # return self.determinePointsOfInterest(e) self._candidate_list_obj.append(frame_stamped) # publish marker self.marker_array = MarkerArray() for frame_stamped in self._candidate_list_obj: self.marker_array.markers.append(self.create_marker(frame_stamped.frame.p.x(), frame_stamped.frame.p.y(), frame_stamped.frame.p.z(), e.id)) self.marker_pub.publish(self.marker_array) return self._candidate_list_obj
def determine_points_of_interest_with_area(self, e, area): """ Determines the points of interest using an area :param e: :param area: :return: """ # Just to be sure, copy e e = self.robot.ed.get_entity(id=e.id) # We want to give it a convex hull using the designated area if area in e.volumes: box = e.volumes[area] else: rospy.logwarn("Entity {0} has no volume named {1}".format(e.id, area)) if not self._candidate_list_obj: for y in [0, self._spacing, -self._spacing, 2.0 * self._spacing, -2.0 * self._spacing]: if y < box.min_corner.y() or y > box.max_corner.y(): rospy.logerr("Spacing of empty spot designator is too large!!!") continue fs = kdl_frame_stamped_from_XYZRPY(frame_id=e.id, x=box.max_corner.x() - self._edge_distance, y=y, z=box.min_corner.z() - 0.04) # 0.04 is the usual z offset self._candidate_list_obj.append(fs) # publish marker self.marker_array = MarkerArray() for ps in self._candidate_list_obj: self.marker_array.markers.append(self.create_marker(fs.vector.p.x, fs.vector.p.y, fs.vector.p.z, e.id)) self.marker_pub.publish(self.marker_array) return self._candidate_list_obj
def determine_points_of_interest_with_area(self, e, area): """ Determines the points of interest using an area :param e: :param area: :return: """ # Just to be sure, copy e e = self.robot.ed.get_entity(id=e.id, parse=True) # We want to give it a convex hull using the designated area if area in e.volumes: box = e.volumes[area] else: rospy.logwarn("Entity {0} has no volume named {1}".format(e.id, area)) if not self._candidate_list_obj: for y in [0, self._spacing, -self._spacing, 2.0 * self._spacing, -2.0 * self._spacing]: if y < box.min_corner.y() or y > box.max_corner.y(): rospy.logerr("Spacing of empty spot designator is too large!!!") continue fs = kdl_frame_stamped_from_XYZRPY(frame_id=e.id, x=box.max_corner.x() - self._edge_distance, y=y, z=box.min_corner.z() - 0.04) # 0.04 is the usual z offset self._candidate_list_obj.append(fs) # publish marker self.marker_array = MarkerArray() for ps in self._candidate_list_obj: self.marker_array.markers.append(self.create_marker(fs.vector.p.x, fs.vector.p.y, fs.vector.p.z, e.id)) self.marker_pub.publish(self.marker_array) return self._candidate_list_obj
def execute(self, userdata=None): item_to_place = self._item_to_place_designator.resolve() if not item_to_place: rospy.logerr("Could not resolve item_to_place") # return "failed" placement_fs = self._placement_pose_designator.resolve() if not placement_fs: rospy.logerr("Could not resolve placement_pose") return "failed" arm = self._arm_designator.resolve() if not arm: rospy.logerr("Could not resolve arm") return "failed" rospy.loginfo("Placing") # placement_pose is a PyKDL.Frame place_pose_bl = placement_fs.projectToFrame(self._robot.robot_name+'/base_link', tf_listener=self._robot.tf_listener) # Wait for torso and arm to finish their motions self._robot.torso.wait_for_motion_done() arm.wait_for_motion_done() try: height = place_pose_bl.frame.p.z() except KeyError: height = 0.8 # Pre place if not arm.send_goal(kdl_frame_stamped_from_XYZRPY(place_pose_bl.frame.p.x(), place_pose_bl.frame.p.y(), height+0.15, 0.0, 0.0, 0.0, frame_id="/{0}/base_link".format(self._robot.robot_name)), timeout=10, pre_grasp=True): # If we can't place, try a little closer place_pose_bl.frame.p.x(place_pose_bl.frame.p.x() - 0.025) rospy.loginfo("Retrying preplace") if not arm.send_goal(kdl_frame_stamped_from_XYZRPY(place_pose_bl.frame.p.x(), place_pose_bl.frame.p.y(), height+0.15, 0.0, 0.0, 0.0, frame_id="/{0}/base_link".format(self._robot.robot_name) ), timeout=10, pre_grasp=True): rospy.logwarn("Cannot pre-place the object") arm.cancel_goals() return 'failed' # Place place_pose_bl = placement_fs.projectToFrame(self._robot.robot_name+'/base_link', tf_listener=self._robot.tf_listener) if not arm.send_goal(kdl_frame_stamped_from_XYZRPY(place_pose_bl.frame.p.x(), place_pose_bl.frame.p.y(), height+0.1, 0.0, 0.0, 0.0, frame_id="/{0}/base_link".format(self._robot.robot_name)), timeout=10, pre_grasp=False): rospy.logwarn("Cannot place the object, dropping it...") place_entity = arm.occupied_by if not place_entity: rospy.logerr("Arm not holding an entity to place. This should never happen") else: self._robot.ed.update_entity(place_entity.id, frame_stamped=placement_fs) arm.occupied_by = None # Open gripper # Since we cannot reliably wait for the gripper, just set this timeout arm.send_gripper_goal('open', timeout=2.0) arm.occupied_by = None # Retract place_pose_bl = placement_fs.projectToFrame(self._robot.robot_name+'/base_link', tf_listener=self._robot.tf_listener) arm.send_goal(kdl_frame_stamped_from_XYZRPY(place_pose_bl.frame.p.x() - 0.1, place_pose_bl.frame.p.y(), place_pose_bl.frame.p.z() + 0.15, 0.0, 0.0, 0.0, frame_id='/'+self._robot.robot_name+'/base_link'), timeout=0.0) arm.wait_for_motion_done() self._robot.base.force_drive(-0.125, 0, 0, 1.5) if not arm.wait_for_motion_done(timeout=5.0): rospy.logwarn('Retraction failed') arm.cancel_goals() # Close gripper arm.send_gripper_goal('close', timeout=0.0) arm.reset() arm.wait_for_motion_done() self._robot.torso.reset() self._robot.torso.wait_for_motion_done() return 'succeeded'
def execute(self, userdata=None): item_to_place = self._item_to_place_designator.resolve() if not item_to_place: rospy.logerr("Could not resolve item_to_place") # return "failed" placement_fs = self._placement_pose_designator.resolve() if not placement_fs: rospy.logerr("Could not resolve placement_pose") return "failed" arm = self._arm_designator.resolve() if not arm: rospy.logerr("Could not resolve arm") return "failed" rospy.loginfo("Placing") # placement_pose is a PyKDL.Frame place_pose_bl = placement_fs.projectToFrame( self._robot.robot_name + '/base_link', tf_listener=self._robot.tf_listener) # Wait for torso and arm to finish their motions self._robot.torso.wait_for_motion_done() arm.wait_for_motion_done() try: height = place_pose_bl.frame.p.z() except KeyError: height = 0.8 # Pre place if not arm.send_goal(kdl_frame_stamped_from_XYZRPY( place_pose_bl.frame.p.x(), place_pose_bl.frame.p.y(), height + 0.15, 0.0, 0.0, 0.0, frame_id="/{0}/base_link".format(self._robot.robot_name)), timeout=10, pre_grasp=True): # If we can't place, try a little closer place_pose_bl.frame.p.x(place_pose_bl.frame.p.x() - 0.025) rospy.loginfo("Retrying preplace") if not arm.send_goal(kdl_frame_stamped_from_XYZRPY( place_pose_bl.frame.p.x(), place_pose_bl.frame.p.y(), height + 0.15, 0.0, 0.0, 0.0, frame_id="/{0}/base_link".format(self._robot.robot_name)), timeout=10, pre_grasp=True): rospy.logwarn("Cannot pre-place the object") arm.cancel_goals() return 'failed' # Place place_pose_bl = placement_fs.projectToFrame( self._robot.robot_name + '/base_link', tf_listener=self._robot.tf_listener) if not arm.send_goal(kdl_frame_stamped_from_XYZRPY( place_pose_bl.frame.p.x(), place_pose_bl.frame.p.y(), height + 0.1, 0.0, 0.0, 0.0, frame_id="/{0}/base_link".format(self._robot.robot_name)), timeout=10, pre_grasp=False): rospy.logwarn("Cannot place the object, dropping it...") place_entity = arm.occupied_by if not place_entity: rospy.logerr( "Arm not holding an entity to place. This should never happen") else: self._robot.ed.update_entity(place_entity.id, frame_stamped=placement_fs) arm.occupied_by = None # Open gripper # Since we cannot reliably wait for the gripper, just set this timeout arm.send_gripper_goal('open', timeout=2.0) arm.occupied_by = None # Retract place_pose_bl = placement_fs.projectToFrame( self._robot.robot_name + '/base_link', tf_listener=self._robot.tf_listener) arm.send_goal(kdl_frame_stamped_from_XYZRPY( place_pose_bl.frame.p.x() - 0.1, place_pose_bl.frame.p.y(), place_pose_bl.frame.p.z() + 0.15, 0.0, 0.0, 0.0, frame_id='/' + self._robot.robot_name + '/base_link'), timeout=0.0) arm.wait_for_motion_done() self._robot.base.force_drive(-0.125, 0, 0, 1.5) if not arm.wait_for_motion_done(timeout=5.0): rospy.logwarn('Retraction failed') arm.cancel_goals() # Close gripper arm.send_gripper_goal('close', timeout=0.0) arm.reset() arm.wait_for_motion_done() self._robot.torso.reset() self._robot.torso.wait_for_motion_done() return 'succeeded'
# First make sure the arms are in a known state for side, arm in robot.arms.items(): robot.speech.speak("I will first reset my {} arm".format(side)) arm.reset() for side, arm in robot.arms.items(): failed_actions = [] robot.speech.speak("I will test my {} arm".format(side)) if not arm.operational: rospy.logerr("{} arm is not operational".format(side)) sys.exit(-1) # Notice the - for the right arm's Y position. if arm == robot.leftArm: goal1 = kdl_conversions.kdl_frame_stamped_from_XYZRPY(0.342, 0.125, 0.748, 0, 0, 0, "/"+robot.robot_name+"/base_link") if arm == robot.rightArm: goal1 = kdl_conversions.kdl_frame_stamped_from_XYZRPY(0.342, -0.125, 0.748, 0, 0, 0, "/"+robot.robot_name+"/base_link") robot.speech.speak("Moving {} arm to dummy goal pose".format(side)) if not arm.send_goal(goal1): robot.speech.speak("{} arm could not reach dummy goal pose".format(side)) failed_actions += [goal1] arm.wait_for_motion_done() if arm.send_gripper_goal("open"): robot.speech.speak("My {} hand is now open".format(side)) else: robot.speech.speak("Could not open {} hand".format(side)) failed_actions += ["open gripper"] arm.wait_for_motion_done()
def execute(self, userdata=None): """ A concurrent state machine which follows and afterwards deletes breadcrumbs from the buffer variable :return: follow_bread if the list of breadcrumbs is not empty no_follow_bread_ask_finalize if the single remaining breadcrumb is the operator no_follow_bread_recovery if buffer is completely empty """ while self._buffer: if self.preempt_requested(): return 'aborted' crumb = self._buffer.popleft() if not self._breadcrumb or self._breadcrumb[-1].crumb.distance_to_2d( crumb._pose.p) > self._breadcrumb_distance: self._breadcrumb.append(CrumbWaypoint(crumb)) rospy.loginfo("Self._breadcrumb length: {}".format(len(self._breadcrumb))) # We don't want to base this on distance since that relies on the operator behavior # if self._robot.base.local_planner.getDistanceToGoal() > 0.65: # 2.0: # len(buffer) > 5: # self._have_followed = True robot_position = self._robot.base.get_location().frame if self._breadcrumb: self._operator = self._breadcrumb[-1].crumb else: return 'no_follow_bread_recovery' # # if self._operator: # operator = self._robot.ed.get_entity(id=self._operator.id) # while True: # Should be timer, I think try: _, semantics = self._robot.hmi.query(description="", grammar="T[true] -> %s stop" % self._robot.robot_name, target="T") if semantics: return 'no_follow_bread_ask_finalize' except hmi.TimeoutException: rospy.logwarn("[HMC] Listening timeout") # return 'no_follow_bread_ask_finalize' # if operator: # rospy.loginfo("Distance to operator: {}".format(operator.distance_to_2d(robot_position.p))) # if self._have_followed and operator.distance_to_2d(robot_position.p) < 1.0: # self._have_followed = False # return 'no_follow_bread_ask_finalize' # self._breadcrumb = [crwp for crwp in self._breadcrumb # if crwp.crumb.distance_to_2d(robot_position.p) > self._lookat_radius] current_index = -1 for i, crumb in enumerate(self._breadcrumb): if crumb.crumb.distance_to_2d(robot_position.p) < self._lookat_radius: current_index = i # throw away all breadcrumbs before where we are self._breadcrumb = self._breadcrumb[current_index+1:] f = self._robot.base.get_location().frame previous_point = f.p operator_position = self._operator._pose.p ''' Define end goal constraint, solely based on the (old) operator position ''' p = PositionConstraint() p.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (operator_position.x(), operator_position.y(), self._operator_radius) o = OrientationConstraint() o.frame = self._operator.id ''' Calculate global plan from robot position, through breadcrumbs, to the operator ''' for crumb_waypoint in self._breadcrumb: crumb = crumb_waypoint.crumb diff = crumb._pose.p - previous_point dx, dy = diff.x(), diff.y() length = crumb.distance_to_2d(previous_point) if length != 0: dx_norm = dx / length dy_norm = dy / length yaw = math.atan2(dy, dx) start = 0 end = int(length / self._resolution) for i in range(start, end): x = previous_point.x() + i * dx_norm * self._resolution y = previous_point.y() + i * dy_norm * self._resolution kdl_pose = kdl_conversions.kdl_frame_stamped_from_XYZRPY(x=x, y=y, z=0, yaw=yaw) crumb_waypoint.waypoint = kdl_conversions.kdl_frame_stamped_to_pose_stamped_msg(kdl_pose) previous_point = copy.deepcopy(crumb._pose.p) if len(self._breadcrumb) > 0: ros_plan = [crwp.waypoint for crwp in self._breadcrumb] if not self._robot.base.global_planner.checkPlan(ros_plan): rospy.loginfo("Breadcrumb plan is blocked, removing blocked points") rospy.loginfo("Breadcrumbs before removing blocked points: {}".format(len(ros_plan))) ros_plan = [crwp.waypoint for crwp in self._breadcrumb if self._robot.base.global_planner.checkPlan([crwp.waypoint])] rospy.loginfo("Breadcrumbs length after removing blocked points: {}".format(len(ros_plan))) # if the new ros plan is empty, we do have an operator but there are no breadcrumbs that we can reach # if not ros_plan: # return 'no_follow_bread_recovery' else: return 'no_follow_bread_recovery' buffer_msg = Marker() buffer_msg.type = Marker.POINTS buffer_msg.header.stamp = rospy.get_rostime() buffer_msg.header.frame_id = "/map" buffer_msg.id = 0 buffer_msg.action = Marker.DELETEALL self._breadcrumb_pub.publish(buffer_msg) buffer_msg = Marker() buffer_msg.type = Marker.POINTS buffer_msg.scale.x = 0.05 buffer_msg.scale.y = 0.05 buffer_msg.header.stamp = rospy.get_rostime() buffer_msg.header.frame_id = "/map" buffer_msg.color.a = 1 buffer_msg.color.r = 1 buffer_msg.color.g = 0 buffer_msg.color.b = 0 # buffer_msg.lifetime = rospy.Time(1) buffer_msg.id = 0 buffer_msg.action = Marker.ADD for crumb in self._breadcrumb: buffer_msg.points.append(crumb.waypoint.pose.position) self._breadcrumb_pub.publish(buffer_msg) # line_strip = Marker() # line_strip.type = Marker.LINE_STRIP # line_strip.scale.x = 0.05 # line_strip.header.frame_id = "/map" # line_strip.header.stamp = rospy.Time.now() # line_strip.color.a = 1 # line_strip.color.r = 0 # line_strip.color.g = 1 # line_strip.color.b = 1 # line_strip.id = 0 # line_strip.action = Marker.ADD # # for crwp in ros_plan: # line_strip.points.append(crwp) #self._plan_marker_pub.publish(line_strip) self._robot.base.local_planner.setPlan(ros_plan, p, o) rospy.sleep(rospy.Duration(0.5)) return 'follow_bread'
for side, arm in robot._arms.items(): robot.speech.speak("I will first reset my {} arm".format(side)) arm.reset() for side, arm in robot._arms.items(): failed_actions = [] robot.speech.speak("I will test my {} arm".format(side)) if not arm.operational: rospy.logerr("{} arm is not operational".format(side)) sys.exit(-1) # Notice the - for the right arm's Y position. if arm == robot.leftArm: goal1 = kdl_conversions.kdl_frame_stamped_from_XYZRPY( 0.342, 0.125, 0.748, 0, 0, 0, "/" + robot.robot_name + "/base_link") if arm == robot.rightArm: goal1 = kdl_conversions.kdl_frame_stamped_from_XYZRPY( 0.342, -0.125, 0.748, 0, 0, 0, "/" + robot.robot_name + "/base_link") robot.speech.speak("Moving {} arm to dummy goal pose".format(side)) if not arm.send_goal(goal1): robot.speech.speak( "{} arm could not reach dummy goal pose".format(side)) failed_actions += [goal1] arm.wait_for_motion_done() if arm.send_gripper_goal("open"): robot.speech.speak("My {} hand is now open".format(side))
def _update_navigation(self): """Set the navigation plan to match the breadcrumbs collected into self._breadcrumbs. This list has all the Entity's of where the operator has been""" self._robot.head.cancel_goal() f = self._robot.base.get_location().frame robot_position = f.p operator_position = self._last_operator._pose.p ''' Define end goal constraint, solely based on the (old) operator position ''' p = PositionConstraint() p.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (operator_position.x(), operator_position.y(), self._operator_radius) o = OrientationConstraint() if self._operator_id: o.frame = self._operator_id else: o.frame = 'map' o.look_at = kdl_conversions.kdl_vector_to_point_msg( self._last_operator.pose.frame.p) ''' Calculate global plan from robot position, through breadcrumbs, to the operator ''' res = 0.05 kdl_plan = [] previous_point = robot_position if self._operator: breadcrumbs = self._breadcrumbs + [self._operator] else: breadcrumbs = self._breadcrumbs + [self._last_operator] for crumb in breadcrumbs: assert isinstance(crumb, Entity) diff = crumb._pose.p - previous_point dx, dy = diff.x(), diff.y() length = crumb.distance_to_2d(previous_point) if length != 0: dx_norm = dx / length dy_norm = dy / length yaw = math.atan2(dy, dx) start = 0 end = int(length / res) for i in range(start, end): x = previous_point.x() + i * dx_norm * res y = previous_point.y() + i * dy_norm * res kdl_plan.append( kdl_conversions.kdl_frame_stamped_from_XYZRPY(x=x, y=y, z=0, yaw=yaw)) previous_point = copy.deepcopy(crumb._pose.p) # Delete the elements from the plan within the operator radius from the robot cutoff = int(self._operator_radius / (2.0 * res)) if len(kdl_plan) > cutoff: del kdl_plan[-cutoff:] ros_plan = frame_stampeds_to_pose_stampeds(kdl_plan) # Check if plan is valid. If not, remove invalid points from the path if not self._robot.base.global_planner.checkPlan(ros_plan): print "Breadcrumb plan is blocked, removing blocked points" # Go through plan from operator to robot and pick the first unoccupied point as goal point ros_plan = [ point for point in ros_plan if self._robot.base.global_planner.checkPlan([point]) ] self._visualize_plan(ros_plan) self._robot.base.local_planner.setPlan(ros_plan, p, o)
def execute(self, userdata): """ A concurrent state machine which follows and afterwards deletes breadcrumbs from the buffer variable :return: follow_bread if the list of breadcrumbs is not empty no_follow_bread_ask_finalize if the single remaining breadcrumb is the operator no_follow_bread_recovery if buffer is completely empty """ operator = None while self._buffer: crumb = self._buffer.popleft() if not self._breadcrumb or self._buffer[-1].crumb.distance_to_2d( crumb._pose.p) > self._breadcrumb_distance: self._breadcrumb.append(CrumbWaypoint(crumb)) # if self._buffer[-1].distance_to_2d(crumb._pose.p) > self._breadcrumb_distance: # self._breadcrumb.append(crumb) # rospy.loginfo("Appending operator, length of buffer: {}".format(len(buffer))) # # self._breadcrumb.append((crumb, None)) # Don't use a tuple, that doesn't work... print self._breadcrumb # buffer = userdata.buffer_follow_in if self._robot.base.local_planner.getDistanceToGoal( ) > 2.0: # len(buffer) > 5: self._have_followed = True robot_position = self._robot.base.get_location().frame if not self._breadcrumb: rospy.sleep( 2 ) # magic number, not necessary, can also return a different outcome that does not lead to # recovery/ask finalize if self._breadcrumb: self._operator = self._breadcrumb[-1].crumb else: return 'no_follow_bread_recovery' if self._operator: #and len(buffer) > 1: operator = self._robot.ed.get_entity(id=self._operator.id) rospy.loginfo( "Buffer length at start of FollowBread: {}, have followed: {}". format(len(self._breadcrumb), self._have_followed)) if operator: rospy.loginfo("Distance to operator: {}".format( operator.distance_to_2d(robot_position.p))) if self._have_followed and operator.distance_to_2d( robot_position.p) < 1.0: self._have_followed = False return 'no_follow_bread_ask_finalize' rospy.loginfo("Buffer length before radius check: {}".format( len(self._breadcrumb))) # Throw away crumbs that are too close self._breadcrumb = [ crwp for crwp in self._breadcrumb if crwp.crumb.distance_to_2d(robot_position.p) > self._lookat_radius ] rospy.loginfo("Buffer length after radius check: {}".format( len(self._buffer))) if not self._breadcrumb: return 'no_follow_bread_recovery' f = self._robot.base.get_location().frame previous_point = f.p operator_position = self._operator._pose.p ''' Define end goal constraint, solely based on the (old) operator position ''' p = PositionConstraint() p.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (operator_position.x(), operator_position.y(), self._operator_radius) o = OrientationConstraint() o.frame = self._operator.id ''' Calculate global plan from robot position, through breadcrumbs, to the operator ''' res = 0.05 # magic number number 2 # ros_plan = [] for crumb_waypoint in self._breadcrumb: crumb = crumb_waypoint.crumb diff = crumb._pose.p - previous_point dx, dy = diff.x(), diff.y() length = crumb.distance_to_2d(previous_point) if length != 0: dx_norm = dx / length dy_norm = dy / length yaw = math.atan2(dy, dx) start = 0 end = int(length / res) for i in range(start, end): x = previous_point.x() + i * dx_norm * res y = previous_point.y() + i * dy_norm * res kdl_pose = kdl_conversions.kdl_frame_stamped_from_XYZRPY( x=x, y=y, z=0, yaw=yaw) crumb_waypoint.waypoint = kdl_conversions.kdl_frame_stamped_to_pose_stamped_msg( kdl_pose) previous_point = copy.deepcopy(crumb._pose.p) # Delete the elements from the plan within the operator radius from the robot #cutoff = int(self._operator_radius / (2.0 * res)) #if len(kdl_plan) > cutoff: # del kdl_plan[-cutoff:] # ros_plan = [kdl_conversions.kdl_frame_stamped_to_pose_stamped_msg(frame) for frame in kdl_plan] # Check if plan is valid. If not, remove invalid points from the path if len(self._breadcrumb) > 0: ros_plan = [crwp.waypoint for crwp in self._breadcrumb] if not self._robot.base.global_planner.checkPlan(ros_plan): print "Breadcrumb plan is blocked, removing blocked points" # Go through plan from operator to robot and pick the first unoccupied point as goal point # ros_plan = [point for point in ros_plan if self._robot.base.global_planner.checkPlan([point])] self._breadcrumb = [ crwp for crwp in self._breadcrumb if self._robot.base.global_planner.checkPlan([crwp.waypoint]) ] # buffer_msg = Marker() # buffer_msg.type = Marker.POINTS # buffer_msg.scale.x = 0.05 # buffer_msg.scale.y = 0.05 # buffer_msg.header.stamp = rospy.get_rostime() # buffer_msg.header.frame_id = "/map" # buffer_msg.color.a = 1 # buffer_msg.color.r = 1 # buffer_msg.color.g = 0 # buffer_msg.color.b = 0 # #buffer_msg.lifetime = rospy.Time(.0) # buffer_msg.id = 0 # buffer_msg.action = Marker.ADD # # for crumb in buffer: # buffer_msg.points.append(kdl_conversions.kdl_vector_to_point_msg(crumb.pose.frame.p)) line_strip = Marker() line_strip.type = Marker.LINE_STRIP line_strip.scale.x = 0.05 line_strip.header.frame_id = "/map" line_strip.header.stamp = rospy.Time.now() line_strip.color.a = 1 line_strip.color.r = 0 line_strip.color.g = 1 line_strip.color.b = 1 line_strip.id = 0 line_strip.action = Marker.ADD # Push back all pnts for pose_stamped in ros_plan: line_strip.points.append(pose_stamped.pose.position) self._plan_marker_pub.publish(line_strip) # self._breadcrumb_pub.publish(buffer_msg) self._robot.base.local_planner.setPlan(ros_plan, p, o) # userdata.buffer_follow_out = buffer # ToDo: make nice rospy.sleep(rospy.Duration(0.5)) return 'follow_bread'
def _update_navigation(self): """Set the navigation plan to match the breadcrumbs collected into self._breadcrumbs. This list has all the Entity's of where the operator has been""" self._robot.head.cancel_goal() f = self._robot.base.get_location().frame robot_position = f.p operator_position = self._last_operator._pose.p ''' Define end goal constraint, solely based on the (old) operator position ''' p = PositionConstraint() p.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2"% (operator_position.x(), operator_position.y(), self._operator_radius) o = OrientationConstraint() if self._operator_id: o.frame = self._operator_id else: o.frame = 'map' o.look_at = kdl_conversions.kdl_vector_to_point_msg(self._last_operator.pose.frame.p) ''' Calculate global plan from robot position, through breadcrumbs, to the operator ''' res = 0.05 kdl_plan = [] previous_point = robot_position if self._operator: breadcrumbs = self._breadcrumbs + [self._operator] else: breadcrumbs = self._breadcrumbs + [self._last_operator] for crumb in breadcrumbs: assert isinstance(crumb, Entity) diff = crumb._pose.p - previous_point dx, dy = diff.x(), diff.y() length = crumb.distance_to_2d(previous_point) if length != 0: dx_norm = dx / length dy_norm = dy / length yaw = math.atan2(dy, dx) start = 0 end = int(length / res) for i in range(start, end): x = previous_point.x() + i * dx_norm * res y = previous_point.y() + i * dy_norm * res kdl_plan.append(kdl_conversions.kdl_frame_stamped_from_XYZRPY(x=x, y=y, z=0, yaw=yaw)) previous_point = copy.deepcopy(crumb._pose.p) # Delete the elements from the plan within the operator radius from the robot cutoff = int(self._operator_radius/(2.0*res)) if len(kdl_plan) > cutoff: del kdl_plan[-cutoff:] ros_plan = frame_stampeds_to_pose_stampeds(kdl_plan) # Check if plan is valid. If not, remove invalid points from the path if not self._robot.base.global_planner.checkPlan(ros_plan): print "Breadcrumb plan is blocked, removing blocked points" # Go through plan from operator to robot and pick the first unoccupied point as goal point ros_plan = [point for point in ros_plan if self._robot.base.global_planner.checkPlan([point])] self._visualize_plan(ros_plan) self._robot.base.local_planner.setPlan(ros_plan, p, o)