drive_tester.py

#! /usr/bin/env python3
# Copyright 2024 Open Navigation LLC
#
# 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 Point32, PolygonStamped
from nav2_msgs.action import DriveOnHeading
from nav2_msgs.msg import Costmap
from nav2_msgs.srv import ManageLifecycleNodes
import rclpy
from rclpy.action import ActionClient  # type: ignore[attr-defined]
from rclpy.action.client import ClientGoalHandle
from rclpy.duration import Duration
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 
 
class DriveTest(Node):
 
    def __init__(self) -> None:
        super().__init__(node_name='drive_tester', namespace='')
        self.costmap_qoscostmap_qos = QoSProfile(
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
            reliability=QoSReliabilityPolicy.RELIABLE,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=1,
        )
        self.action_clientaction_client = ActionClient(self, DriveOnHeading, 'drive_on_heading')
        self.costmap_pubcostmap_pub = self.create_publisher(
            Costmap, 'local_costmap/costmap_raw', self.costmap_qoscostmap_qos)
        self.footprint_pubfootprint_pub = self.create_publisher(
            PolygonStamped, 'local_costmap/published_footprint', 10)
        self.goal_handlegoal_handle: Optional[ClientGoalHandle[
                DriveOnHeading.Goal, DriveOnHeading.Result,
                DriveOnHeading.Feedback]] = None
        self.action_resultaction_result = DriveOnHeading.Result()
 
    def sendCommand(self, command: DriveOnHeading.Goal) -> bool:
        self.info_msginfo_msg('Sending goal request...')
        self.goal_futuregoal_future = self.action_clientaction_client.send_goal_async(command)
        try:
            rclpy.spin_until_future_complete(self, self.goal_futuregoal_future)
            self.goal_handlegoal_handle = self.goal_futuregoal_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')
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
 
        self.info_msginfo_msg("Waiting for 'DriveOnHeading' action to complete")
        try:
            rclpy.spin_until_future_complete(self, self.result_futureresult_future)
            status = self.result_futureresult_future.result().status  # type: ignore[union-attr]
            result = self.result_futureresult_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 self.action_resultaction_result.error_code == 0:
            self.info_msginfo_msg('DriveOnHeading was successful!')
            return True
        self.info_msginfo_msg('DriveOnHeading failed to meet target!')
        return False
 
    def sendAndPreemptWithFasterCommand(self, command: DriveOnHeading.Goal) -> bool:
        # Send initial goal
        self.info_msginfo_msg('Sending goal request...')
        self.goal_futuregoal_future = self.action_clientaction_client.send_goal_async(command)
        try:
            rclpy.spin_until_future_complete(self, self.goal_futuregoal_future)
            self.goal_handlegoal_handle = self.goal_futuregoal_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')
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
 
        # Now preempt it
        time.sleep(0.5)
        self.info_msginfo_msg('Sending preemption request...')
        command.speed = 0.2
        self.goal_futuregoal_future = self.action_clientaction_client.send_goal_async(command)
        try:
            rclpy.spin_until_future_complete(self, self.goal_futuregoal_future)
            self.goal_handlegoal_handle = self.goal_futuregoal_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('Preemption rejected')
            return False
 
        self.info_msginfo_msg('Preemption accepted')
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
 
        # Wait for new goal to complete
        self.info_msginfo_msg("Waiting for 'DriveOnHeading' action Preemption to complete")
        try:
            rclpy.spin_until_future_complete(self, self.result_futureresult_future)
            status = self.result_futureresult_future.result().status  # type: ignore[union-attr]
            result = self.result_futureresult_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 self.action_resultaction_result.error_code == 0:
            self.info_msginfo_msg('DriveOnHeading was successful!')
            return True
        self.info_msginfo_msg('DriveOnHeading failed to meet target!')
        return False
 
    def sendAndCancelCommand(self, command: DriveOnHeading.Goal) -> bool:
        self.info_msginfo_msg('Sending goal request...')
        self.goal_futuregoal_future = self.action_clientaction_client.send_goal_async(command)
        try:
            rclpy.spin_until_future_complete(self, self.goal_futuregoal_future)
            self.goal_handlegoal_handle = self.goal_futuregoal_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')
        self.result_futureresult_future = self.goal_handlegoal_handle.get_result_async()
 
        # Now cancel it
        time.sleep(0.5)
        cancel_future = self.goal_handlegoal_handle.cancel_goal_async()
        rclpy.spin_until_future_complete(self, cancel_future)
        rclpy.spin_until_future_complete(self, self.result_futureresult_future)
        status = self.result_futureresult_future.result().status  # type: ignore[union-attr]
        if status != GoalStatus.STATUS_CANCELED:
            self.info_msginfo_msg(f'Goal failed with status code: {status}')
            return False
        else:
            self.info_msginfo_msg('Goal was canceled successfully')
            return True
 
    def sendFreeCostmap(self) -> None:
        costmap_msg = Costmap()
        costmap_msg.header.frame_id = 'odom'
        costmap_msg.header.stamp = self.get_clock().now().to_msg()
        costmap_msg.metadata.resolution = 0.05
        costmap_msg.metadata.size_x = 100
        costmap_msg.metadata.size_y = 100
        costmap_msg.metadata.origin.position.x = -2.5
        costmap_msg.metadata.origin.position.y = -2.5
        costmap_msg.data = [0] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y)
        self.costmap_pubcostmap_pub.publish(costmap_msg)
 
        footprint_msg = PolygonStamped()
        footprint_msg.header.frame_id = 'odom'
        footprint_msg.header.stamp = self.get_clock().now().to_msg()
        footprint_msg.polygon.points = [
            Point32(x=0.1, y=0.1, z=0.0),
            Point32(x=0.1, y=-0.1, z=0.0),
            Point32(x=-0.1, y=-0.1, z=0.0),
            Point32(x=-0.1, y=0.1, z=0.0)
        ]
        self.footprint_pubfootprint_pub.publish(footprint_msg)
 
    def sendOccupiedCostmap(self) -> None:
        costmap_msg = Costmap()
        costmap_msg.header.frame_id = 'odom'
        costmap_msg.header.stamp = self.get_clock().now().to_msg()
        costmap_msg.metadata.resolution = 0.05
        costmap_msg.metadata.size_x = 100
        costmap_msg.metadata.size_y = 100
        costmap_msg.metadata.origin.position.x = -2.5
        costmap_msg.metadata.origin.position.y = -2.5
        costmap_msg.data = [254] * (costmap_msg.metadata.size_x * costmap_msg.metadata.size_y)
        self.costmap_pubcostmap_pub.publish(costmap_msg)
 
        footprint_msg = PolygonStamped()
        footprint_msg.header.frame_id = 'odom'
        footprint_msg.header.stamp = self.get_clock().now().to_msg()
        footprint_msg.polygon.points = [
            Point32(x=0.1, y=0.1, z=0.0),
            Point32(x=0.1, y=-0.1, z=0.0),
            Point32(x=-0.1, y=-0.1, z=0.0),
            Point32(x=-0.1, y=0.1, z=0.0)
        ]
        self.footprint_pubfootprint_pub.publish(footprint_msg)
 
    def run(self) -> bool:
        while not self.action_clientaction_client.wait_for_server(timeout_sec=1.0):
            self.info_msginfo_msg("'DriveOnHeading' action server not available, waiting...")
 
        # Test A: Send without valid costmap
        action_request = DriveOnHeading.Goal()
        action_request.speed = 0.15
        action_request.target.x = 0.15
        action_request.time_allowance = Duration(seconds=5).to_msg()
        cmd1 = self.sendCommandsendCommand(action_request)
 
        if cmd1:
            self.error_msgerror_msg('Test A failed: Passed while costmap was not available!')
            return not cmd1
        else:
            self.info_msginfo_msg('Test A passed')
 
        # Test B: Send with valid costmap and DriveOnHeading a couple of times
        self.sendFreeCostmapsendFreeCostmap()
        time.sleep(1)
        cmd1 = self.sendCommandsendCommand(action_request)
        action_request.target.x = 0.1
        cmd2 = self.sendCommandsendCommand(action_request)
 
        if not cmd1 or not cmd2:
            self.error_msgerror_msg('Test B failed: Failed to DriveOnHeading with valid costmap!')
            return not cmd1 or not cmd2
 
        action_request.target.x = 0.5
        cmd_preempt = self.sendAndPreemptWithFasterCommandsendAndPreemptWithFasterCommand(action_request)
        if not cmd_preempt:
            self.error_msgerror_msg('Test B failed: Failed to preempt and invert command!')
            return not cmd_preempt
 
        cmd_cancel = self.sendAndCancelCommandsendAndCancelCommand(action_request)
        if not cmd_cancel:
            self.error_msgerror_msg('Test B failed: Failed to cancel command!')
            return not cmd_cancel
        else:
            self.info_msginfo_msg('Test B passed')
 
        # Test C: Send with impossible command in time allowance & target * signs
        action_request.time_allowance = Duration(seconds=0.1).to_msg()
        cmd3 = self.sendCommandsendCommand(action_request)
        if cmd3:
            self.error_msgerror_msg('Test C failed: Passed while impoossible timing requested!')
            return not cmd3
 
        action_request.target.y = 0.5
        cmd_invalid_target = self.sendCommandsendCommand(action_request)
        if cmd_invalid_target:
            self.error_msgerror_msg('Test C failed: Passed while impoossible target requested!')
            return not cmd_invalid_target
        else:
            action_request.target.y = 0.0
 
        action_request.target.x = -0.5
        cmd_invalid_sign = self.sendCommandsendCommand(action_request)
        if cmd_invalid_sign:
            self.error_msgerror_msg('Test C failed: Passed while impoossible target sign requested!')
            return not cmd_invalid_sign
        else:
            action_request.target.x = 0.5
            self.info_msginfo_msg('Test C passed')
 
        # Test D: Send with lethal costmap and DriveOnHeading
        action_request.time_allowance = Duration(seconds=5).to_msg()
        self.sendOccupiedCostmapsendOccupiedCostmap()
        time.sleep(1)
        cmd4 = self.sendCommandsendCommand(action_request)
        if cmd4:
            self.error_msgerror_msg('Test D failed: Passed while costmap was not lethal!')
            return not cmd4
        else:
            self.info_msginfo_msg('Test D passed')
        return True
 
    def shutdown(self) -> None:
        self.info_msginfo_msg('Shutting down')
 
        self.action_clientaction_client.destroy()
        self.info_msginfo_msg('Destroyed DriveOnHeading 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')
 
    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()
    time.sleep(10)
    test = DriveTest()
    result = test.run()
    test.shutdown()
 
    if not result:
        test.info_msg('Exiting failed')
        exit(1)
    else:
        test.info_msg('Exiting passed')
        exit(0)
 
 
if __name__ == '__main__':
    main()