ifly/factory_alignment_demo/scripts/factory_candidate_navigator.py

#!/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())