tester.py

#! /usr/bin/env python3
# Copyright 2019 Samsung Research America
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
 
import sys
import time
from typing import Optional
 
from action_msgs.msg import GoalStatus
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
from nav2_msgs.action import ComputePathToPose, FollowWaypoints
from nav2_msgs.srv import ManageLifecycleNodes
from rcl_interfaces.srv import SetParameters
import rclpy
from rclpy.action import ActionClient  # type: ignore[attr-defined]
from rclpy.action.client import ClientGoalHandle
from rclpy.node import Node
from rclpy.parameter import Parameter
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 
 
class WaypointFollowerTest(Node):
 
    def __init__(self) -> None:
        super().__init__(node_name='nav2_waypoint_tester', namespace='')
        self.waypointswaypoints: list[float] = []
        self.action_clientaction_client = ActionClient(self, FollowWaypoints, 'follow_waypoints')
        self.initial_pose_pubinitial_pose_pub = self.create_publisher(
            PoseWithCovarianceStamped, 'initialpose', 10
        )
        self.initial_pose_receivedinitial_pose_received = False
        self.goal_handlegoal_handle: Optional[ClientGoalHandle[
                FollowWaypoints.Goal, FollowWaypoints.Result,
                FollowWaypoints.Feedback]] = None
        self.action_resultaction_result = FollowWaypoints.Result()
 
        pose_qos = QoSProfile(
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
            reliability=QoSReliabilityPolicy.RELIABLE,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=1,
        )
 
        self.model_pose_submodel_pose_sub = self.create_subscription(
            PoseWithCovarianceStamped, 'amcl_pose', self.poseCallbackposeCallback, pose_qos
        )
        self.param_cliparam_cli = self.create_client(
            SetParameters, '/waypoint_follower/set_parameters'
        )
 
    def setInitialPose(self, pose: list[float]) -> None:
        self.init_poseinit_pose = PoseWithCovarianceStamped()
        self.init_poseinit_pose.pose.pose.position.x = pose[0]
        self.init_poseinit_pose.pose.pose.position.y = pose[1]
        self.init_poseinit_pose.header.frame_id = 'map'
        self.publishInitialPosepublishInitialPose()
        time.sleep(5)
 
    def poseCallback(self, msg: PoseWithCovarianceStamped) -> None:
        self.info_msginfo_msg('Received amcl_pose')
        self.initial_pose_receivedinitial_pose_received = True
 
    def setWaypoints(self, waypoints: list[list[float]]) -> None:
        self.waypointswaypoints = []
        for wp in waypoints:
            msg = PoseStamped()
            msg.header.frame_id = 'map'
            msg.pose.position.x = wp[0]
            msg.pose.position.y = wp[1]
            msg.pose.orientation.w = 1.0
            self.waypointswaypoints.append(msg)
 
    def run(self, block: bool, cancel: bool) -> bool:
        # if not self.waypoints:
        #     rclpy.error_msg('Did not set valid waypoints before running test!')
        #     return False
 
        while not self.action_clientaction_client.wait_for_server(timeout_sec=1.0):
            self.info_msginfo_msg("'follow_waypoints' action server not available, waiting...")
 
        action_request = FollowWaypoints.Goal()
        action_request.poses = self.waypointswaypoints
 
        self.info_msginfo_msg('Sending goal request...')
        send_goal_future = self.action_clientaction_client.send_goal_async(action_request)
        try:
            rclpy.spin_until_future_complete(self, send_goal_future)
            self.goal_handlegoal_handle = send_goal_future.result()
        except Exception as e:  # noqa: B902
            self.error_msgerror_msg(f'Service call failed {e!r}')
 
        if not self.goal_handlegoal_handle or not self.goal_handlegoal_handle.accepted:
            self.error_msgerror_msg('Goal rejected')
            return False
 
        self.info_msginfo_msg('Goal accepted')
        if not block:
            return True
 
        get_result_future = self.goal_handlegoal_handle.get_result_async()
        if cancel:
            time.sleep(2)
            self.cancel_goalcancel_goal()
 
        self.info_msginfo_msg("Waiting for 'follow_waypoints' action to complete")
        try:
            rclpy.spin_until_future_complete(self, get_result_future)
            status = get_result_future.result().status  # type: ignore[union-attr]
            result = get_result_future.result().result  # type: ignore[union-attr]
            self.action_resultaction_result = result
        except Exception as e:  # noqa: B902
            self.error_msgerror_msg(f'Service call failed {e!r}')
 
        if status != GoalStatus.STATUS_SUCCEEDED:
            self.info_msginfo_msg(f'Goal failed with status code: {status}')
            return False
        if len(self.action_resultaction_result.missed_waypoints) > 0:
            self.info_msginfo_msg(
                'Goal failed to process all waypoints,'
                f' missed {len(self.action_result.missed_waypoints)} wps.'
            )
            return False
 
        self.info_msginfo_msg('Goal succeeded!')
        return True
 
    def publishInitialPose(self) -> None:
        self.initial_pose_pubinitial_pose_pub.publish(self.init_poseinit_pose)
 
    def setStopFailureParam(self, value: bool) -> None:
        req = SetParameters.Request()
        req.parameters = [
            Parameter('stop_on_failure', Parameter.Type.BOOL, value).to_parameter_msg()
        ]
        future = self.param_cliparam_cli.call_async(req)
        rclpy.spin_until_future_complete(self, future)
 
    def shutdown(self) -> None:
        self.info_msginfo_msg('Shutting down')
 
        self.action_clientaction_client.destroy()
        self.info_msginfo_msg('Destroyed follow_waypoints action client')
 
        transition_service = 'lifecycle_manager_navigation/manage_nodes'
        mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
        while not mgr_client.wait_for_service(timeout_sec=1.0):
            self.info_msginfo_msg(f'{transition_service} service not available, waiting...')
 
        req = ManageLifecycleNodes.Request()
        req.command = ManageLifecycleNodes.Request().SHUTDOWN
        future = mgr_client.call_async(req)
        try:
            rclpy.spin_until_future_complete(self, future)
            future.result()
        except Exception as e:  # noqa: B902
            self.error_msgerror_msg(f'{transition_service} service call failed {e!r}')
 
        self.info_msginfo_msg(f'{transition_service} finished')
 
        transition_service = 'lifecycle_manager_localization/manage_nodes'
        mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
        while not mgr_client.wait_for_service(timeout_sec=1.0):
            self.info_msginfo_msg(f'{transition_service} service not available, waiting...')
 
        req = ManageLifecycleNodes.Request()
        req.command = ManageLifecycleNodes.Request().SHUTDOWN
        future = mgr_client.call_async(req)
        try:
            rclpy.spin_until_future_complete(self, future)
            future.result()
        except Exception as e:  # noqa: B902
            self.error_msgerror_msg(f'{transition_service} service call failed {e!r}')
 
        self.info_msginfo_msg(f'{transition_service} finished')
 
    def cancel_goal(self) -> None:
        cancel_future = self.goal_handlegoal_handle.cancel_goal_async()  # type: ignore[union-attr]
        rclpy.spin_until_future_complete(self, cancel_future)
 
    def info_msg(self, msg: str) -> None:
        self.get_logger().info(msg)
 
    def warn_msg(self, msg: str) -> None:
        self.get_logger().warn(msg)
 
    def error_msg(self, msg: str) -> None:
        self.get_logger().error(msg)
 
 
