|
| 1 | +# Software License Agreement (BSD License) |
| 2 | +# |
| 3 | +# Copyright (c) 2024, PickNik Robotics Inc. |
| 4 | +# All rights reserved. |
| 5 | +# |
| 6 | +# Redistribution and use in source and binary forms, with or without |
| 7 | +# modification, are permitted provided that the following conditions |
| 8 | +# are met: |
| 9 | +# |
| 10 | +# # Redistributions of source code must retain the above copyright |
| 11 | +# notice, this list of conditions and the following disclaimer. |
| 12 | +# # Redistributions in binary form must reproduce the above |
| 13 | +# copyright notice, this list of conditions and the following |
| 14 | +# disclaimer in the documentation and/or other materials provided |
| 15 | +# with the distribution. |
| 16 | +# # Neither the name of Willow Garage nor the names of its |
| 17 | +# contributors may be used to endorse or promote products derived |
| 18 | +# from this software without specific prior written permission. |
| 19 | +# |
| 20 | +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 21 | +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 22 | +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
| 23 | +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
| 24 | +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
| 25 | +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
| 26 | +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| 27 | +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| 28 | +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| 29 | +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
| 30 | +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 31 | +# POSSIBILITY OF SUCH DAMAGE. |
| 32 | + |
| 33 | +# Author: Tyler Weaver |
| 34 | + |
| 35 | +import unittest |
| 36 | +from threading import Event |
| 37 | +from threading import Thread |
| 38 | + |
| 39 | +import launch |
| 40 | +import launch_ros |
| 41 | +import launch_testing |
| 42 | +import rclpy |
| 43 | +from rclpy.node import Node |
| 44 | +from rclpy.qos import ( |
| 45 | + QoSProfile, |
| 46 | + QoSReliabilityPolicy, |
| 47 | + QoSDurabilityPolicy, |
| 48 | +) |
| 49 | +from std_msgs.msg import String |
| 50 | + |
| 51 | +import pytest |
| 52 | + |
| 53 | + |
| 54 | +@pytest.mark.launch_test |
| 55 | +def generate_test_description(): |
| 56 | + # dut: device under test |
| 57 | + srdf_publisher = launch_ros.actions.Node( |
| 58 | + package="moveit_ros_planning", |
| 59 | + parameters=[{"robot_description_semantic": "test value"}], |
| 60 | + executable="srdf_publisher", |
| 61 | + output="screen", |
| 62 | + ) |
| 63 | + |
| 64 | + return launch.LaunchDescription( |
| 65 | + [ |
| 66 | + srdf_publisher, |
| 67 | + # Start tests right away - no need to wait for anything |
| 68 | + launch_testing.actions.ReadyToTest(), |
| 69 | + ] |
| 70 | + ), {"srdf_publisher": srdf_publisher} |
| 71 | + |
| 72 | + |
| 73 | +class SrdfObserverNode(Node): |
| 74 | + def __init__(self, name="srdf_observer_node"): |
| 75 | + super().__init__(name) |
| 76 | + self.msg_event_object = Event() |
| 77 | + self.srdf = "" |
| 78 | + |
| 79 | + def start_subscriber(self): |
| 80 | + # Create a subscriber |
| 81 | + self.subscription = self.create_subscription( |
| 82 | + String, |
| 83 | + "robot_description_semantic", |
| 84 | + self.subscriber_callback, |
| 85 | + qos_profile=QoSProfile( |
| 86 | + reliability=QoSReliabilityPolicy.RELIABLE, |
| 87 | + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, |
| 88 | + depth=1, |
| 89 | + ), |
| 90 | + ) |
| 91 | + |
| 92 | + # Add a spin thread |
| 93 | + self.ros_spin_thread = Thread( |
| 94 | + target=lambda node: rclpy.spin(node), args=(self,) |
| 95 | + ) |
| 96 | + self.ros_spin_thread.start() |
| 97 | + |
| 98 | + def subscriber_callback(self, msg): |
| 99 | + self.srdf = msg.data |
| 100 | + self.msg_event_object.set() |
| 101 | + |
| 102 | + |
| 103 | +# These tests will run concurrently with the dut process. After all these tests are done, |
| 104 | +# the launch system will shut down the processes that it started up |
| 105 | +class TestFixture(unittest.TestCase): |
| 106 | + def test_rosout_msgs_published(self, proc_output): |
| 107 | + rclpy.init() |
| 108 | + try: |
| 109 | + node = SrdfObserverNode() |
| 110 | + node.start_subscriber() |
| 111 | + msgs_received_flag = node.msg_event_object.wait(timeout=5.0) |
| 112 | + assert ( |
| 113 | + msgs_received_flag |
| 114 | + ), "Did not receive message on `/robot_description_semantic`!" |
| 115 | + assert node.srdf == "test value" |
| 116 | + finally: |
| 117 | + rclpy.shutdown() |
| 118 | + |
| 119 | + |
| 120 | +@launch_testing.post_shutdown_test() |
| 121 | +class TestProcessOutput(unittest.TestCase): |
| 122 | + def test_exit_code(self, proc_info): |
| 123 | + # Check that all processes in the launch (in this case, there's just one) exit |
| 124 | + # with code 0 |
| 125 | + launch_testing.asserts.assertExitCodes(proc_info) |
0 commit comments