#!/usr/bin/env python3 """Try factory candidate poses in order until one move_base goal succeeds.""" import math import sys import time import actionlib import rospy from actionlib_msgs.msg import GoalStatus from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal from std_msgs.msg import String class FactoryCandidateNavigator: _FAILURE_STATES = { GoalStatus.PREEMPTED, GoalStatus.ABORTED, GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.LOST, } def __init__(self): rospy.init_node("factory_candidate_navigator") self._status_pub = rospy.Publisher( "/factory_candidate_navigator/status", String, queue_size=10, latch=True ) self._initial_pose_pub = rospy.Publisher( "/initialpose", PoseWithCovarianceStamped, queue_size=1 ) self._frame_id = rospy.get_param("~frame_id", "map") self._start_pose = rospy.get_param("~start_pose") self._candidates = rospy.get_param("~candidate_goals") self._server_timeout = float(rospy.get_param("~move_base_server_timeout_seconds", 10.0)) self._goal_timeout = float(rospy.get_param("~goal_timeout_seconds", 120.0)) self._initial_pose_confirmation_timeout = float( rospy.get_param("~initial_pose_confirmation_timeout_seconds", 5.0) ) self._initial_pose_position_tolerance = float( rospy.get_param("~initial_pose_position_tolerance_m", 0.3) ) self._initial_pose_yaw_tolerance = float( rospy.get_param("~initial_pose_yaw_tolerance_rad", 0.2) ) self._latest_amcl_pose = None self._latest_amcl_monotonic = 0.0 self._validate_config() rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self._amcl_callback, queue_size=10) self._client = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.on_shutdown(self._cancel_goal) def _publish(self, text): rospy.loginfo("factory candidate navigator: %s", text) self._status_pub.publish(String(data=text)) def _validate_pose(self, pose, label): try: values = (float(pose["x"]), float(pose["y"]), float(pose["yaw"])) except (KeyError, TypeError, ValueError) as error: raise ValueError("%s pose is invalid: %s" % (label, error)) if not all(math.isfinite(value) for value in values): raise ValueError("%s pose has non-finite values" % label) return values def _validate_config(self): if self._frame_id != "map": raise ValueError("factory candidate goals must use the map frame") self._validate_pose(self._start_pose, "start") if not isinstance(self._candidates, list) or len(self._candidates) != 3: raise ValueError("candidate_goals must contain exactly three poses") for index, candidate in enumerate(self._candidates, start=1): self._validate_pose(candidate, "candidate %d" % index) if self._goal_timeout <= 0.0 or self._server_timeout <= 0.0: raise ValueError("navigation timeouts must be positive") if self._initial_pose_confirmation_timeout <= 0.0: raise ValueError("initial pose confirmation timeout must be positive") if self._initial_pose_position_tolerance <= 0.0 or self._initial_pose_yaw_tolerance <= 0.0: raise ValueError("initial pose confirmation tolerances must be positive") def _amcl_callback(self, message): orientation = message.pose.pose.orientation yaw = math.atan2( 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y), 1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z), ) self._latest_amcl_pose = ( message.pose.pose.position.x, message.pose.pose.position.y, yaw ) self._latest_amcl_monotonic = time.monotonic() @staticmethod def _wrap_to_pi(angle): return math.atan2(math.sin(angle), math.cos(angle)) def _goal_from_candidate(self, candidate): x, y, yaw = self._validate_pose(candidate, "candidate") goal = MoveBaseGoal() goal.target_pose.header.frame_id = self._frame_id goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y = y goal.target_pose.pose.orientation = Quaternion( z=math.sin(yaw / 2.0), w=math.cos(yaw / 2.0) ) return goal, x, y, yaw def _publish_initial_pose(self): x, y, yaw = self._validate_pose(self._start_pose, "start") message = PoseWithCovarianceStamped() message.header.frame_id = self._frame_id message.header.stamp = rospy.Time.now() message.pose.pose.position.x = x message.pose.pose.position.y = y message.pose.pose.orientation.z = math.sin(yaw / 2.0) message.pose.pose.orientation.w = math.cos(yaw / 2.0) message.pose.covariance[0] = 0.25 message.pose.covariance[7] = 0.25 message.pose.covariance[35] = 0.0685 self._publish("INITIALIZING: setting AMCL pose x=%.3f y=%.3f yaw=%.3f" % (x, y, yaw)) for _ in range(3): message.header.stamp = rospy.Time.now() self._initial_pose_pub.publish(message) rospy.sleep(0.1) def _wait_for_initial_pose_confirmation(self): target_x, target_y, target_yaw = self._validate_pose(self._start_pose, "start") deadline = time.monotonic() + self._initial_pose_confirmation_timeout while not rospy.is_shutdown() and time.monotonic() < deadline: pose = self._latest_amcl_pose fresh = time.monotonic() - self._latest_amcl_monotonic <= 1.0 if pose is not None and fresh: x, y, yaw = pose position_error = math.hypot(x - target_x, y - target_y) yaw_error = abs(self._wrap_to_pi(yaw - target_yaw)) if (position_error <= self._initial_pose_position_tolerance and yaw_error <= self._initial_pose_yaw_tolerance): self._publish("INITIAL_POSE_CONFIRMED: position_error=%.3f yaw_error=%.3f" % ( position_error, yaw_error )) return True rospy.sleep(0.05) self._publish("INITIAL_POSE_TIMEOUT: candidate navigation not started") return False def _cancel_goal(self): if hasattr(self, "_client"): self._client.cancel_goal() def run(self): sx, sy, syaw = self._validate_pose(self._start_pose, "start") self._publish_initial_pose() if not self._wait_for_initial_pose_confirmation(): return 1 self._publish("START: task2 start x=%.3f y=%.3f yaw=%.3f" % (sx, sy, syaw)) if not self._client.wait_for_server(rospy.Duration(self._server_timeout)): self._publish("FAILED: move_base unavailable") return 1 for index, candidate in enumerate(self._candidates, start=1): goal, x, y, yaw = self._goal_from_candidate(candidate) self._publish("NAVIGATING: candidate %d/3 x=%.3f y=%.3f yaw=%.3f" % ( index, x, y, yaw )) self._client.send_goal(goal) if not self._client.wait_for_result(rospy.Duration(self._goal_timeout)): self._client.cancel_goal() self._publish("FAILED: candidate %d/3 timed out; trying next" % index) continue state = self._client.get_state() if state == GoalStatus.SUCCEEDED: self._publish("SUCCEEDED: candidate %d/3 reached; remaining candidates skipped" % index) return 0 if state in self._FAILURE_STATES: self._publish("FAILED: candidate %d/3 move_base state=%d; trying next" % ( index, state )) else: self._client.cancel_goal() self._publish("FAILED: candidate %d/3 unexpected state=%d; trying next" % ( index, state )) self._publish("ALL_CANDIDATES_FAILED") return 1 def main(): try: navigator = FactoryCandidateNavigator() return navigator.run() except (ValueError, rospy.ROSException) as error: rospy.logerr("factory candidate navigator configuration error: %s", error) return 2 if __name__ == "__main__": sys.exit(main())