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 action_msgs.msg import GoalStatus
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
from nav2_msgs.action import FollowWaypoints
from nav2_msgs.srv import ManageLifecycleNodes
 
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
from rclpy.qos import QoSProfile
 
 
class WaypointFollowerTest(Node):
 
    def __init__(self):
        super().__init__(node_name='nav2_waypoint_tester', namespace='')
        self.waypointswaypoints = None
        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 = None
 
        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)
 
    def setInitialPose(self, pose):
        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):
        self.info_msginfo_msg('Received amcl_pose')
        self.initial_pose_receivedinitial_pose_received = True
 
    def setWaypoints(self, waypoints):
        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):
        if not self.waypointswaypoints:
            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.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()
 
        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
            result = get_result_future.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(result.missed_waypoints) > 0:
            self.info_msginfo_msg('Goal failed to process all waypoints,'
                          ' missed {0} wps.'.format(len(result.missed_waypoints)))
            return False
 
        self.info_msginfo_msg('Goal succeeded!')
        return True
 
    def publishInitialPose(self):
        self.initial_pose_pubinitial_pose_pub.publish(self.init_poseinit_pose)
 
    def shutdown(self):
        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):
        cancel_future = self.goal_handlegoal_handle.cancel_goal_async()
        rclpy.spin_until_future_complete(self, cancel_future)
 
    def info_msg(self, msg: str):
        self.get_logger().info(msg)
 
    def warn_msg(self, msg: str):
        self.get_logger().warn(msg)
 
    def error_msg(self, msg: str):
        self.get_logger().error(msg)
 
 
def main(argv=sys.argv[1:]):
    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], [0.58, 0.52]]
    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)
    assert result
 
    # preempt with new point
    test.setWaypoints([starting_pose])
    result = test.run(False)
    time.sleep(2)
    test.setWaypoints([wps[1]])
    result = test.run(False)
 
    # cancel
    time.sleep(2)
    test.cancel_goal()
 
    # a failure case
    time.sleep(2)
    test.setWaypoints([[100.0, 100.0]])
    result = test.run(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()