def main(argv: list[str] = sys.argv[1:]):  # type: ignore[no-untyped-def]
    rclpy.init()
 
    # wait a few seconds to make sure entire stacks are up
    time.sleep(10)
 
    wps = [[-0.52, -0.54], [0.58, -0.55], [1.78, -0.57]]
    starting_pose = [-2.0, -0.5]
 
    test = WaypointFollowerTest()
    test.setWaypoints(wps)
 
    retry_count = 0
    retries = 2
    while not test.initial_pose_received and retry_count <= retries:
        retry_count += 1
        test.info_msg('Setting initial pose')
        test.setInitialPose(starting_pose)
        test.info_msg('Waiting for amcl_pose to be received')
        rclpy.spin_once(test, timeout_sec=1.0)  # wait for poseCallback
 
    result = test.run(True, False)
    assert result
 
    # preempt with new point
    test.setWaypoints([starting_pose])
    result = test.run(False, False)
    time.sleep(2)
    test.setWaypoints([wps[1]])
    result = test.run(False, False)
 
    # cancel
    time.sleep(2)
    test.cancel_goal()
 
    # set waypoint outside of map
    time.sleep(2)
    test.setWaypoints([[100.0, 100.0]])
    result = test.run(True, False)
    assert not result
    result = not result
    assert (
        test.action_result.missed_waypoints[0].error_code
        == ComputePathToPose.Result().GOAL_OUTSIDE_MAP
    )
    assert (test.action_result.missed_waypoints[0].error_msg != '')
 
    # stop on failure test with bogus waypoint
    test.setStopFailureParam(True)
    bwps = [[-0.52, -0.54], [100.0, 100.0], [0.58, 0.52]]
    test.setWaypoints(bwps)
    result = test.run(True, False)
    assert not result
    result = not result
    mwps = test.action_result.missed_waypoints
    result = (len(mwps) == 1) & (mwps[0] == 1)
    test.setStopFailureParam(False)
 
    # Zero goal test
    test.setWaypoints([])
    result = test.run(True, False)
 
    # Cancel test
    test.setWaypoints(wps)
    result = test.run(True, True)
    assert not result
    result = not result
 
    test.shutdown()
    test.info_msg('Done Shutting Down.')
 
    if not result:
        test.info_msg('Exiting failed')
        exit(1)
    else:
        test.info_msg('Exiting passed')
        exit(0)
 
 
if __name__ == '__main__':
    main()