diff --git a/rosbot_description/urdf/components/vl53lox.urdf.xacro b/rosbot_description/urdf/components/vl53lox.urdf.xacro index 39365688..6518a943 100644 --- a/rosbot_description/urdf/components/vl53lox.urdf.xacro +++ b/rosbot_description/urdf/components/vl53lox.urdf.xacro @@ -5,11 +5,9 @@ - - @@ -26,7 +24,7 @@ - ${gz_control_namespace}range/${prefix} + ${namespace_ext}range/${prefix} ${prefix}_range ${prefix}_range @@ -55,12 +53,9 @@ false - + ${namespace_ext} - - ogre2 - diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index 78ca5b19..97b6d170 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -16,20 +16,20 @@ namespace="$(arg namespace)" /> - + - - + + diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py index fb6723f7..e9c92eec 100644 --- a/rosbot_gazebo/launch/spawn.launch.py +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -17,7 +17,11 @@ IncludeLaunchDescription, DeclareLaunchArgument, ) -from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, PythonExpression +from launch.substitutions import ( + PathJoinSubstitution, + LaunchConfiguration, + PythonExpression, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from nav2_common.launch import ReplaceString from launch_ros.actions import Node, SetParameter diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py index 2c8c58f0..e941c497 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_diff_drive_simulation.py @@ -105,11 +105,25 @@ def test_diff_drive_simulation(): assert ( node.controller_odom_flag ), "ROSbot does not rotate properly. Check rosbot_base_controller!" - assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not rotate properly. Check ekf_filter_node!" flag = node.scan_event.wait(timeout=20.0) assert flag, "ROSbot's lidar does not work properly!" + for i in range(len(node.RANGE_SENSORS_TOPICS)): + flag = node.ranges_events[i].wait(timeout=20.0) + assert ( + flag + ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!" + + flag = node.camera_color_event.wait(timeout=20.0) + assert flag, "ROSbot's camera color image does not work properly!" + + flag = node.camera_points_event.wait(timeout=20.0) + assert flag, "ROSbot's camera point cloud does not work properly!" + finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching # several tests in a row. diff --git a/rosbot_gazebo/test/test_flake8.py b/rosbot_gazebo/test/test_flake8.py index 49c1644f..ee79f31a 100644 --- a/rosbot_gazebo/test/test_flake8.py +++ b/rosbot_gazebo/test/test_flake8.py @@ -20,4 +20,6 @@ @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py index 537288c9..0e51fc49 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_mecanum_simulation.py @@ -118,11 +118,25 @@ def test_mecanum_simulation(): assert ( node.controller_odom_flag ), "ROSbot does not rotate properly. Check rosbot_base_controller!" - assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not rotate properly. Check ekf_filter_node!" flag = node.scan_event.wait(timeout=20.0) assert flag, "ROSbot's lidar does not work properly!" + for i in range(len(node.RANGE_SENSORS_TOPICS)): + flag = node.ranges_events[i].wait(timeout=20.0) + assert ( + flag + ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!" + + flag = node.camera_color_event.wait(timeout=20.0) + assert flag, "ROSbot's camera color image does not work properly!" + + flag = node.camera_points_event.wait(timeout=20.0) + assert flag, "ROSbot's camera point cloud does not work properly!" + finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching # several tests in a row. diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py index 572c5b47..50799df1 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -78,7 +78,9 @@ def test_multirobot_simulation(): nodes.append(node) executor.add_node(node) - ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) + ros_spin_thread = Thread( + target=lambda executor: executor.spin(), args=(executor,) + ) ros_spin_thread.start() for node in nodes: @@ -116,6 +118,18 @@ def test_multirobot_simulation(): flag = node.scan_event.wait(timeout=20.0) assert flag, f"{robot_name}'s lidar does not work properly!" + for i in range(len(node.RANGE_SENSORS_TOPICS)): + flag = node.ranges_events[i].wait(timeout=20.0) + assert ( + flag + ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!" + + flag = node.camera_color_event.wait(timeout=20.0) + assert flag, "ROSbot's camera color image does not work properly!" + + flag = node.camera_points_event.wait(timeout=20.0) + assert flag, "ROSbot's camera point cloud does not work properly!" + node.destroy_node() finally: diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py index 906cb700..2c1b035d 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py @@ -81,7 +81,9 @@ def test_multirobot_mecanum_simulation(): nodes.append(node) executor.add_node(node) - ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) + ros_spin_thread = Thread( + target=lambda executor: executor.spin(), args=(executor,) + ) ros_spin_thread.start() for node in nodes: @@ -136,6 +138,18 @@ def test_multirobot_mecanum_simulation(): flag = node.scan_event.wait(timeout=20.0) assert flag, f"{robot_name}'s lidar does not work properly!" + for i in range(len(node.RANGE_SENSORS_TOPICS)): + flag = node.ranges_events[i].wait(timeout=20.0) + assert ( + flag + ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!" + + flag = node.camera_color_event.wait(timeout=20.0) + assert flag, "ROSbot's camera color image does not work properly!" + + flag = node.camera_points_event.wait(timeout=20.0) + assert flag, "ROSbot's camera point cloud does not work properly!" + node.destroy_node() finally: diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py index 7a757cef..93e36c3a 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py @@ -101,11 +101,25 @@ def test_namespaced_diff_drive_simulation(): assert ( node.controller_odom_flag ), "ROSbot does not rotate properly. Check rosbot_base_controller!" - assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not rotate properly. Check ekf_filter_node!" flag = node.scan_event.wait(timeout=20.0) assert flag, "ROSbot's lidar does not work properly!" + for i in range(len(node.RANGE_SENSORS_TOPICS)): + flag = node.ranges_events[i].wait(timeout=20.0) + assert ( + flag + ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!" + + flag = node.camera_color_event.wait(timeout=20.0) + assert flag, "ROSbot's camera color image does not work properly!" + + flag = node.camera_points_event.wait(timeout=20.0) + assert flag, "ROSbot's camera point cloud does not work properly!" + finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching # several tests in a row. diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py index 92ff2570..ea2d364a 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py @@ -114,11 +114,25 @@ def test_namespaced_mecanum_simulation(): assert ( node.controller_odom_flag ), "ROSbot does not rotate properly. Check rosbot_base_controller!" - assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not rotate properly. Check ekf_filter_node!" flag = node.scan_event.wait(timeout=20.0) assert flag, "ROSbot's lidar does not work properly!" + for i in range(len(node.RANGE_SENSORS_TOPICS)): + flag = node.ranges_events[i].wait(timeout=20.0) + assert ( + flag + ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!" + + flag = node.camera_color_event.wait(timeout=20.0) + assert flag, "ROSbot's camera color image does not work properly!" + + flag = node.camera_points_event.wait(timeout=20.0) + assert flag, "ROSbot's camera point cloud does not work properly!" + finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching # several tests in a row. diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index 91eded67..a9e0c44d 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -22,7 +22,7 @@ from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry -from sensor_msgs.msg import LaserScan +from sensor_msgs.msg import LaserScan, Image, PointCloud2 class SimulationTestNode(Node): @@ -31,11 +31,16 @@ class SimulationTestNode(Node): # cause the rosbot_base_controller to determine inaccurate odometry. ACCURACY = 0.10 # 10% accuracy + RANGE_SENSORS_TOPICS = ["range/fl", "range/fr", "range/rl", "range/rr"] + RANGE_SENSORS_FRAMES = ["fl_range", "fr_range", "rl_range", "rr_range"] + def __init__(self, name="test_node", namespace=None): super().__init__(name, namespace=namespace) # Use simulation time to correct run on slow machine - use_sim_time = rclpy.parameter.Parameter("use_sim_time", rclpy.Parameter.Type.BOOL, True) + use_sim_time = rclpy.parameter.Parameter( + "use_sim_time", rclpy.Parameter.Type.BOOL, True + ) self.set_parameters([use_sim_time]) self.VELOCITY_STABILIZATION_DELAY = 3 @@ -51,6 +56,9 @@ def __init__(self, name="test_node", namespace=None): self.ekf_odom_flag = False self.odom_tf_event = Event() self.scan_event = Event() + self.ranges_events = [Event() for _ in range(len(self.RANGE_SENSORS_TOPICS))] + self.camera_color_event = Event() + self.camera_points_event = Event() self.ros_node_spin_event = Event() def clear_odom_flag(self): @@ -76,16 +84,37 @@ def create_test_subscribers_and_publishers(self): Odometry, "odometry/filtered", self.ekf_callback, 10 ) - self.scan_sub = self.create_subscription(LaserScan, "scan", self.scan_callback, 10) + self.scan_sub = self.create_subscription( + LaserScan, "scan", self.scan_callback, 10 + ) + + self.range_subs = [] + for range_topic_name in self.RANGE_SENSORS_TOPICS: + sub = self.create_subscription( + LaserScan, range_topic_name, self.ranges_callback, 10 + ) + self.range_subs.append(sub) + + self.camera_color_sub = self.create_subscription( + Image, "camera/image", self.camera_image_callback, 10 + ) + + self.camera_points_sub = self.create_subscription( + PointCloud2, "camera/points", self.camera_points_callback, 10 + ) self.timer = self.create_timer(1.0 / 10.0, self.timer_callback) def start_node_thread(self): - self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread = Thread( + target=lambda node: rclpy.spin(node), args=(self,) + ) self.ros_spin_thread.start() def is_twist_ok(self, twist: Twist): - def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, eps=0.01): + def are_close_to_each_other( + current_value, dest_value, tolerance=self.ACCURACY, eps=0.01 + ): acceptable_range = dest_value * tolerance return abs(current_value - dest_value) <= acceptable_range + eps @@ -109,7 +138,10 @@ def timer_callback(self): self.publish_cmd_vel_messages() self.current_time = 1e-9 * self.get_clock().now().nanoseconds - if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY: + if ( + self.current_time + > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY + ): self.vel_stabilization_time_event.set() def scan_callback(self, data: LaserScan): @@ -117,6 +149,19 @@ def scan_callback(self, data: LaserScan): if data.ranges: self.scan_event.set() + def ranges_callback(self, data: LaserScan): + index = self.RANGE_SENSORS_FRAMES.index(data.header.frame_id) + if len(data.ranges) == 1: + self.ranges_events[index].set() + + def camera_image_callback(self, data: Image): + if data.data: + self.camera_color_event.set() + + def camera_points_callback(self, data: PointCloud2): + if data.data: + self.camera_points_event.set() + def publish_cmd_vel_messages(self): twist_msg = Twist()