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 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 _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 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 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 _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 _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)
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 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)