def _turn_towards_operator(self): f = self._robot.base.get_location().frame robot_position = f.p operator_position = self._last_operator._pose.p 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) dx = operator_position.x() - robot_position.x() dy = operator_position.y() - robot_position.y() yaw = math.atan2(dy, dx) # ToDo: make nice! pose = geometry_msgs.msg.PoseStamped() pose.header.frame_id = "/map" pose.header.stamp = rospy.Time.now() pose.pose.position.x = robot_position.x() pose.pose.position.y = robot_position.y() xx, yy, zz, ww = kdl.Rotation.RPY(0, 0, yaw).GetQuaternion() pose.pose.orientation.x = xx pose.pose.orientation.y = yy pose.pose.orientation.z = zz pose.pose.orientation.w = ww plan = [pose] print "Operator within self._lookat_radius" self._robot.base.local_planner.setPlan(plan, p, o)
def execute(self, userdata=None): door_1_position = self._robot.ed.get_entity(id=challenge_knowledge.reentry_door_1).pose.position door_2_position = self._robot.ed.get_entity(id=challenge_knowledge.reentry_door_2).pose.position door_1_constraint = PositionConstraint() door_1_constraint.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2"% (door_1_position.x, door_1_position.y, 0.7) door_2_constraint = PositionConstraint() door_2_constraint.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (door_2_position.x, door_2_position.y, 0.7) # TODO: make sure that waypoints exist!!! # TODO: make sure door id designator is writeable while True: plan1 = self._robot.base.global_planner.getPlan(door_1_constraint) plan2 = self._robot.base.global_planner.getPlan(door_2_constraint) print "Plan 1 length: %i" % len(plan1) print "Plan 2 length: %i" % len(plan2) if len(plan1) < 3: self._door_id_designator.writeable.write(challenge_knowledge.target_door_1) return "door_found" elif len(plan2) < 3: self._door_id_designator.writeable.write(challenge_knowledge.target_door_2) return "door_found" if self.preempt_requested(): return 'preempted' time.sleep(1)
def move(self, position_constraint_string, frame): p = PositionConstraint() p.constraint = position_constraint_string p.frame = frame plan = self.global_planner.getPlan(p) if plan: o = OrientationConstraint() o.frame = frame self.local_planner.setPlan(plan, p, o) return plan
def execute(self, userdata=None): e = self.destination_designator.resolve() if not e: rospy.logerr( "CheckDoorPassable::execute: No entity could be resolved from designator '%s'" % self.destination_designator) return None try: pose = e.data["pose"] x = pose["x"] y = pose["y"] rz = e.data["pose"]["rz"] except: try: x = e.pose.position.x y = e.pose.position.y except: return None pc = PositionConstraint(constraint="(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, 0.5), frame="/map") plan = self.robot.base.global_planner.getPlan(pc) if plan and len(plan) < 3 and computePathLength(plan) < 3.0: return "passable" else: return "blocked"
def generateConstraint(self): arm = self.arm.resolve() if arm == self.robot.arms['left']: angle_offset = math.atan2(-self.robot.grasp_offset.y, self.robot.grasp_offset.x) elif arm == self.robot.arms['right']: angle_offset = math.atan2(self.robot.grasp_offset.y, self.robot.grasp_offset.x) else: raise IndexError('Arm not found') radius = math.hypot(self.robot.grasp_offset.x, self.robot.grasp_offset.y) x = self.point.point.x y = self.point.point.y frame_id = self.point.header.frame_id # Outer radius radius -= 0.1 ro = "(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, radius + 0.075) ri = "(x-%f)^2+(y-%f)^2 > %f^2" % (x, y, radius - 0.075) pc = PositionConstraint(constraint=ri + " and " + ro, frame=frame_id) oc = OrientationConstraint(look_at=Point(x, y, 0.0), frame=frame_id, angle_offset=angle_offset) return pc, oc
def execute(self, userdata=None): door_1_position = self._robot.ed.get_entity( id=challenge_knowledge.reentry_door_1).pose.position door_2_position = self._robot.ed.get_entity( id=challenge_knowledge.reentry_door_2).pose.position door_1_constraint = PositionConstraint() door_1_constraint.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % ( door_1_position.x, door_1_position.y, 0.7) door_2_constraint = PositionConstraint() door_2_constraint.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % ( door_2_position.x, door_2_position.y, 0.7) # TODO: make sure that waypoints exist!!! # TODO: make sure door id designator is writeable while True: plan1 = self._robot.base.global_planner.getPlan(door_1_constraint) plan2 = self._robot.base.global_planner.getPlan(door_2_constraint) print "Plan 1 length: %i" % len(plan1) print "Plan 2 length: %i" % len(plan2) if len(plan1) < 3: self._door_id_designator.writeable.write( challenge_knowledge.target_door_1) return "door_found" elif len(plan2) < 3: self._door_id_designator.writeable.write( challenge_knowledge.target_door_2) return "door_found" if self.preempt_requested(): return 'preempted' time.sleep(1)
def __init__(self, robot, entity_id, obstacle_radius): smach.State.__init__(self, outcomes=["done", "timeout"]) self._robot = robot try: pose = robot.ed.get_entity(id=entity_id).pose except Exception as e: rospy.logerr(e) sys.exit(1) self.pc = PositionConstraint(frame="/map", constraint="(x-%f)^2+(y-%f)^2 < 0.05" % (pose.position.x, pose.position.y)) self.obstacle_radius = obstacle_radius
def _turn_towards_operator(self): f = self._robot.base.get_location().frame robot_position = f.p operator_position = self._last_operator._pose.p 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.kdlVectorToPointMsg( self._last_operator.pose.frame.p) dx = operator_position.x() - robot_position.x() dy = operator_position.y() - robot_position.y() yaw = math.atan2(dy, dx) # ToDo: make nice! pose = geometry_msgs.msg.PoseStamped() pose.header.frame_id = "/map" pose.header.stamp = rospy.Time.now() pose.pose.position.x = robot_position.x() pose.pose.position.y = robot_position.y() xx, yy, zz, ww = kdl.Rotation.RPY(0, 0, yaw).GetQuaternion() pose.pose.orientation.x = xx pose.pose.orientation.y = yy pose.pose.orientation.z = zz pose.pose.orientation.w = ww plan = [pose] print "Operator within self._lookat_radius" self._robot.base.local_planner.setPlan(plan, p, o)
def get_position_constraint(self, entity_id_area_name_map): try: res = self._get_constraint_srv( entity_ids=[k for k in entity_id_area_name_map], area_names=[v for k, v in entity_id_area_name_map.iteritems()]) except Exception as e: rospy.logerr("Can't get position constraint: {}".format(e)) return None if res.error_msg != '': rospy.logerr(res.error_msg) return None return PositionConstraint(constraint=res.position_constraint_map_frame, frame="/map")
def distance_to_poi_area(poi): #Derived from navigate_to_place radius = math.hypot(self.robot.grasp_offset.x, self.robot.grasp_offset.y) x = poi.point.x y = poi.point.y ro = "(x-%f)^2+(y-%f)^2 < %f^2"%(x, y, radius+0.075) ri = "(x-%f)^2+(y-%f)^2 > %f^2"%(x, y, radius-0.075) pos_constraint = PositionConstraint(constraint=ri+" and "+ro, frame="/map") plan_to_poi = self.robot.base.global_planner.getPlan(pos_constraint) if plan_to_poi: distance = len(plan_to_poi) print "Distance: %s"%distance else: distance = None return distance
def distance_to_poi_area(frame_stamped): # Derived from navigate_to_place radius = math.hypot(self.robot.grasp_offset.x, self.robot.grasp_offset.y) x = frame_stamped.frame.p.x() y = frame_stamped.frame.p.y() ro = "(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, radius + 0.075) ri = "(x-%f)^2+(y-%f)^2 > %f^2" % (x, y, radius - 0.075) pos_constraint = PositionConstraint(constraint=ri + " and " + ro, frame=frame_stamped.frame_id) plan_to_poi = self.robot.base.global_planner.getPlan(pos_constraint) if plan_to_poi: distance = len(plan_to_poi) # print "Distance to {fs}: {dist}".format(dist=distance, fs=frame_stamped.frame.p) else: distance = None return distance
class Navigation(RobotPart): def __init__(self, robot_name, tf_listener): super(Navigation, self).__init__(robot_name=robot_name, tf_listener=tf_listener) self._get_constraint_srv = self.create_service_client('/%s/ed/navigation/get_constraint' % robot_name, GetGoalConstraint) def get_position_constraint(self, entity_id_area_name_map): try: res = self._get_constraint_srv(entity_ids=[ k for k in entity_id_area_name_map ], area_names=[ v for k,v in entity_id_area_name_map.iteritems() ]) except Exception, e: rospy.logerr(e) return None if res.error_msg != '': rospy.logerr(res.error_msg) return None return PositionConstraint(constraint=res.position_constraint_map_frame, frame="/map")
def distance_to_poi_area(self, frame_stamped): # ToDo: cook up something better: we need the arm that we're currently using but this would require a # rather large API break (issue #739) base_offset = self.robot.arms.values()[0].base_offset radius = math.hypot(base_offset.x(), base_offset.y()) x = frame_stamped.frame.p.x() y = frame_stamped.frame.p.y() radius -= 0.1 ro = "(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, radius + 0.075) ri = "(x-%f)^2+(y-%f)^2 > %f^2" % (x, y, radius - 0.075) pos_constraint = PositionConstraint(constraint=ri + " and " + ro, frame=frame_stamped.frame_id) plan_to_poi = self.robot.base.global_planner.getPlan(pos_constraint) if plan_to_poi: distance = len(plan_to_poi) # print "Distance to {fs}: {dist}".format(dist=distance, fs=frame_stamped.frame.p) else: distance = None return distance
def _replan(self): self._replan_attempts += 1 print "Trying to get a global plan" operator_position = self._last_operator._pose.p # Define end goal constraint, solely based on the (old) operator position self._replan_pc = PositionConstraint() self._replan_pc.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % ( operator_position.x(), operator_position.y(), self._operator_radius) ros_plan = self._robot.base.global_planner.getPlan(self._replan_pc) if not ros_plan or not self._robot.base.global_planner.checkPlan( ros_plan): print "No global plan possible" else: self._robot.speech.speak("Just a sec, let me try this way.") print "Found a global plan, sending it to local planner" self._replan_time = rospy.Time.now() self._replan_active = True oc = self._robot.base.local_planner.getCurrentOrientationConstraint( ) self._visualize_plan(ros_plan) self._robot.base.local_planner.setPlan(ros_plan, self._replan_pc, oc) self._breadcrumbs = []
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'
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 __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done','Aborted']) self.robot = robot self.position_constraint = PositionConstraint() self.orientation_constraint = OrientationConstraint() self.position_constraint.constraint = "x^2+y^2 < 1" self.requested_location = None rospy.Subscriber("/location_request", std_msgs.msg.String, self.requestedLocationcallback) self.random_nav_designator = RandomNavDesignator(self.robot) with self: smach.StateMachine.add( "WAIT_A_SEC", states.WaitTime(robot, waittime=1.0), transitions={'waited' :"SELECT_ACTION", 'preempted':"Aborted"}) smach.StateMachine.add( "SELECT_ACTION", SelectAction(), transitions={ 'continue' : "DETERMINE_TARGET", 'pause' : "SELECT_ACTION", 'stop' : "SAY_DONE"}) @smach.cb_interface(outcomes=['target_determined', 'no_targets_available'], input_keys=[], output_keys=[]) def determine_target(userdata, designator): entity_id = designator.getRandomGoal() sentences = [ "Lets go look at the %s", "Lets have a look at the %s", "Lets go to the %s", "Lets move to the %s", "I will go to the %s", "I will now move to the %s", "I will now drive to the %s", "I will look the %s", "The %s will be my next location", "The %s it is", "New goal, the %s", "Going to look at the %s", "Moving to the %s", "Driving to the %s", "On to the %s", "On the move to the %s", "Going to the %s"] robot.speech.speak(random.choice(sentences)%entity_id, block=False) return 'target_determined' smach.StateMachine.add('DETERMINE_TARGET', smach.CBState(determine_target, cb_kwargs={'designator': self.random_nav_designator}), transitions={ 'target_determined':'DRIVE', 'no_targets_available':'SELECT_ACTION'}) smach.StateMachine.add( 'DRIVE', states.NavigateToObserve(robot, self.random_nav_designator), transitions={ "arrived":"SAY_SUCCEEDED", "unreachable":'SAY_UNREACHABLE', "goal_not_defined":'SELECT_ACTION'}) smach.StateMachine.add("SAY_SUCCEEDED", states.Say(robot, [ "I am here", "Goal succeeded", "Another goal succeeded", "Goal reached", "Another goal reached", "Target reached", "Another target reached", "Destination reached", "Another destination reached", "I have arrived", "I have arrived at my goal", "I have arrived at my target", "I have arrived at my destination", "I am at my goal", "I am at my target", "I am at my destination", "Here I am",]), transitions={ 'spoken':'SELECT_ACTION'}) smach.StateMachine.add("SAY_UNREACHABLE", states.Say(robot, [ "I can't find a way to my goal, better try something else", "This goal is unreachable, I better find somewhere else to go", "I am having a hard time getting there so I will look for a new target"]), transitions={ 'spoken':'SELECT_ACTION'}) smach.StateMachine.add("SAY_DONE", states.Say(robot, [ "That's all folks", "I'll stay here for a while", "Goodbye"]), transitions={ 'spoken':'Done'})
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.kdlVectorToPointMsg( 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.kdlFrameStampedFromXYZRPY(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)