From 2213a81e538eba9cd531ec7fd1b5fdbbff6a8567 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 13 Nov 2023 10:06:29 +0100 Subject: [PATCH 01/79] added namespaces to ros2_control Signed-off-by: Jakub Delicat --- .../config/diff_drive_controller.yaml | 8 +-- rosbot_controller/launch/controller.launch.py | 58 ++++++++++++++++--- 2 files changed, 53 insertions(+), 13 deletions(-) diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index f69481cd..d5d1a85e 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -4,7 +4,7 @@ simulation_ignition_ros_control: # Separate controller manager used for simulation - only difference is # the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) -simulation_controller_manager: +/**/simulation_controller_manager: ros__parameters: use_sim_time: true update_rate: 20 # Hz @@ -17,7 +17,7 @@ simulation_controller_manager: type: imu_sensor_broadcaster/IMUSensorBroadcaster -controller_manager: +/**/controller_manager: ros__parameters: use_sim_time: false update_rate: 20 # Hz @@ -29,7 +29,7 @@ controller_manager: imu_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster -imu_broadcaster: +/**/imu_broadcaster: ros__parameters: sensor_name: imu frame_id: imu_link @@ -37,7 +37,7 @@ imu_broadcaster: static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally -rosbot_base_controller: +/**/rosbot_base_controller: ros__parameters: left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index f6382f6b..e46ca020 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -14,7 +14,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import RegisterEventHandler, DeclareLaunchArgument +from launch.actions import RegisterEventHandler, DeclareLaunchArgument, GroupAction from launch.conditions import UnlessCondition from launch.event_handlers import OnProcessExit from launch.substitutions import ( @@ -30,6 +30,13 @@ def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", @@ -69,13 +76,24 @@ def generate_launch_description(): ] ) - controller_manager_name = PythonExpression( + controller_manager_type_name = PythonExpression( [ - "'/simulation_controller_manager' if ", + "'simulation_controller_manager' if ", use_sim, - " else '/controller_manager'", + " else 'controller_manager'", ] ) + controller_manager_name = LaunchConfiguration("controller_manager_name") + namespace_for_controller_name = PythonExpression( + ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"] + ) + + namespace_for_controller_name + declare_controller_manager_name_arg = DeclareLaunchArgument( + "controller_manager_name", + default_value=[namespace_for_controller_name, controller_manager_type_name], + description="ros2_control controller manager name", + ) # Get URDF via xacro robot_description_content = Command( @@ -114,12 +132,13 @@ def generate_launch_description(): executable="ros2_control_node", parameters=[robot_description, robot_controllers], remappings=[ - ("/imu_sensor_node/imu", "/_imu/data_raw"), + ("imu_sensor_node/imu", "/_imu/data_raw"), ("~/motors_cmd", "/_motors_cmd"), ("~/motors_response", "/_motors_response"), - ("/rosbot_base_controller/cmd_vel_unstamped", "/cmd_vel"), + ("rosbot_base_controller/cmd_vel_unstamped", "cmd_vel"), ], condition=UnlessCondition(use_sim), + namespace=namespace, ) robot_state_pub_node = Node( @@ -137,6 +156,8 @@ def generate_launch_description(): controller_manager_name, "--controller-manager-timeout", "120", + "--namespace", + namespace, ], ) @@ -149,6 +170,8 @@ def generate_launch_description(): controller_manager_name, "--controller-manager-timeout", "120", + "--namespace", + namespace, ], ) @@ -169,6 +192,8 @@ def generate_launch_description(): controller_manager_name, "--controller-manager-timeout", "120", + "--namespace", + namespace, ], ) @@ -181,12 +206,16 @@ def generate_launch_description(): ) ) - actions = [ + args_declarations_actions = [ + declare_namespace_arg, declare_mecanum_arg, declare_use_sim_arg, declare_use_gpu_arg, declare_simulation_engine_arg, - SetParameter(name="use_sim_time", value=use_sim), + declare_controller_manager_name_arg, + ] + + nodes_actions = [ control_node, robot_state_pub_node, joint_state_broadcaster_spawner, @@ -194,4 +223,15 @@ def generate_launch_description(): delay_imu_broadcaster_spawner_after_robot_controller_spawner, ] - return LaunchDescription(actions) + return LaunchDescription( + args_declarations_actions + + [ + GroupAction( + actions=[ + # PushRosNamespace(namespace), + SetParameter(name="use_sim_time", value=use_sim), + ] + + nodes_actions + ) + ] + ) From 2b479a6d02e520134b175d6b1f92b6d23ee9603b Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 13 Nov 2023 10:14:49 +0100 Subject: [PATCH 02/79] removed action group Signed-off-by: Jakub Delicat --- rosbot_controller/launch/controller.launch.py | 44 +++++++------------ 1 file changed, 16 insertions(+), 28 deletions(-) diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index e46ca020..8f809158 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -14,7 +14,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import RegisterEventHandler, DeclareLaunchArgument, GroupAction +from launch.actions import RegisterEventHandler, DeclareLaunchArgument from launch.conditions import UnlessCondition from launch.event_handlers import OnProcessExit from launch.substitutions import ( @@ -83,12 +83,12 @@ def generate_launch_description(): " else 'controller_manager'", ] ) + controller_manager_name = LaunchConfiguration("controller_manager_name") namespace_for_controller_name = PythonExpression( ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"] ) - namespace_for_controller_name declare_controller_manager_name_arg = DeclareLaunchArgument( "controller_manager_name", default_value=[namespace_for_controller_name, controller_manager_type_name], @@ -145,6 +145,7 @@ def generate_launch_description(): package="robot_state_publisher", executable="robot_state_publisher", parameters=[robot_description], + namespace=namespace, ) joint_state_broadcaster_spawner = Node( @@ -206,32 +207,19 @@ def generate_launch_description(): ) ) - args_declarations_actions = [ - declare_namespace_arg, - declare_mecanum_arg, - declare_use_sim_arg, - declare_use_gpu_arg, - declare_simulation_engine_arg, - declare_controller_manager_name_arg, - ] - - nodes_actions = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - delay_imu_broadcaster_spawner_after_robot_controller_spawner, - ] - return LaunchDescription( - args_declarations_actions - + [ - GroupAction( - actions=[ - # PushRosNamespace(namespace), - SetParameter(name="use_sim_time", value=use_sim), - ] - + nodes_actions - ) + [ + declare_namespace_arg, + declare_mecanum_arg, + declare_use_sim_arg, + declare_use_gpu_arg, + declare_simulation_engine_arg, + declare_controller_manager_name_arg, + SetParameter("use_sim_time"), + control_node, + robot_state_pub_node, + joint_state_broadcaster_spawner, + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + delay_imu_broadcaster_spawner_after_robot_controller_spawner, ] ) From ae14e26159f229fecf40fca6b273e87cdb4f1709 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 13 Nov 2023 11:55:28 +0100 Subject: [PATCH 03/79] Added namespace to robot description Signed-off-by: Jakub Delicat --- .wordlist.txt | 1 + rosbot_controller/launch/controller.launch.py | 4 +- rosbot_description/urdf/body.urdf.xacro | 55 +++++++++------- .../urdf/components/vl53lox.urdf.xacro | 25 +++++--- rosbot_description/urdf/rosbot.urdf.xacro | 26 ++++++-- .../urdf/rosbot_macro.urdf.xacro | 64 +++++++++++-------- rosbot_description/urdf/wheel.urdf.xacro | 29 +++++---- 7 files changed, 126 insertions(+), 78 deletions(-) diff --git a/.wordlist.txt b/.wordlist.txt index 5a21da62..a6979606 100644 --- a/.wordlist.txt +++ b/.wordlist.txt @@ -120,3 +120,4 @@ Palacios env pids pgrep +namespace diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index 8f809158..a507e909 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -115,6 +115,8 @@ def generate_launch_description(): use_gpu, " simulation_engine:=", simulation_engine, + " tf_prefix:=", + namespace, ] ) robot_description = {"robot_description": robot_description_content} @@ -215,7 +217,7 @@ def generate_launch_description(): declare_use_gpu_arg, declare_simulation_engine_arg, declare_controller_manager_name_arg, - SetParameter("use_sim_time"), + SetParameter("use_sim_time", value=use_sim), control_node, robot_state_pub_node, joint_state_broadcaster_spawner, diff --git a/rosbot_description/urdf/body.urdf.xacro b/rosbot_description/urdf/body.urdf.xacro index 53505b58..65535519 100644 --- a/rosbot_description/urdf/body.urdf.xacro +++ b/rosbot_description/urdf/body.urdf.xacro @@ -1,18 +1,25 @@ - + - + + + + + + - + + + - - + + - + @@ -38,17 +45,17 @@ - + Gazebo/White - + - - + + - + @@ -58,31 +65,31 @@ - + Gazebo/Red - + - - + + - + - + - - + + - + - - - - + + + + diff --git a/rosbot_description/urdf/components/vl53lox.urdf.xacro b/rosbot_description/urdf/components/vl53lox.urdf.xacro index bcbe6ec6..a0b4891e 100644 --- a/rosbot_description/urdf/components/vl53lox.urdf.xacro +++ b/rosbot_description/urdf/components/vl53lox.urdf.xacro @@ -1,25 +1,32 @@ - + - + + + + + + + + - + - + - - + + - /range/${prefix} - ${prefix}_range - ${prefix}_range + /range/${side} + ${tf_prefix_ext}${side}_range + ${tf_prefix_ext}${side}_range 5.0 diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index d3b27224..c3dabd65 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -5,23 +5,39 @@ + + + + + + + + + - + + simulation_engine="$(arg simulation_engine)" + tf_prefix="$(arg tf_prefix)" /> + simulation_engine="$(arg simulation_engine)" + tf_prefix="$(arg tf_prefix)" /> diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 4f3d8613..ea3d3ca0 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -1,6 +1,6 @@ - + @@ -9,18 +9,26 @@ + + + + + + + + - + - - - - + + + + @@ -54,10 +62,10 @@ - rr_wheel_joint, - rl_wheel_joint, - fr_wheel_joint, - fl_wheel_joint + ${tf_prefix_ext}rr_wheel_joint, + ${tf_prefix_ext}rl_wheel_joint, + ${tf_prefix_ext}fr_wheel_joint, + ${tf_prefix_ext}fl_wheel_joint @@ -71,22 +79,22 @@ - + - + - + - + @@ -127,7 +135,7 @@ - + true @@ -135,8 +143,8 @@ imu/data_raw false false - imu_link - imu_link + ${tf_prefix_ext}imu_link + ${tf_prefix_ext}imu_link @@ -149,10 +157,10 @@ 10.0 2 - fl_wheel_joint - fr_wheel_joint - rl_wheel_joint - rr_wheel_joint + ${tf_prefix_ext}fl_wheel_joint + ${tf_prefix_ext}fr_wheel_joint + ${tf_prefix_ext}rl_wheel_joint + ${tf_prefix_ext}rr_wheel_joint 0.192 0.192 ${wheel_radius*2.0} @@ -162,22 +170,22 @@ true true true - odom - base_link + ${tf_prefix_ext}odom + ${tf_prefix_ext}base_link - fl_wheel_joint - fr_wheel_joint - rl_wheel_joint - rr_wheel_joint + ${tf_prefix_ext}fl_wheel_joint + ${tf_prefix_ext}fr_wheel_joint + ${tf_prefix_ext}rl_wheel_joint + ${tf_prefix_ext}rr_wheel_joint - + true 25 diff --git a/rosbot_description/urdf/wheel.urdf.xacro b/rosbot_description/urdf/wheel.urdf.xacro index 5fe9e7c2..47af25e6 100644 --- a/rosbot_description/urdf/wheel.urdf.xacro +++ b/rosbot_description/urdf/wheel.urdf.xacro @@ -1,7 +1,14 @@ - + + + + + + + + @@ -29,28 +36,28 @@ - + - + - + - + @@ -58,16 +65,16 @@ - - - + + + - + @@ -90,14 +97,14 @@ - + 1.0 0.0 - ${fdir} + ${fdir} From 2b37af8c2e237d33958db4fe3904a64ea78f9de1 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 13 Nov 2023 13:08:03 +0100 Subject: [PATCH 04/79] added tests for namespaces in rosbot_controller Signed-off-by: Jakub Delicat --- .../test/test_diff_drive_controllers.py | 3 +- .../test/test_mecanum_controllers.py | 3 +- rosbot_controller/test/test_utils.py | 16 +++++---- rosbot_controller/test/test_xacro.py | 33 +++++++++++++++++-- 4 files changed, 43 insertions(+), 12 deletions(-) diff --git a/rosbot_controller/test/test_diff_drive_controllers.py b/rosbot_controller/test/test_diff_drive_controllers.py index 3b69459c..9deea532 100644 --- a/rosbot_controller/test/test_diff_drive_controllers.py +++ b/rosbot_controller/test/test_diff_drive_controllers.py @@ -43,6 +43,7 @@ def generate_test_description(): "use_sim": "False", "mecanum": "False", "use_gpu": "False", + "namespace": "rosbot2r", }.items(), ) @@ -53,7 +54,7 @@ def generate_test_description(): def test_controllers_startup_fail(): rclpy.init() try: - node = ControllersTestNode("test_controllers_bringup") + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") node.create_test_subscribers_and_publishers() node.start_node_thread() diff --git a/rosbot_controller/test/test_mecanum_controllers.py b/rosbot_controller/test/test_mecanum_controllers.py index 5f2607e3..b2968e90 100644 --- a/rosbot_controller/test/test_mecanum_controllers.py +++ b/rosbot_controller/test/test_mecanum_controllers.py @@ -43,6 +43,7 @@ def generate_test_description(): "use_sim": "False", "mecanum": "True", "use_gpu": "False", + "namespace": "rosbot2r", }.items(), ) @@ -53,7 +54,7 @@ def generate_test_description(): def test_controllers_startup_fail(): rclpy.init() try: - node = ControllersTestNode("test_controllers_bringup") + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") node.create_test_subscribers_and_publishers() node.start_node_thread() diff --git a/rosbot_controller/test/test_utils.py b/rosbot_controller/test/test_utils.py index db48c06a..831eadad 100644 --- a/rosbot_controller/test/test_utils.py +++ b/rosbot_controller/test/test_utils.py @@ -29,26 +29,27 @@ class ControllersTestNode(Node): __test__ = False - def __init__(self, name="test_node"): - super().__init__(name) + def __init__(self, name="test_node", namespace=None): + super().__init__(name, namespace=namespace) self.joint_state_msg_event = Event() self.odom_msg_event = Event() self.imu_msg_event = Event() def create_test_subscribers_and_publishers(self): self.joint_state_sub = self.create_subscription( - JointState, "/joint_states", self.joint_states_callback, 10 + JointState, "joint_states", self.joint_states_callback, 10 ) self.odom_sub = self.create_subscription( - Odometry, "/rosbot_base_controller/odom", self.odometry_callback, 10 + Odometry, "rosbot_base_controller/odom", self.odometry_callback, 10 ) - self.imu_sub = self.create_subscription(Imu, "/imu_broadcaster/imu", self.imu_callback, 10) + self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10) - self.imu_publisher = self.create_publisher(Imu, "_imu/data_raw", 10) + # TODO: @delipl namespaces have not been implemented in microros yet + self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10) - self.joint_states_publisher = self.create_publisher(JointState, "_motors_response", 10) + self.joint_states_publisher = self.create_publisher(JointState, "/_motors_response", 10) self.timer = None @@ -71,6 +72,7 @@ def start_publishing_fake_hardware(self): self.publish_fake_hardware_messages, ) + # TODO: @delipl namespaces have not been implemented in microros yet def publish_fake_hardware_messages(self): imu_msg = Imu() imu_msg.header.stamp = self.get_clock().now().to_msg() diff --git a/rosbot_controller/test/test_xacro.py b/rosbot_controller/test/test_xacro.py index 17216c75..95e46489 100644 --- a/rosbot_controller/test/test_xacro.py +++ b/rosbot_controller/test/test_xacro.py @@ -23,23 +23,50 @@ def test_rosbot_description_parsing(): use_sim_values = ["true", "false"] use_gpu_values = ["true", "false"] simulation_engine_values = ["ignition-gazebo", "webots"] # 'gazebo-classic' + tf_prefixes = ["rosbot2r", "rosbot2pro", "husarion", "", "None"] all_combinations = list( - itertools.product(mecanum_values, use_sim_values, use_gpu_values, simulation_engine_values) + itertools.product( + mecanum_values, use_sim_values, use_gpu_values, simulation_engine_values, tf_prefixes + ) ) for combination in all_combinations: - mecanum, use_sim, use_gpu, simulation_engine = combination + mecanum, use_sim, use_gpu, simulation_engine, tf_prefix = combination mappings = { "mecanum": mecanum, "use_sim": use_sim, "use_gpu": use_gpu, "simulation_engine": simulation_engine, + "tf_prefix": tf_prefix, } rosbot_description = get_package_share_directory("rosbot_description") xacro_path = os.path.join(rosbot_description, "urdf/rosbot.urdf.xacro") try: - xacro.process_file(xacro_path, mappings=mappings) + urdf = xacro.process_file(xacro_path, mappings=mappings) + + namespace = tf_prefix + + links = urdf.getElementsByTagName("link") + for link in links: + link_name = link.getAttribute("name") + if namespace != "None": + if tf_prefix not in link_name: + assert False, f"Link name '{link_name}' does not contain '{namespace}'" + else: + if tf_prefix in link_name: + assert False, f"Link name '{link_name}' does not contain '{namespace}'" + + joints = urdf.getElementsByTagName("joint") + for joint in joints: + joint_name = joint.getAttribute("name") + if namespace != "None": + if tf_prefix not in joint_name: + assert False, f"Joint name '{joint_name}' does not contain '{namespace}'" + else: + if tf_prefix in joint_name: + assert False, f"Joint name '{joint_name}' does not contain '{namespace}'" + except xacro.XacroException as e: assert False, ( f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, use_sim:" From 72bef343df92d3a8afe504939d4b5c848ad5b4b0 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 13 Nov 2023 13:10:59 +0100 Subject: [PATCH 05/79] update worldlist Signed-off-by: Jakub Delicat --- .wordlist.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.wordlist.txt b/.wordlist.txt index a6979606..5600e10d 100644 --- a/.wordlist.txt +++ b/.wordlist.txt @@ -121,3 +121,7 @@ env pids pgrep namespace +TODO +delipl +microros +namespaces From 0d82f27fbf75a1866a643ce83a8fe1d5a5762c74 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 13 Nov 2023 16:06:41 +0100 Subject: [PATCH 06/79] fixed ros2_control namespaces | added namespace to ekf Signed-off-by: Jakub Delicat --- rosbot_bringup/config/ekf.yaml | 9 +++-- rosbot_bringup/launch/bringup.launch.py | 39 ++++++++++++++++++- .../config/diff_drive_controller.yaml | 4 +- rosbot_controller/launch/controller.launch.py | 32 +++++++++++---- .../urdf/rosbot_macro.urdf.xacro | 18 +++++---- rosbot_description/urdf/wheel.urdf.xacro | 1 + 6 files changed, 82 insertions(+), 21 deletions(-) diff --git a/rosbot_bringup/config/ekf.yaml b/rosbot_bringup/config/ekf.yaml index db48dcc7..4934c896 100644 --- a/rosbot_bringup/config/ekf.yaml +++ b/rosbot_bringup/config/ekf.yaml @@ -1,7 +1,7 @@ # Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html ## ekf config file ### -ekf_filter_node: +/**/ekf_filter_node: ros__parameters: frequency: 25.0 sensor_timeout: 0.05 @@ -10,6 +10,7 @@ ekf_filter_node: transform_time_offset: 0.0 transform_timeout: 0.05 + # All transforms are overwritten in launch file due to the namespace argument map_frame: map odom_frame: odom base_link_frame: base_link @@ -17,7 +18,8 @@ ekf_filter_node: publish_tf: true publish_acceleration: false - odom0: /rosbot_base_controller/odom + # The odometry topic name is overwritten in launch file due to the namespace argument + odom0: rosbot_base_controller/odom odom0_config: [false, false, false, # X , Y , Z false, false, false, # roll , pitch ,yaw true, true, false, # dX , dY , dZ @@ -29,7 +31,8 @@ ekf_filter_node: odom0_differential: false odom0_relative: true - imu0: /imu_broadcaster/imu + # The IMU topic name is overwritten in launch file due to the namespace argument + imu0: imu_broadcaster/imu imu0_config: [false, false, false, # X , Y , Z false, false, true, # roll , pitch ,yaw false, false, false, # dX , dY , dZ diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index 30b85f65..a5c06c6d 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -17,10 +17,20 @@ from ament_index_python.packages import get_package_share_directory from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + namespace_tf_prefix = PythonExpression( + ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "_'"] + ) + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + use_sim = LaunchConfiguration("use_sim") declare_use_sim_arg = DeclareLaunchArgument( "use_sim", @@ -70,6 +80,7 @@ def generate_launch_description(): "mecanum": mecanum, "use_gpu": use_gpu, "simulation_engine": simulation_engine, + "namespace": namespace, }.items(), ) @@ -80,10 +91,34 @@ def generate_launch_description(): executable="ekf_node", name="ekf_filter_node", output="screen", - parameters=[ekf_config], + parameters=[ + ekf_config, + { + "map_frame": LaunchConfiguration( + "ekf_map_frame", default=[namespace_tf_prefix, "map"] + ) + }, + { + "odom_frame": LaunchConfiguration( + "ekf_odom_frame", default=[namespace_tf_prefix, "odom"] + ) + }, + { + "base_link_frame": LaunchConfiguration( + "ekf_base_link_frame", default=[namespace_tf_prefix, "base_link"] + ) + }, + { + "world_frame": LaunchConfiguration( + "ekf_world_frame", default=[namespace_tf_prefix, "odom"] + ) + }, + ], + namespace=namespace, ) actions = [ + declare_namespace_arg, declare_mecanum_arg, declare_use_sim_arg, declare_use_gpu_arg, diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index d5d1a85e..590ad36c 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -32,13 +32,15 @@ simulation_ignition_ros_control: /**/imu_broadcaster: ros__parameters: sensor_name: imu - frame_id: imu_link + # All transforms are overwritten in launch file due to the namespace argument + # frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally /**/rosbot_base_controller: ros__parameters: + tf_frame_prefix_enable: True left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index a507e909..cd461240 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -84,15 +84,12 @@ def generate_launch_description(): ] ) - controller_manager_name = LaunchConfiguration("controller_manager_name") namespace_for_controller_name = PythonExpression( ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"] ) - - declare_controller_manager_name_arg = DeclareLaunchArgument( + controller_manager_name = LaunchConfiguration( "controller_manager_name", - default_value=[namespace_for_controller_name, controller_manager_type_name], - description="ros2_control controller manager name", + default=[namespace_for_controller_name, controller_manager_type_name], ) # Get URDF via xacro @@ -132,7 +129,29 @@ def generate_launch_description(): control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, robot_controllers], + parameters=[ + robot_description, + robot_controllers, + # imu_broadcaster frame_id override + {"frame_id": LaunchConfiguration("imu_frame", default=[namespace, "_imu_link"])}, + { + "tf_frame_prefix": LaunchConfiguration( + "diff_drive_tf_frame_prefix", default=[namespace] + ) + }, + { + "left_wheel_names": LaunchConfiguration( + "left_wheels_joints", + default=["[", namespace, "_fl_wheel_joint,", namespace, "_rl_wheel_joint]"], + ) + }, + { + "right_wheel_names": LaunchConfiguration( + "right_wheels_joints", + default=["[", namespace, "_fr_wheel_joint,", namespace, "_rr_wheel_joint]"], + ) + }, + ], remappings=[ ("imu_sensor_node/imu", "/_imu/data_raw"), ("~/motors_cmd", "/_motors_cmd"), @@ -216,7 +235,6 @@ def generate_launch_description(): declare_use_sim_arg, declare_use_gpu_arg, declare_simulation_engine_arg, - declare_controller_manager_name_arg, SetParameter("use_sim_time", value=use_sim), control_node, robot_state_pub_node, diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index ea3d3ca0..2f155cc8 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -60,12 +60,13 @@ 120000 500 + - ${tf_prefix_ext}rr_wheel_joint, - ${tf_prefix_ext}rl_wheel_joint, - ${tf_prefix_ext}fr_wheel_joint, - ${tf_prefix_ext}fl_wheel_joint + rr_wheel_joint, + rl_wheel_joint, + fr_wheel_joint, + fl_wheel_joint @@ -79,22 +80,23 @@ - + + - + - + - + diff --git a/rosbot_description/urdf/wheel.urdf.xacro b/rosbot_description/urdf/wheel.urdf.xacro index 47af25e6..1ab02bbe 100644 --- a/rosbot_description/urdf/wheel.urdf.xacro +++ b/rosbot_description/urdf/wheel.urdf.xacro @@ -65,6 +65,7 @@ + From ad2ef7c646ea82eaafe5dbadf35b8a5ee2171de6 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 13 Nov 2023 21:23:05 +0100 Subject: [PATCH 07/79] added mecanum Signed-off-by: Jakub Delicat --- rosbot/rosbot_hardware.repos | 11 +++++++++-- .../config/mecanum_drive_controller.yaml | 14 ++++++++------ rosbot_controller/launch/controller.launch.py | 14 +------------- rosbot_description/urdf/rosbot_macro.urdf.xacro | 9 +++++---- 4 files changed, 23 insertions(+), 25 deletions(-) diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index 27f4a344..c204dd2c 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -2,7 +2,7 @@ repositories: rosbot_hardware_interfaces: type: git url: https://github.com/husarion/rosbot_hardware_interfaces.git - version: main + version: add-namespace ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git @@ -10,4 +10,11 @@ repositories: rosbot_controllers: type: git url: https://github.com/husarion/rosbot_controllers - version: main + version: add-namespace + # Waiting for backporting to the humble version + # https://github.com/ros-controls/ros2_controllers/pull/726 and + # https://github.com/ros-controls/ros2_controllers/pull/831 + ros2_controllers: + type: git + url: https://github.com/delihus/ros2_controllers + version: humble-backport diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index 56c56c94..074ee25d 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -1,4 +1,4 @@ -controller_manager: +/**/controller_manager: ros__parameters: use_sim_time: false update_rate: 20 # Hz @@ -12,7 +12,7 @@ controller_manager: # Separate controller manager used for simulation - only difference is # the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) -simulation_controller_manager: +/**/simulation_controller_manager: ros__parameters: use_sim_time: true update_rate: 20 # Hz @@ -24,16 +24,18 @@ simulation_controller_manager: imu_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster -imu_broadcaster: +/**/imu_broadcaster: ros__parameters: - sensor_name: "imu" - frame_id: "imu_link" + sensor_name: imu + # All transforms are overwritten in launch file due to the namespace argument + # frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive -rosbot_base_controller: +/**/rosbot_base_controller: ros__parameters: + tf_frame_prefix_enable: True front_left_wheel_name: fl_wheel_joint front_right_wheel_name: fr_wheel_joint rear_left_wheel_name: rl_wheel_joint diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index cd461240..c2be0083 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -136,19 +136,7 @@ def generate_launch_description(): {"frame_id": LaunchConfiguration("imu_frame", default=[namespace, "_imu_link"])}, { "tf_frame_prefix": LaunchConfiguration( - "diff_drive_tf_frame_prefix", default=[namespace] - ) - }, - { - "left_wheel_names": LaunchConfiguration( - "left_wheels_joints", - default=["[", namespace, "_fl_wheel_joint,", namespace, "_rl_wheel_joint]"], - ) - }, - { - "right_wheel_names": LaunchConfiguration( - "right_wheels_joints", - default=["[", namespace, "_fr_wheel_joint,", namespace, "_rr_wheel_joint]"], + "rosbot_base_tf_frame_prefix", default=[namespace] ) }, ], diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 2f155cc8..5272ee47 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -68,6 +68,7 @@ fr_wheel_joint, fl_wheel_joint + ${tf_prefix_ext} @@ -81,22 +82,22 @@ - + - + - + - + From 64952ce93dbb5ccb794c10695ba1913c1156d77a Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 13 Nov 2023 21:28:52 +0100 Subject: [PATCH 08/79] added ros2_controllers to .gitignore Signed-off-by: Jakub Delicat --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index f044df5f..33aa2f87 100644 --- a/.gitignore +++ b/.gitignore @@ -12,6 +12,7 @@ ros_components_description/ rosbot_controllers/ husarion/husarion_office_gz gazebosim/gz_ros2_control +ros2_controllers # pyspelling *.dic From 9a3e90d0d616b4e50da5cf23c2de8c282663dc17 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 14 Nov 2023 08:06:48 +0100 Subject: [PATCH 09/79] ignored ros2_controllers during Industrial CI Signed-off-by: Jakub Delicat --- .github/workflows/industrial_ci.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index ad5f6cf1..c3a83c0d 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -39,6 +39,7 @@ jobs: - name: Clone installation requirements shell: bash run: python3 -m pip install -U vcstool && vcs import . < ./rosbot/rosbot_hardware.repos && vcs import . < ./rosbot/rosbot_simulation.repos + && touch ros2_controllers/COLCON_IGNORE - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: ${{matrix.ROS_DISTRO}} From 539cd8981514c68f15f2e2fb7c158940a0c1005f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 14 Nov 2023 08:48:59 +0100 Subject: [PATCH 10/79] fixed tests Signed-off-by: Jakub Delicat --- rosbot_controller/test/test_diff_drive_controllers.py | 8 ++++---- rosbot_controller/test/test_mecanum_controllers.py | 8 ++++---- rosbot_controller/test/test_utils.py | 8 +++++--- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/rosbot_controller/test/test_diff_drive_controllers.py b/rosbot_controller/test/test_diff_drive_controllers.py index 9deea532..ed4f0b2a 100644 --- a/rosbot_controller/test/test_diff_drive_controllers.py +++ b/rosbot_controller/test/test_diff_drive_controllers.py @@ -54,21 +54,21 @@ def generate_test_description(): def test_controllers_startup_fail(): rclpy.init() try: - node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node = ControllersTestNode("test_controllers_bringup") node.create_test_subscribers_and_publishers() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + msgs_received_flag = node.joint_state_msg_event.wait(timeout=20.0) assert not msgs_received_flag, ( "Received JointStates message that should not have appeared. Check whether other" " robots are connected to your network.!" ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + msgs_received_flag = node.odom_msg_event.wait(timeout=20.0) assert not msgs_received_flag, ( "Received Odom message that should not have appeared. Check whether other robots are" " connected to your network.!" ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + msgs_received_flag = node.imu_msg_event.wait(timeout=20.0) assert not msgs_received_flag, ( "Received Imu message that should not have appeared. Check whether other robots are" " connected to your network.!" diff --git a/rosbot_controller/test/test_mecanum_controllers.py b/rosbot_controller/test/test_mecanum_controllers.py index b2968e90..9a2b8861 100644 --- a/rosbot_controller/test/test_mecanum_controllers.py +++ b/rosbot_controller/test/test_mecanum_controllers.py @@ -54,21 +54,21 @@ def generate_test_description(): def test_controllers_startup_fail(): rclpy.init() try: - node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node = ControllersTestNode("test_controllers_bringup") node.create_test_subscribers_and_publishers() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + msgs_received_flag = node.joint_state_msg_event.wait(timeout=20.0) assert not msgs_received_flag, ( "Received JointStates message that should not have appeared. Check whether other" " robots are connected to your network.!" ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + msgs_received_flag = node.odom_msg_event.wait(timeout=20.0) assert not msgs_received_flag, ( "Received Odom message that should not have appeared. Check whether other robots are" " connected to your network.!" ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + msgs_received_flag = node.imu_msg_event.wait(timeout=20.0) assert not msgs_received_flag, ( "Received Imu message that should not have appeared. Check whether other robots are" " connected to your network.!" diff --git a/rosbot_controller/test/test_utils.py b/rosbot_controller/test/test_utils.py index 831eadad..79e3372b 100644 --- a/rosbot_controller/test/test_utils.py +++ b/rosbot_controller/test/test_utils.py @@ -37,14 +37,16 @@ def __init__(self, name="test_node", namespace=None): def create_test_subscribers_and_publishers(self): self.joint_state_sub = self.create_subscription( - JointState, "joint_states", self.joint_states_callback, 10 + JointState, "/rosbot2r/joint_states", self.joint_states_callback, 10 ) self.odom_sub = self.create_subscription( - Odometry, "rosbot_base_controller/odom", self.odometry_callback, 10 + Odometry, "/rosbot2r/rosbot_base_controller/odom", self.odometry_callback, 10 ) - self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10) + self.imu_sub = self.create_subscription( + Imu, "/rosbot2r/imu_broadcaster/imu", self.imu_callback, 10 + ) # TODO: @delipl namespaces have not been implemented in microros yet self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10) From 976a6da62727b94d71d2a2620d2c1f04fe48e74f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 14 Nov 2023 10:09:32 +0100 Subject: [PATCH 11/79] added tf_prefix to imu_sensor_broadcaster Signed-off-by: Jakub Delicat --- rosbot_controller/config/diff_drive_controller.yaml | 4 ++-- rosbot_controller/config/mecanum_drive_controller.yaml | 4 ++-- rosbot_controller/launch/controller.launch.py | 9 ++++++++- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index 590ad36c..36512202 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -32,8 +32,8 @@ simulation_ignition_ros_control: /**/imu_broadcaster: ros__parameters: sensor_name: imu - # All transforms are overwritten in launch file due to the namespace argument - # frame_id: imu_link + tf_frame_prefix_enable: True + frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index 074ee25d..81148dc5 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -27,8 +27,8 @@ /**/imu_broadcaster: ros__parameters: sensor_name: imu - # All transforms are overwritten in launch file due to the namespace argument - # frame_id: imu_link + tf_frame_prefix_enable: True + frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index c2be0083..f429f78d 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -36,6 +36,9 @@ def generate_launch_description(): default_value="", description="Namespace for all topics and tfs", ) + namespace_tf_prefix = PythonExpression( + ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "_'"] + ) mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( @@ -133,7 +136,11 @@ def generate_launch_description(): robot_description, robot_controllers, # imu_broadcaster frame_id override - {"frame_id": LaunchConfiguration("imu_frame", default=[namespace, "_imu_link"])}, + { + "frame_id": LaunchConfiguration( + "imu_frame", default=[namespace_tf_prefix, "imu_link"] + ) + }, { "tf_frame_prefix": LaunchConfiguration( "rosbot_base_tf_frame_prefix", default=[namespace] From d0274a69de0069d7350d8373632470a86606219e Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 14 Nov 2023 10:57:35 +0100 Subject: [PATCH 12/79] copied only diff_drive and imu_broadcaster Signed-off-by: Jakub Delicat --- .github/workflows/industrial_ci.yaml | 4 +++- rosbot_controller/launch/controller.launch.py | 9 --------- 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index c3a83c0d..4b4b1b79 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -39,7 +39,9 @@ jobs: - name: Clone installation requirements shell: bash run: python3 -m pip install -U vcstool && vcs import . < ./rosbot/rosbot_hardware.repos && vcs import . < ./rosbot/rosbot_simulation.repos - && touch ros2_controllers/COLCON_IGNORE + - name: Copy only diff_drive_controller and imu_sensor_broadcaster, waits for features from ros2-control + shell: bash + run: cp -r ros2_controllers/diff_drive_controller . && cp -r ros2_controllers/imu_sensor_broadcaster . && rm -rf ros2_controllers - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: ${{matrix.ROS_DISTRO}} diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index f429f78d..a450f210 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -36,9 +36,6 @@ def generate_launch_description(): default_value="", description="Namespace for all topics and tfs", ) - namespace_tf_prefix = PythonExpression( - ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "_'"] - ) mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( @@ -135,12 +132,6 @@ def generate_launch_description(): parameters=[ robot_description, robot_controllers, - # imu_broadcaster frame_id override - { - "frame_id": LaunchConfiguration( - "imu_frame", default=[namespace_tf_prefix, "imu_link"] - ) - }, { "tf_frame_prefix": LaunchConfiguration( "rosbot_base_tf_frame_prefix", default=[namespace] From 1df32161ba000ff8d80c7b93d0d496c20c36cc4d Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 14 Nov 2023 11:55:16 +0100 Subject: [PATCH 13/79] changed comments | changed timeout for controller test Signed-off-by: Jakub Delicat --- rosbot_controller/test/test_diff_drive_controllers.py | 6 +++--- rosbot_controller/test/test_mecanum_controllers.py | 6 +++--- rosbot_description/urdf/rosbot_macro.urdf.xacro | 4 ++-- rosbot_description/urdf/wheel.urdf.xacro | 1 - 4 files changed, 8 insertions(+), 9 deletions(-) diff --git a/rosbot_controller/test/test_diff_drive_controllers.py b/rosbot_controller/test/test_diff_drive_controllers.py index ed4f0b2a..5e15c606 100644 --- a/rosbot_controller/test/test_diff_drive_controllers.py +++ b/rosbot_controller/test/test_diff_drive_controllers.py @@ -58,17 +58,17 @@ def test_controllers_startup_fail(): node.create_test_subscribers_and_publishers() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=20.0) + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) assert not msgs_received_flag, ( "Received JointStates message that should not have appeared. Check whether other" " robots are connected to your network.!" ) - msgs_received_flag = node.odom_msg_event.wait(timeout=20.0) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) assert not msgs_received_flag, ( "Received Odom message that should not have appeared. Check whether other robots are" " connected to your network.!" ) - msgs_received_flag = node.imu_msg_event.wait(timeout=20.0) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) assert not msgs_received_flag, ( "Received Imu message that should not have appeared. Check whether other robots are" " connected to your network.!" diff --git a/rosbot_controller/test/test_mecanum_controllers.py b/rosbot_controller/test/test_mecanum_controllers.py index 9a2b8861..6ff9e192 100644 --- a/rosbot_controller/test/test_mecanum_controllers.py +++ b/rosbot_controller/test/test_mecanum_controllers.py @@ -58,17 +58,17 @@ def test_controllers_startup_fail(): node.create_test_subscribers_and_publishers() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=20.0) + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) assert not msgs_received_flag, ( "Received JointStates message that should not have appeared. Check whether other" " robots are connected to your network.!" ) - msgs_received_flag = node.odom_msg_event.wait(timeout=20.0) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) assert not msgs_received_flag, ( "Received Odom message that should not have appeared. Check whether other robots are" " connected to your network.!" ) - msgs_received_flag = node.imu_msg_event.wait(timeout=20.0) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) assert not msgs_received_flag, ( "Received Imu message that should not have appeared. Check whether other robots are" " connected to your network.!" diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 5272ee47..31a01c7f 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -60,7 +60,8 @@ 120000 500 - + + rr_wheel_joint, @@ -81,7 +82,6 @@ - diff --git a/rosbot_description/urdf/wheel.urdf.xacro b/rosbot_description/urdf/wheel.urdf.xacro index 1ab02bbe..47af25e6 100644 --- a/rosbot_description/urdf/wheel.urdf.xacro +++ b/rosbot_description/urdf/wheel.urdf.xacro @@ -65,7 +65,6 @@ - From 86c758839ac9cb0d8a8cf9274a0e535c5060f67e Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 14 Nov 2023 12:00:32 +0100 Subject: [PATCH 14/79] param changed to parameter Signed-off-by: Jakub Delicat --- rosbot_description/urdf/rosbot_macro.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 31a01c7f..d52f1a43 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -61,7 +61,7 @@ 500 - + rr_wheel_joint, From 4a783a6910d24cd2be897df8e1dd109f923460d9 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 28 Nov 2023 13:28:16 +0100 Subject: [PATCH 15/79] Multi robot diff drive example Signed-off-by: Jakub Delicat --- rosbot_bringup/launch/bringup.launch.py | 31 +-- .../config/diff_drive_controller.yaml | 26 +-- .../config/mecanum_drive_controller.yaml | 18 +- rosbot_controller/launch/controller.launch.py | 20 +- rosbot_description/urdf/body.urdf.xacro | 59 +++--- .../urdf/components/vl53lox.urdf.xacro | 29 +-- rosbot_description/urdf/rosbot.urdf.xacro | 24 +-- .../urdf/rosbot_macro.urdf.xacro | 111 +++++----- rosbot_description/urdf/wheel.urdf.xacro | 22 +- .../config/multi_diff_drive_controller.yaml | 180 +++++++++++++++++ .../multi_mecanum_drive_controller.yaml | 191 ++++++++++++++++++ rosbot_gazebo/launch/simulation.launch.py | 93 ++------- rosbot_gazebo/launch/spawn.launch.py | 174 ++++++++++++++++ rosbot_gazebo/setup.py | 1 + 14 files changed, 700 insertions(+), 279 deletions(-) create mode 100644 rosbot_gazebo/config/multi_diff_drive_controller.yaml create mode 100644 rosbot_gazebo/config/multi_mecanum_drive_controller.yaml create mode 100644 rosbot_gazebo/launch/spawn.launch.py diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index a5c06c6d..b25a6e96 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -17,14 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution def generate_launch_description(): namespace = LaunchConfiguration("namespace") - namespace_tf_prefix = PythonExpression( - ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "_'"] - ) declare_namespace_arg = DeclareLaunchArgument( "namespace", default_value="", @@ -91,28 +88,10 @@ def generate_launch_description(): executable="ekf_node", name="ekf_filter_node", output="screen", - parameters=[ - ekf_config, - { - "map_frame": LaunchConfiguration( - "ekf_map_frame", default=[namespace_tf_prefix, "map"] - ) - }, - { - "odom_frame": LaunchConfiguration( - "ekf_odom_frame", default=[namespace_tf_prefix, "odom"] - ) - }, - { - "base_link_frame": LaunchConfiguration( - "ekf_base_link_frame", default=[namespace_tf_prefix, "base_link"] - ) - }, - { - "world_frame": LaunchConfiguration( - "ekf_world_frame", default=[namespace_tf_prefix, "odom"] - ) - }, + parameters=[ekf_config], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), ], namespace=namespace, ) diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index 36512202..12b9f7d1 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -1,25 +1,6 @@ -simulation_ignition_ros_control: - ros__parameters: - use_sim_time: true - -# Separate controller manager used for simulation - only difference is -# the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) -/**/simulation_controller_manager: - ros__parameters: - use_sim_time: true - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: diff_drive_controller/DiffDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - - /**/controller_manager: ros__parameters: - use_sim_time: false + use_sim_time: False update_rate: 20 # Hz joint_state_broadcaster: @@ -32,7 +13,7 @@ simulation_ignition_ros_control: /**/imu_broadcaster: ros__parameters: sensor_name: imu - tf_frame_prefix_enable: True + tf_frame_prefix_enable: False frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally @@ -40,7 +21,8 @@ simulation_ignition_ros_control: /**/rosbot_base_controller: ros__parameters: - tf_frame_prefix_enable: True + tf_frame_prefix_enable: False + left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index 81148dc5..340da789 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -10,24 +10,10 @@ imu_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster -# Separate controller manager used for simulation - only difference is -# the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) -/**/simulation_controller_manager: - ros__parameters: - use_sim_time: true - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: mecanum_drive_controller/MecanumDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - /**/imu_broadcaster: ros__parameters: sensor_name: imu - tf_frame_prefix_enable: True + tf_frame_prefix_enable: False frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive @@ -35,7 +21,7 @@ /**/rosbot_base_controller: ros__parameters: - tf_frame_prefix_enable: True + tf_frame_prefix_enable: False front_left_wheel_name: fl_wheel_joint front_right_wheel_name: fr_wheel_joint rear_left_wheel_name: rl_wheel_joint diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index a450f210..ac2fd839 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -76,20 +76,12 @@ def generate_launch_description(): ] ) - controller_manager_type_name = PythonExpression( - [ - "'simulation_controller_manager' if ", - use_sim, - " else 'controller_manager'", - ] - ) - namespace_for_controller_name = PythonExpression( ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"] ) controller_manager_name = LaunchConfiguration( "controller_manager_name", - default=[namespace_for_controller_name, controller_manager_type_name], + default=[namespace_for_controller_name, "controller_manager"], ) # Get URDF via xacro @@ -112,7 +104,7 @@ def generate_launch_description(): use_gpu, " simulation_engine:=", simulation_engine, - " tf_prefix:=", + " namespace:=", namespace, ] ) @@ -132,17 +124,14 @@ def generate_launch_description(): parameters=[ robot_description, robot_controllers, - { - "tf_frame_prefix": LaunchConfiguration( - "rosbot_base_tf_frame_prefix", default=[namespace] - ) - }, ], remappings=[ ("imu_sensor_node/imu", "/_imu/data_raw"), ("~/motors_cmd", "/_motors_cmd"), ("~/motors_response", "/_motors_response"), ("rosbot_base_controller/cmd_vel_unstamped", "cmd_vel"), + ("/tf", "tf"), + ("/tf_static", "tf_static"), ], condition=UnlessCondition(use_sim), namespace=namespace, @@ -152,6 +141,7 @@ def generate_launch_description(): package="robot_state_publisher", executable="robot_state_publisher", parameters=[robot_description], + remappings=[("/tf", "tf"), ("/tf_static", "tf_static")], namespace=namespace, ) diff --git a/rosbot_description/urdf/body.urdf.xacro b/rosbot_description/urdf/body.urdf.xacro index 65535519..c178f1f8 100644 --- a/rosbot_description/urdf/body.urdf.xacro +++ b/rosbot_description/urdf/body.urdf.xacro @@ -1,28 +1,21 @@ - + - - - - - - + - - - + - - + + - + - + @@ -45,51 +38,51 @@ - + Gazebo/White - + - - + + - + - + - + Gazebo/Red - + - - + + - + - + - - + + - + - - - - + + + + diff --git a/rosbot_description/urdf/components/vl53lox.urdf.xacro b/rosbot_description/urdf/components/vl53lox.urdf.xacro index a0b4891e..53b44bb8 100644 --- a/rosbot_description/urdf/components/vl53lox.urdf.xacro +++ b/rosbot_description/urdf/components/vl53lox.urdf.xacro @@ -1,32 +1,25 @@ - + - - - - - - - - + - + - + - - - + diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index c3dabd65..8774a228 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -5,15 +5,7 @@ - - - - - - - - - + + namespace="$(arg namespace)" /> - + diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index d52f1a43..8223cc63 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -1,6 +1,13 @@ - + + + + + + + + @@ -9,36 +16,28 @@ - - - - - - - - - + - - - - + + + + - + rosbot_hardware_interfaces/RosbotImuSensor 120000 500 - + @@ -53,7 +52,7 @@ - + rosbot_hardware_interfaces/RosbotSystem @@ -69,7 +68,7 @@ fr_wheel_joint, fl_wheel_joint - ${tf_prefix_ext} + ${namespace_ext} @@ -82,30 +81,30 @@ - + - + - + - + - + @@ -126,30 +125,46 @@ - - $(find rosbot_controller)/config/mecanum_drive_controller.yaml - - - $(find rosbot_controller)/config/diff_drive_controller.yaml + + + $(find rosbot_gazebo)/config/multi_mecanum_drive_controller.yaml + + + $(find rosbot_gazebo)/config/multi_diff_drive_controller.yaml + - simulation + + + + $(find rosbot_bringup)/config/mecanum_drive_controller.yaml + + + $(find rosbot_bringup)/config/diff_drive_controller.yaml + + + - /rosbot_base_controller/cmd_vel_unstamped:=/cmd_vel + ${namespace} + rosbot_base_controller/cmd_vel_unstamped:=cmd_vel + /tf:=tf - - + @@ -160,10 +175,10 @@ 10.0 2 - ${tf_prefix_ext}fl_wheel_joint - ${tf_prefix_ext}fr_wheel_joint - ${tf_prefix_ext}rl_wheel_joint - ${tf_prefix_ext}rr_wheel_joint + fl_wheel_joint + fr_wheel_joint + rl_wheel_joint + rr_wheel_joint 0.192 0.192 ${wheel_radius*2.0} @@ -173,22 +188,22 @@ true true true - ${tf_prefix_ext}odom - ${tf_prefix_ext}base_link + odom + base_link - ${tf_prefix_ext}fl_wheel_joint - ${tf_prefix_ext}fr_wheel_joint - ${tf_prefix_ext}rl_wheel_joint - ${tf_prefix_ext}rr_wheel_joint + fl_wheel_joint + fr_wheel_joint + rl_wheel_joint + rr_wheel_joint - + true 25 diff --git a/rosbot_description/urdf/wheel.urdf.xacro b/rosbot_description/urdf/wheel.urdf.xacro index 47af25e6..73c3d3ca 100644 --- a/rosbot_description/urdf/wheel.urdf.xacro +++ b/rosbot_description/urdf/wheel.urdf.xacro @@ -1,13 +1,13 @@ - + - - + + - - + + @@ -65,16 +65,16 @@ - - - + + + - + @@ -97,14 +97,14 @@ - + 1.0 0.0 - ${fdir} + ${fdir} diff --git a/rosbot_gazebo/config/multi_diff_drive_controller.yaml b/rosbot_gazebo/config/multi_diff_drive_controller.yaml new file mode 100644 index 00000000..9b0e3a6a --- /dev/null +++ b/rosbot_gazebo/config/multi_diff_drive_controller.yaml @@ -0,0 +1,180 @@ +# This file has controllers configuration for 2 robots because the ign_ros2_control::IgnitionROS2ControlPlugin +# parameter parses only one file. +# +/rosbot1/controller_manager: + ros__parameters: + use_sim_time: True + update_rate: 20 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + rosbot_base_controller: + type: diff_drive_controller/DiffDriveController + imu_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + +/rosbot1/imu_broadcaster: + ros__parameters: + use_sim_time: True + sensor_name: rosbot1/imu + tf_frame_prefix_enable: False + frame_id: imu_link + static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet + static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally + static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally + +/rosbot1/rosbot_base_controller: + ros__parameters: + use_sim_time: True + tf_frame_prefix_enable: False + + left_wheel_names: ["rosbot1/fl_wheel_joint", "rosbot1/rl_wheel_joint"] + right_wheel_names: ["rosbot1/fr_wheel_joint", "rosbot1/rr_wheel_joint"] + + wheel_separation: 0.186 + wheel_radius: 0.0425 + + # For skid drive kinematics it will act as ICR coefficient + # Kinematic model with ICR coefficient isn't totally accurate and this coefficient can differ + # for various ground types, but checking on our office floor 1.3 looked approximately alright + wheel_separation_multiplier: 1.45 + + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + # Feedback from motors is published at around 20 Hz + publish_rate: 20.0 + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally + + # Whether to use feedback or commands for odometry calculations + open_loop: false + + # Update odometry from velocity + #! IMPORTANT at the time of writing it is bugged on deb galactic version and require fork of diff drive controller installed from sources + # in sensor fusion only velocity is used and with this setting it is more accurate + position_feedback: false + # velocity computation filtering + velocity_rolling_window_size: 1 + + enable_odom_tf: false + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + use_stamped_vel: false + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + # top speed is around 1.2m/s + max_velocity: 0.9 # m/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 1.0 # m/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # m/s^3 + + angular: + z: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + max_velocity: 3.0 # rad/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 4.0 # rad/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # rad/s^3 + +/rosbot2/controller_manager: + ros__parameters: + use_sim_time: True + update_rate: 20 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + rosbot_base_controller: + type: diff_drive_controller/DiffDriveController + imu_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + +/rosbot2/imu_broadcaster: + ros__parameters: + use_sim_time: True + sensor_name: rosbot2/imu + tf_frame_prefix_enable: False + frame_id: imu_link + static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet + static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally + static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally + +/rosbot2/rosbot_base_controller: + ros__parameters: + use_sim_time: True + tf_frame_prefix_enable: False + + left_wheel_names: ["rosbot2/fl_wheel_joint", "rosbot2/rl_wheel_joint"] + right_wheel_names: ["rosbot2/fr_wheel_joint", "rosbot2/rr_wheel_joint"] + + wheel_separation: 0.186 + wheel_radius: 0.0425 + + # For skid drive kinematics it will act as ICR coefficient + # Kinematic model with ICR coefficient isn't totally accurate and this coefficient can differ + # for various ground types, but checking on our office floor 1.3 looked approximately alright + wheel_separation_multiplier: 1.45 + + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + # Feedback from motors is published at around 20 Hz + publish_rate: 20.0 + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally + + # Whether to use feedback or commands for odometry calculations + open_loop: false + + # Update odometry from velocity + #! IMPORTANT at the time of writing it is bugged on deb galactic version and require fork of diff drive controller installed from sources + # in sensor fusion only velocity is used and with this setting it is more accurate + position_feedback: false + # velocity computation filtering + velocity_rolling_window_size: 1 + + enable_odom_tf: false + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + use_stamped_vel: false + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + # top speed is around 1.2m/s + max_velocity: 0.9 # m/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 1.0 # m/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # m/s^3 + + angular: + z: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + max_velocity: 3.0 # rad/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 4.0 # rad/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # rad/s^3 diff --git a/rosbot_gazebo/config/multi_mecanum_drive_controller.yaml b/rosbot_gazebo/config/multi_mecanum_drive_controller.yaml new file mode 100644 index 00000000..f352830e --- /dev/null +++ b/rosbot_gazebo/config/multi_mecanum_drive_controller.yaml @@ -0,0 +1,191 @@ +/rosbot1/controller_manager: + ros__parameters: + use_sim_time: false + update_rate: 20 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + rosbot_base_controller: + type: mecanum_drive_controller/MecanumDriveController + imu_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + +/rosbot1/imu_broadcaster: + ros__parameters: + sensor_name: rosbot1/imu + tf_frame_prefix_enable: False + frame_id: imu_link + static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet + static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive + static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive + +/rosbot1/rosbot_base_controller: + ros__parameters: + tf_frame_prefix_enable: False + front_left_wheel_name: rosbot1/fl_wheel_joint + front_right_wheel_name: rosbot1/fr_wheel_joint + rear_left_wheel_name: rosbot1/rl_wheel_joint + rear_right_wheel_name: rosbot1/rr_wheel_joint + + wheel_separation_x: 0.106 + wheel_separation_y: 0.192 + wheel_radius: 0.047 + + + wheel_separation_multiplier_x: 1.0 + wheel_separation_multiplier_y: 1.0 + + wheel_radius_multiplier: 1.0 + + # Feedback from motors is published at around 20 Hz + publish_rate: 25.0 + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally + + # Whether to use feedback or commands for odometry calculations + open_loop: false + + # Update odometry from velocity + # in sensor fusion only velocity is used and with this setting it is more accurate + position_feedback: false + # velocity computation filtering + velocity_rolling_window_size: 1 + + enable_odom_tf: false + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + use_stamped_vel: false + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + # top speed is around 1.2m/s + max_velocity: 0.9 # m/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 1.0 # m/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # m/s^3 + y: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + # top speed is around 1.2m/s + max_velocity: 0.9 # m/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 1.0 # m/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # m/s^3 + + angular: + z: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + max_velocity: 3.0 # rad/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 4.0 # rad/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # rad/s^3 + +/rosbot2/controller_manager: + ros__parameters: + use_sim_time: false + update_rate: 20 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + rosbot_base_controller: + type: mecanum_drive_controller/MecanumDriveController + imu_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + +/rosbot2/imu_broadcaster: + ros__parameters: + sensor_name: rosbot2/imu + tf_frame_prefix_enable: False + frame_id: imu_link + static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet + static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive + static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive + +/rosbot2/rosbot_base_controller: + ros__parameters: + tf_frame_prefix_enable: False + front_left_wheel_name: rosbot2/fl_wheel_joint + front_right_wheel_name: rosbot2/fr_wheel_joint + rear_left_wheel_name: rosbot2/rl_wheel_joint + rear_right_wheel_name: rosbot2/rr_wheel_joint + + wheel_separation_x: 0.106 + wheel_separation_y: 0.192 + wheel_radius: 0.047 + + + wheel_separation_multiplier_x: 1.0 + wheel_separation_multiplier_y: 1.0 + + wheel_radius_multiplier: 1.0 + + # Feedback from motors is published at around 20 Hz + publish_rate: 25.0 + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally + + # Whether to use feedback or commands for odometry calculations + open_loop: false + + # Update odometry from velocity + # in sensor fusion only velocity is used and with this setting it is more accurate + position_feedback: false + # velocity computation filtering + velocity_rolling_window_size: 1 + + enable_odom_tf: false + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + use_stamped_vel: false + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + # top speed is around 1.2m/s + max_velocity: 0.9 # m/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 1.0 # m/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # m/s^3 + y: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + # top speed is around 1.2m/s + max_velocity: 0.9 # m/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 1.0 # m/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # m/s^3 + + angular: + z: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + max_velocity: 3.0 # rad/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 4.0 # rad/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # rad/s^3 diff --git a/rosbot_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py index 786c21a0..65e8f817 100644 --- a/rosbot_gazebo/launch/simulation.launch.py +++ b/rosbot_gazebo/launch/simulation.launch.py @@ -24,12 +24,19 @@ ) from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node, SetParameter +from launch_ros.actions import SetParameter from ament_index_python.packages import get_package_share_directory def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", @@ -85,59 +92,13 @@ def generate_launch_description(): }.items(), ) - gz_spawn_entity = Node( - package="ros_gz_sim", - executable="create", - arguments=[ - "-name", - "rosbot", - "-allow_renaming", - "true", - "-topic", - "robot_description", - "-x", - "0", - "-y", - "2.0", - "-z", - "0.2", - ], - output="screen", - ) - - ign_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="ros_gz_bridge", - arguments=[ - "/scan@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - "/camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo", - "/camera/depth_image@sensor_msgs/msg/Image[ignition.msgs.Image", - "/camera/image@sensor_msgs/msg/Image[ignition.msgs.Image", - "/camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked", - "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", - # an IR sensor is not implemented yet https://github.com/gazebosim/gz-sensors/issues/19 - "/range/fl@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - "/range/fr@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - "/range/rl@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - "/range/rr@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - ], - remappings=[ - ("/camera/camera_info", "/camera/color/camera_info"), - ("/camera/image", "/camera/color/image_raw"), - ("/camera/depth_image", "/camera/depth/image_raw"), - ("/camera/points", "/camera/depth/points"), - ], - output="screen", - ) - - bringup_launch = IncludeLaunchDescription( + spawn_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( [ - get_package_share_directory("rosbot_bringup"), + get_package_share_directory("rosbot_gazebo"), "launch", - "bringup.launch.py", + "spawn.launch.py", ] ) ), @@ -146,30 +107,19 @@ def generate_launch_description(): "use_sim": "True", "use_gpu": use_gpu, "simulation_engine": "ignition-gazebo", + "namespace": namespace, + "x": LaunchConfiguration("x", default="0.00"), + "y": LaunchConfiguration("y", default="2.00"), + "z": LaunchConfiguration("z", default="0.20"), + "roll": LaunchConfiguration("roll", default="0.00"), + "pitch": LaunchConfiguration("pitch", default="0.00"), + "yaw": LaunchConfiguration("yaw", default="0.00"), }.items(), ) - # The frame of the pointcloud from ignition gazebo 6 isn't provided by . - # See https://github.com/gazebosim/gz-sensors/issues/239 - depth_cam_frame_fixer = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="depth_to_camera", - output="log", - arguments=[ - "0.0", - "0.0", - "0.0", - "1.57", - "-1.57", - "0.0", - "camera_depth_optical_frame", - "rosbot/base_link/camera_orbbec_astra_camera", - ], - ) - return LaunchDescription( [ + declare_namespace_arg, declare_mecanum_arg, declare_world_arg, declare_headless_arg, @@ -178,9 +128,6 @@ def generate_launch_description(): # (doesn't work for nodes started from ignition gazebo) SetParameter(name="use_sim_time", value=True), gz_sim, - ign_bridge, - gz_spawn_entity, - bringup_launch, - depth_cam_frame_fixer, + spawn_launch, ] ) diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py new file mode 100644 index 00000000..12dffba1 --- /dev/null +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -0,0 +1,174 @@ +# Copyright 2023 Husarion +# +# 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. + +from launch import LaunchDescription +from launch.actions import ( + IncludeLaunchDescription, + DeclareLaunchArgument, +) +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, PythonExpression +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import Node, SetParameter + +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + + mecanum = LaunchConfiguration("mecanum") + declare_mecanum_arg = DeclareLaunchArgument( + "mecanum", + default_value="False", + description=( + "Whether to use mecanum drive controller (otherwise diff drive controller is used)" + ), + ) + + use_gpu = LaunchConfiguration("use_gpu") + declare_use_gpu_arg = DeclareLaunchArgument( + "use_gpu", + default_value="True", + description="Whether GPU acceleration is used", + ) + + robot_name = PythonExpression( + ["'rosbot'", " if '", namespace, "' == '' ", "else ", "'", namespace, "'"] + ) + + gz_spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + robot_name, + "-allow_renaming", + "true", + "-topic", + "robot_description", + "-x", + LaunchConfiguration("x", default="0.00"), + "-y", + LaunchConfiguration("y", default="0.00"), + "-z", + LaunchConfiguration("z", default="0.00"), + "-R", + LaunchConfiguration("roll", default="0.00"), + "-P", + LaunchConfiguration("pitch", default="0.00"), + "-Y", + LaunchConfiguration("yaw", default="0.00"), + ], + output="screen", + namespace=namespace, + ) + + ign_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="ros_gz_bridge", + arguments=[ + "/scan@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", + "/camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo", + "/camera/depth_image@sensor_msgs/msg/Image[ignition.msgs.Image", + "/camera/image@sensor_msgs/msg/Image[ignition.msgs.Image", + "/camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked", + "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", + # an IR sensor is not implemented yet https://github.com/gazebosim/gz-sensors/issues/19 + "/range/fl@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", + "/range/fr@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", + "/range/rl@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", + "/range/rr@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", + ], + remappings=[ + ("/camera/camera_info", "camera/color/camera_info"), + ("/camera/image", "camera/color/image_raw"), + ("/camera/depth_image", "camera/depth/image_raw"), + ("/camera/points", "camera/depth/points"), + ("/scan", "scan"), + ("/range/fl", "range/fl"), + ("/range/fr", "range/fr"), + ("/range/rl", "range/rl"), + ("/range/rr", "range/rr"), + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + output="screen", + namespace=namespace, + ) + + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("rosbot_bringup"), + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": mecanum, + "use_sim": "True", + "use_gpu": use_gpu, + "simulation_engine": "ignition-gazebo", + "namespace": namespace, + }.items(), + ) + + # The frame of the pointcloud from ignition gazebo 6 isn't provided by . + # See https://github.com/gazebosim/gz-sensors/issues/239 + depth_cam_frame_fixer = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="depth_to_camera", + output="log", + arguments=[ + "0.0", + "0.0", + "0.0", + "1.57", + "-1.57", + "0.0", + # Here should be namespace + "camera_depth_optical_frame", + "rosbot/base_link/camera_orbbec_astra_camera", + ], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + namespace=namespace, + ) + + return LaunchDescription( + [ + declare_namespace_arg, + declare_mecanum_arg, + declare_use_gpu_arg, + # Sets use_sim_time for all nodes started below + # (doesn't work for nodes started from ignition gazebo) + SetParameter(name="use_sim_time", value=True), + ign_bridge, + gz_spawn_entity, + bringup_launch, + depth_cam_frame_fixer, + ] + ) diff --git a/rosbot_gazebo/setup.py b/rosbot_gazebo/setup.py index 0d432bd4..26443e75 100644 --- a/rosbot_gazebo/setup.py +++ b/rosbot_gazebo/setup.py @@ -26,6 +26,7 @@ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), + (os.path.join("share", package_name, "config"), glob("config/*.yaml")), ], install_requires=["setuptools"], zip_safe=True, From 9d850bf50579da547bcbecaf1081791aa5dcaf17 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 28 Nov 2023 13:36:20 +0100 Subject: [PATCH 16/79] imu also works Signed-off-by: Jakub Delicat --- rosbot_description/urdf/rosbot_macro.urdf.xacro | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 8223cc63..724beb3f 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -102,7 +102,7 @@ - + @@ -150,7 +150,7 @@ - + From 02fe7d24cd12e35818be5ea1ddb4da3679a83016 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 28 Nov 2023 16:28:49 +0100 Subject: [PATCH 17/79] Added use_multirobot_system | found imu error Signed-off-by: Jakub Delicat --- rosbot_description/urdf/body.urdf.xacro | 2 +- .../urdf/components/vl53lox.urdf.xacro | 7 +++++-- rosbot_description/urdf/rosbot.urdf.xacro | 21 ++++--------------- .../urdf/rosbot_macro.urdf.xacro | 14 ++++++------- rosbot_gazebo/launch/simulation.launch.py | 13 ++++++++++-- rosbot_gazebo/launch/spawn.launch.py | 9 ++++++++ 6 files changed, 37 insertions(+), 29 deletions(-) diff --git a/rosbot_description/urdf/body.urdf.xacro b/rosbot_description/urdf/body.urdf.xacro index c178f1f8..16ec6563 100644 --- a/rosbot_description/urdf/body.urdf.xacro +++ b/rosbot_description/urdf/body.urdf.xacro @@ -1,7 +1,7 @@ - + diff --git a/rosbot_description/urdf/components/vl53lox.urdf.xacro b/rosbot_description/urdf/components/vl53lox.urdf.xacro index 53b44bb8..83d60c8b 100644 --- a/rosbot_description/urdf/components/vl53lox.urdf.xacro +++ b/rosbot_description/urdf/components/vl53lox.urdf.xacro @@ -13,7 +13,7 @@ - + diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index 8774a228..5edb6dac 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -5,7 +5,8 @@ - + + - - + namespace="$(arg namespace)" + use_multirobot_system="$(arg use_multirobot_system)" /> diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 724beb3f..0453740f 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -1,6 +1,6 @@ - + @@ -21,7 +21,7 @@ - + @@ -125,23 +125,23 @@ - + $(find rosbot_gazebo)/config/multi_mecanum_drive_controller.yaml $(find rosbot_gazebo)/config/multi_diff_drive_controller.yaml - + - + $(find rosbot_bringup)/config/mecanum_drive_controller.yaml $(find rosbot_bringup)/config/diff_drive_controller.yaml - + ${namespace} @@ -155,7 +155,7 @@ true 25 - imu/data_raw + ${namespace_ext}imu/data_raw false false imu_link diff --git a/rosbot_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py index 65e8f817..be66c2d5 100644 --- a/rosbot_gazebo/launch/simulation.launch.py +++ b/rosbot_gazebo/launch/simulation.launch.py @@ -46,6 +46,13 @@ def generate_launch_description(): ), ) + use_multirobot_system = LaunchConfiguration("use_multirobot_system") + declare_use_multirobot_system_arg = DeclareLaunchArgument( + "use_multirobot_system", + default_value="false", + description="Enable correct Ignition Gazebo configuration in URDF", + ) + world_package = get_package_share_directory("husarion_office_gz") world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"]) world_cfg = LaunchConfiguration("world") @@ -62,9 +69,9 @@ def generate_launch_description(): headless_cfg = PythonExpression( [ - "'--headless-rendering -s -r' if ", + "'--headless-rendering -s -v 4 -r' if ", headless, - " else '-r'", + " else '-r -v 4 '", ] ) gz_args = [headless_cfg, " ", world_cfg] @@ -104,6 +111,7 @@ def generate_launch_description(): ), launch_arguments={ "mecanum": mecanum, + "use_multirobot_system": use_multirobot_system, "use_sim": "True", "use_gpu": use_gpu, "simulation_engine": "ignition-gazebo", @@ -120,6 +128,7 @@ def generate_launch_description(): return LaunchDescription( [ declare_namespace_arg, + declare_use_multirobot_system_arg, declare_mecanum_arg, declare_world_arg, declare_headless_arg, diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py index 12dffba1..18fc6ddf 100644 --- a/rosbot_gazebo/launch/spawn.launch.py +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -42,6 +42,13 @@ def generate_launch_description(): ), ) + use_multirobot_system = LaunchConfiguration("use_multirobot_system") + declare_use_multirobot_system_arg = DeclareLaunchArgument( + "use_multirobot_system", + default_value="false", + description="Enable correct Ignition Gazebo configuration in URDF", + ) + use_gpu = LaunchConfiguration("use_gpu") declare_use_gpu_arg = DeclareLaunchArgument( "use_gpu", @@ -125,6 +132,7 @@ def generate_launch_description(): ) ), launch_arguments={ + "use_multirobot_system": use_multirobot_system, "mecanum": mecanum, "use_sim": "True", "use_gpu": use_gpu, @@ -162,6 +170,7 @@ def generate_launch_description(): [ declare_namespace_arg, declare_mecanum_arg, + declare_use_multirobot_system_arg, declare_use_gpu_arg, # Sets use_sim_time for all nodes started below # (doesn't work for nodes started from ignition gazebo) From 1b8d92f6db7c5415da437da06fc2efd1ff1726c6 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 28 Nov 2023 17:51:31 +0100 Subject: [PATCH 18/79] changed all topics to ns Signed-off-by: Jakub Delicat --- rosbot/rosbot_simulation.repos | 4 +- .../config/diff_drive_controller.yaml | 13 +++-- .../config/mecanum_drive_controller.yaml | 16 ++--- rosbot_controller/launch/controller.launch.py | 58 ++++++++++++++++++- 4 files changed, 73 insertions(+), 18 deletions(-) diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos index a4014f93..affc3289 100644 --- a/rosbot/rosbot_simulation.repos +++ b/rosbot/rosbot_simulation.repos @@ -2,9 +2,7 @@ repositories: gazebosim/gz_ros2_control: type: git url: https://github.com/ros-controls/gz_ros2_control.git - # on branch humble hardware isn't activated - # recently on master API breaking change was introduced, it is necessary to use commit before this change - version: b296ff2f5c3758b637a70bd496fe6ed875ab03ce + version: humble husarion/husarion_office_gz: type: git diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index 12b9f7d1..de1c2cf0 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -12,8 +12,9 @@ /**/imu_broadcaster: ros__parameters: - sensor_name: imu - tf_frame_prefix_enable: False + tf_frame_prefix_enable: false + # Overwritten in controller.launch.py + # sensor_name: imu frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally @@ -21,10 +22,10 @@ /**/rosbot_base_controller: ros__parameters: - tf_frame_prefix_enable: False - - left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] - right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] + tf_frame_prefix_enable: false + # Overwritten in controller.launch.py + # left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] + # right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] wheel_separation: 0.186 wheel_radius: 0.0425 diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index 340da789..fb778820 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -12,8 +12,9 @@ /**/imu_broadcaster: ros__parameters: - sensor_name: imu - tf_frame_prefix_enable: False + tf_frame_prefix_enable: false + # Overwritten in controller.launch.py + # sensor_name: imu frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive @@ -21,11 +22,12 @@ /**/rosbot_base_controller: ros__parameters: - tf_frame_prefix_enable: False - front_left_wheel_name: fl_wheel_joint - front_right_wheel_name: fr_wheel_joint - rear_left_wheel_name: rl_wheel_joint - rear_right_wheel_name: rr_wheel_joint + tf_frame_prefix_enable: false + # Overwritten in controller.launch.py + # front_left_wheel_name: fl_wheel_joint + # front_right_wheel_name: fr_wheel_joint + # rear_left_wheel_name: rl_wheel_joint + # rear_right_wheel_name: rr_wheel_joint wheel_separation_x: 0.106 wheel_separation_y: 0.192 diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index ac2fd839..d569f7ab 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -76,12 +76,12 @@ def generate_launch_description(): ] ) - namespace_for_controller_name = PythonExpression( + namespace_ext = PythonExpression( ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"] ) controller_manager_name = LaunchConfiguration( "controller_manager_name", - default=[namespace_for_controller_name, "controller_manager"], + default=[namespace_ext, "controller_manager"], ) # Get URDF via xacro @@ -124,6 +124,60 @@ def generate_launch_description(): parameters=[ robot_description, robot_controllers, + { + "left_wheel_names": LaunchConfiguration( + "left_wheels_joints", + default=[ + "[", + namespace_ext, + "fl_wheel_joint,", + namespace_ext, + "rl_wheel_joint]", + ], + ) + }, + { + "right_wheel_names": LaunchConfiguration( + "right_wheels_joints", + default=[ + "[", + namespace_ext, + "fr_wheel_joint,", + namespace_ext, + "rr_wheel_joint]", + ], + ) + }, + { + "sensor_name": LaunchConfiguration( + "imu_sensor_name", + default=[namespace, "/imu"], + ) + }, + { + "front_left_wheel_name": LaunchConfiguration( + "front_left_wheel_name", + default=[namespace_ext, "fl_wheel_joint"], + ), + }, + { + "front_right_wheel_name": LaunchConfiguration( + "front_right_wheel_name", + default=[namespace_ext, "fr_wheel_joint"], + ), + }, + { + "rear_left_wheel_name": LaunchConfiguration( + "rear_left_wheel_name", + default=[namespace_ext, "rl_wheel_joint"], + ), + }, + { + "rear_right_wheel_name": LaunchConfiguration( + "rear_right_wheel_name", + default=[namespace_ext, "rr_wheel_joint"], + ), + }, ], remappings=[ ("imu_sensor_node/imu", "/_imu/data_raw"), From 5cd3a69c2106907c635258ec53d9365aedea1869 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 28 Nov 2023 17:55:49 +0100 Subject: [PATCH 19/79] removed tf_prefix from xacro tests Signed-off-by: Jakub Delicat --- rosbot_controller/launch/controller.launch.py | 2 +- rosbot_controller/test/test_xacro.py | 33 ++----------------- 2 files changed, 4 insertions(+), 31 deletions(-) diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index d569f7ab..96258681 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -151,7 +151,7 @@ def generate_launch_description(): { "sensor_name": LaunchConfiguration( "imu_sensor_name", - default=[namespace, "/imu"], + default=[namespace_ext, "imu"], ) }, { diff --git a/rosbot_controller/test/test_xacro.py b/rosbot_controller/test/test_xacro.py index 95e46489..17216c75 100644 --- a/rosbot_controller/test/test_xacro.py +++ b/rosbot_controller/test/test_xacro.py @@ -23,50 +23,23 @@ def test_rosbot_description_parsing(): use_sim_values = ["true", "false"] use_gpu_values = ["true", "false"] simulation_engine_values = ["ignition-gazebo", "webots"] # 'gazebo-classic' - tf_prefixes = ["rosbot2r", "rosbot2pro", "husarion", "", "None"] all_combinations = list( - itertools.product( - mecanum_values, use_sim_values, use_gpu_values, simulation_engine_values, tf_prefixes - ) + itertools.product(mecanum_values, use_sim_values, use_gpu_values, simulation_engine_values) ) for combination in all_combinations: - mecanum, use_sim, use_gpu, simulation_engine, tf_prefix = combination + mecanum, use_sim, use_gpu, simulation_engine = combination mappings = { "mecanum": mecanum, "use_sim": use_sim, "use_gpu": use_gpu, "simulation_engine": simulation_engine, - "tf_prefix": tf_prefix, } rosbot_description = get_package_share_directory("rosbot_description") xacro_path = os.path.join(rosbot_description, "urdf/rosbot.urdf.xacro") try: - urdf = xacro.process_file(xacro_path, mappings=mappings) - - namespace = tf_prefix - - links = urdf.getElementsByTagName("link") - for link in links: - link_name = link.getAttribute("name") - if namespace != "None": - if tf_prefix not in link_name: - assert False, f"Link name '{link_name}' does not contain '{namespace}'" - else: - if tf_prefix in link_name: - assert False, f"Link name '{link_name}' does not contain '{namespace}'" - - joints = urdf.getElementsByTagName("joint") - for joint in joints: - joint_name = joint.getAttribute("name") - if namespace != "None": - if tf_prefix not in joint_name: - assert False, f"Joint name '{joint_name}' does not contain '{namespace}'" - else: - if tf_prefix in joint_name: - assert False, f"Joint name '{joint_name}' does not contain '{namespace}'" - + xacro.process_file(xacro_path, mappings=mappings) except xacro.XacroException as e: assert False, ( f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, use_sim:" From f5a95975073016517cbb54746fa1a4e16dd0bc96 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 28 Nov 2023 18:08:38 +0100 Subject: [PATCH 20/79] removed comments | add use_multirobot_system arg Signed-off-by: Jakub Delicat --- rosbot_bringup/config/ekf.yaml | 3 --- rosbot_bringup/launch/bringup.launch.py | 9 +++++++++ rosbot_controller/launch/controller.launch.py | 10 ++++++++++ rosbot_controller/test/test_xacro.py | 12 ++++++++++-- rosbot_description/urdf/rosbot.urdf.xacro | 18 +++++++++++++++++- rosbot_gazebo/launch/spawn.launch.py | 2 +- 6 files changed, 47 insertions(+), 7 deletions(-) diff --git a/rosbot_bringup/config/ekf.yaml b/rosbot_bringup/config/ekf.yaml index 4934c896..71ca55ac 100644 --- a/rosbot_bringup/config/ekf.yaml +++ b/rosbot_bringup/config/ekf.yaml @@ -10,7 +10,6 @@ transform_time_offset: 0.0 transform_timeout: 0.05 - # All transforms are overwritten in launch file due to the namespace argument map_frame: map odom_frame: odom base_link_frame: base_link @@ -18,7 +17,6 @@ publish_tf: true publish_acceleration: false - # The odometry topic name is overwritten in launch file due to the namespace argument odom0: rosbot_base_controller/odom odom0_config: [false, false, false, # X , Y , Z false, false, false, # roll , pitch ,yaw @@ -31,7 +29,6 @@ odom0_differential: false odom0_relative: true - # The IMU topic name is overwritten in launch file due to the namespace argument imu0: imu_broadcaster/imu imu0_config: [false, false, false, # X , Y , Z false, false, true, # roll , pitch ,yaw diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index b25a6e96..4225276b 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -62,6 +62,13 @@ def generate_launch_description(): ), ) + use_multirobot_system = LaunchConfiguration("use_multirobot_system") + declare_use_multirobot_system_arg = DeclareLaunchArgument( + "use_multirobot_system", + default_value="false", + description="Enable correct Ignition Gazebo configuration in URDF", + ) + controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -78,6 +85,7 @@ def generate_launch_description(): "use_gpu": use_gpu, "simulation_engine": simulation_engine, "namespace": namespace, + "use_multirobot_system": use_multirobot_system, }.items(), ) @@ -102,6 +110,7 @@ def generate_launch_description(): declare_use_sim_arg, declare_use_gpu_arg, declare_simulation_engine_arg, + declare_use_multirobot_system_arg, SetParameter(name="use_sim_time", value=use_sim), controller_launch, robot_localization_node, diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index 96258681..8f6202ad 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -68,6 +68,13 @@ def generate_launch_description(): choices=["ignition-gazebo", "gazebo-classic", "webots"], ) + use_multirobot_system = LaunchConfiguration("use_multirobot_system") + declare_use_multirobot_system_arg = DeclareLaunchArgument( + "use_multirobot_system", + default_value="false", + description="Enable correct Ignition Gazebo configuration in URDF", + ) + controller_config_name = PythonExpression( [ "'mecanum_drive_controller.yaml' if ", @@ -106,6 +113,8 @@ def generate_launch_description(): simulation_engine, " namespace:=", namespace, + " use_multirobot_system:=", + use_multirobot_system, ] ) robot_description = {"robot_description": robot_description_content} @@ -265,6 +274,7 @@ def generate_launch_description(): declare_use_sim_arg, declare_use_gpu_arg, declare_simulation_engine_arg, + declare_use_multirobot_system_arg, SetParameter("use_sim_time", value=use_sim), control_node, robot_state_pub_node, diff --git a/rosbot_controller/test/test_xacro.py b/rosbot_controller/test/test_xacro.py index 17216c75..0e52cfe6 100644 --- a/rosbot_controller/test/test_xacro.py +++ b/rosbot_controller/test/test_xacro.py @@ -23,18 +23,26 @@ def test_rosbot_description_parsing(): use_sim_values = ["true", "false"] use_gpu_values = ["true", "false"] simulation_engine_values = ["ignition-gazebo", "webots"] # 'gazebo-classic' + use_multirobot_system_values = ["true", "false"] all_combinations = list( - itertools.product(mecanum_values, use_sim_values, use_gpu_values, simulation_engine_values) + itertools.product( + mecanum_values, + use_sim_values, + use_gpu_values, + simulation_engine_values, + use_multirobot_system_values, + ) ) for combination in all_combinations: - mecanum, use_sim, use_gpu, simulation_engine = combination + mecanum, use_sim, use_gpu, simulation_engine, use_multirobot_system = combination mappings = { "mecanum": mecanum, "use_sim": use_sim, "use_gpu": use_gpu, "simulation_engine": simulation_engine, + "use_multirobot_system": use_multirobot_system, } rosbot_description = get_package_share_directory("rosbot_description") xacro_path = os.path.join(rosbot_description, "urdf/rosbot.urdf.xacro") diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index 5edb6dac..1fdaee18 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -6,7 +6,7 @@ - + + + + + + + + diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py index 18fc6ddf..fc8baab9 100644 --- a/rosbot_gazebo/launch/spawn.launch.py +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -132,12 +132,12 @@ def generate_launch_description(): ) ), launch_arguments={ - "use_multirobot_system": use_multirobot_system, "mecanum": mecanum, "use_sim": "True", "use_gpu": use_gpu, "simulation_engine": "ignition-gazebo", "namespace": namespace, + "use_multirobot_system": use_multirobot_system, }.items(), ) From 297ebc165182aa4d44d75e177af3b2e485c43852 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 28 Nov 2023 18:37:50 +0100 Subject: [PATCH 21/79] moved multi robot system controllers config to rosbot_controller Signed-off-by: Jakub Delicat --- .../config/multi_diff_drive_controller.yaml | 0 .../config/multi_mecanum_drive_controller.yaml | 0 rosbot_description/urdf/rosbot_macro.urdf.xacro | 4 ++-- rosbot_gazebo/setup.py | 1 - 4 files changed, 2 insertions(+), 3 deletions(-) rename {rosbot_gazebo => rosbot_controller}/config/multi_diff_drive_controller.yaml (100%) rename {rosbot_gazebo => rosbot_controller}/config/multi_mecanum_drive_controller.yaml (100%) diff --git a/rosbot_gazebo/config/multi_diff_drive_controller.yaml b/rosbot_controller/config/multi_diff_drive_controller.yaml similarity index 100% rename from rosbot_gazebo/config/multi_diff_drive_controller.yaml rename to rosbot_controller/config/multi_diff_drive_controller.yaml diff --git a/rosbot_gazebo/config/multi_mecanum_drive_controller.yaml b/rosbot_controller/config/multi_mecanum_drive_controller.yaml similarity index 100% rename from rosbot_gazebo/config/multi_mecanum_drive_controller.yaml rename to rosbot_controller/config/multi_mecanum_drive_controller.yaml diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 0453740f..721eff42 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -127,10 +127,10 @@ - $(find rosbot_gazebo)/config/multi_mecanum_drive_controller.yaml + $(find rosbot_bringup)/config/multi_mecanum_drive_controller.yaml - $(find rosbot_gazebo)/config/multi_diff_drive_controller.yaml + $(find rosbot_bringup)/config/multi_diff_drive_controller.yaml diff --git a/rosbot_gazebo/setup.py b/rosbot_gazebo/setup.py index 26443e75..0d432bd4 100644 --- a/rosbot_gazebo/setup.py +++ b/rosbot_gazebo/setup.py @@ -26,7 +26,6 @@ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), - (os.path.join("share", package_name, "config"), glob("config/*.yaml")), ], install_requires=["setuptools"], zip_safe=True, From 9cf6c1bf42d8e06e34d06ef2e710badce062f9ed Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 28 Nov 2023 23:22:23 +0100 Subject: [PATCH 22/79] fixed rosbot_gazebo test Signed-off-by: Jakub Delicat --- .../simulation_diff_drive_controller.yaml | 92 ++++++++++++++++ .../simulation_mecanum_drive_controller.yaml | 102 ++++++++++++++++++ .../urdf/rosbot_macro.urdf.xacro | 27 +++-- 3 files changed, 206 insertions(+), 15 deletions(-) create mode 100644 rosbot_controller/config/simulation_diff_drive_controller.yaml create mode 100644 rosbot_controller/config/simulation_mecanum_drive_controller.yaml diff --git a/rosbot_controller/config/simulation_diff_drive_controller.yaml b/rosbot_controller/config/simulation_diff_drive_controller.yaml new file mode 100644 index 00000000..94c9aa89 --- /dev/null +++ b/rosbot_controller/config/simulation_diff_drive_controller.yaml @@ -0,0 +1,92 @@ +# This file is the controllers configuration for one robot without namespace in Ignition Gazebo +# This file is separated because it is not possible to override sensor_name nad wheels names with +# launch file like in the controller.launch.py. gz_ros2_control/ign_ros2_control get parameters from +# this file not from the launch file. + +/**/controller_manager: + ros__parameters: + use_sim_time: True + update_rate: 20 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + rosbot_base_controller: + type: diff_drive_controller/DiffDriveController + imu_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + +/**/imu_broadcaster: + ros__parameters: + tf_frame_prefix_enable: false + + sensor_name: imu + frame_id: imu_link + static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet + static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally + static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally + +/**/rosbot_base_controller: + ros__parameters: + tf_frame_prefix_enable: false + + left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] + right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] + + wheel_separation: 0.186 + wheel_radius: 0.0425 + + # For skid drive kinematics it will act as ICR coefficient + # Kinematic model with ICR coefficient isn't totally accurate and this coefficient can differ + # for various ground types, but checking on our office floor 1.3 looked approximately alright + wheel_separation_multiplier: 1.45 + + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + # Feedback from motors is published at around 20 Hz + publish_rate: 20.0 + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally + + # Whether to use feedback or commands for odometry calculations + open_loop: false + + # Update odometry from velocity + #! IMPORTANT at the time of writing it is bugged on deb galactic version and require fork of diff drive controller installed from sources + # in sensor fusion only velocity is used and with this setting it is more accurate + position_feedback: false + # velocity computation filtering + velocity_rolling_window_size: 1 + + enable_odom_tf: false + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + use_stamped_vel: false + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + # top speed is around 1.2m/s + max_velocity: 0.9 # m/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 1.0 # m/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # m/s^3 + + angular: + z: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + max_velocity: 3.0 # rad/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 4.0 # rad/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # rad/s^3 diff --git a/rosbot_controller/config/simulation_mecanum_drive_controller.yaml b/rosbot_controller/config/simulation_mecanum_drive_controller.yaml new file mode 100644 index 00000000..92c3ed73 --- /dev/null +++ b/rosbot_controller/config/simulation_mecanum_drive_controller.yaml @@ -0,0 +1,102 @@ +# This file is the controllers configuration for one robot without namespace in Ignition Gazebo +# This file is separated because it is not possible to override sensor_name nad wheels names with +# launch file like in the controller.launch.py. gz_ros2_control/ign_ros2_control get parameters from +# this file not from the launch file. + +/**/controller_manager: + ros__parameters: + use_sim_time: True + update_rate: 20 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + rosbot_base_controller: + type: mecanum_drive_controller/MecanumDriveController + imu_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + +/**/imu_broadcaster: + ros__parameters: + tf_frame_prefix_enable: false + + sensor_name: imu + frame_id: imu_link + static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet + static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive + static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive + +/**/rosbot_base_controller: + ros__parameters: + tf_frame_prefix_enable: false + + front_left_wheel_name: fl_wheel_joint + front_right_wheel_name: fr_wheel_joint + rear_left_wheel_name: rl_wheel_joint + rear_right_wheel_name: rr_wheel_joint + + wheel_separation_x: 0.106 + wheel_separation_y: 0.192 + wheel_radius: 0.047 + + + wheel_separation_multiplier_x: 1.0 + wheel_separation_multiplier_y: 1.0 + + wheel_radius_multiplier: 1.0 + + # Feedback from motors is published at around 20 Hz + publish_rate: 25.0 + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally + + # Whether to use feedback or commands for odometry calculations + open_loop: false + + # Update odometry from velocity + # in sensor fusion only velocity is used and with this setting it is more accurate + position_feedback: false + # velocity computation filtering + velocity_rolling_window_size: 1 + + enable_odom_tf: false + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + use_stamped_vel: false + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + # top speed is around 1.2m/s + max_velocity: 0.9 # m/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 1.0 # m/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # m/s^3 + y: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + # top speed is around 1.2m/s + max_velocity: 0.9 # m/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 1.0 # m/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # m/s^3 + + angular: + z: + has_velocity_limits: true + has_acceleration_limits: true + has_jerk_limits: false + max_velocity: 3.0 # rad/s + # min_velocity - When unspecified, -max_velocity is used + max_acceleration: 4.0 # rad/s^2 + # min_acceleration - When unspecified, -max_acceleration is used. + max_jerk: 0.0 # rad/s^3 diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 721eff42..82fddfa5 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -125,26 +125,23 @@ + - - $(find rosbot_bringup)/config/multi_mecanum_drive_controller.yaml - - - $(find rosbot_bringup)/config/multi_diff_drive_controller.yaml - + - - - $(find rosbot_bringup)/config/mecanum_drive_controller.yaml - - - $(find rosbot_bringup)/config/diff_drive_controller.yaml - + + + + + $(find rosbot_controller)/config/${controller_config_prefix}mecanum_drive_controller.yaml + + + $(find rosbot_controller)/config/${controller_config_prefix}diff_drive_controller.yaml - ${namespace} + ${namespace_ext} rosbot_base_controller/cmd_vel_unstamped:=cmd_vel /tf:=tf @@ -161,7 +158,7 @@ imu_link imu_link - ${namespace} + ${namespace_ext} From 1ad2db6ff9a7eeff22bcc90c7d9c419bb925c921 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 29 Nov 2023 01:31:00 +0100 Subject: [PATCH 23/79] separated namespaced tests | added multirobot gazebo example with broken imu Signed-off-by: Jakub Delicat --- rosbot_bringup/package.xml | 1 + rosbot_bringup/test/test_diff_drive_ekf.py | 4 +- rosbot_bringup/test/test_mecanum_ekf.py | 4 +- .../test/test_namespaced_diff_drive_ekf.py | 68 +++++ .../test/test_namespaced_mecanum_ekf.py | 68 +++++ rosbot_bringup/test/test_utils.py | 31 +-- .../config/diff_drive_controller.yaml | 10 +- .../config/mecanum_drive_controller.yaml | 14 +- .../simulation_diff_drive_controller.yaml | 92 ------- .../simulation_mecanum_drive_controller.yaml | 102 -------- rosbot_controller/launch/controller.launch.py | 54 ---- .../test/test_diff_drive_controllers.py | 1 - .../test/test_mecanum_controllers.py | 1 - .../test_namespaced_diff_drive_controllers.py | 102 ++++++++ .../test_namespaced_mecanum_controllers.py | 102 ++++++++ rosbot_controller/test/test_utils.py | 8 +- .../urdf/rosbot_macro.urdf.xacro | 232 ------------------ .../launch/multirobot_simulation.launch.py | 151 ++++++++++++ 18 files changed, 523 insertions(+), 522 deletions(-) create mode 100644 rosbot_bringup/test/test_namespaced_diff_drive_ekf.py create mode 100644 rosbot_bringup/test/test_namespaced_mecanum_ekf.py delete mode 100644 rosbot_controller/config/simulation_diff_drive_controller.yaml delete mode 100644 rosbot_controller/config/simulation_mecanum_drive_controller.yaml create mode 100644 rosbot_controller/test/test_namespaced_diff_drive_controllers.py create mode 100644 rosbot_controller/test/test_namespaced_mecanum_controllers.py delete mode 100644 rosbot_description/urdf/rosbot_macro.urdf.xacro create mode 100644 rosbot_gazebo/launch/multirobot_simulation.launch.py diff --git a/rosbot_bringup/package.xml b/rosbot_bringup/package.xml index b3f84d63..b33998ac 100644 --- a/rosbot_bringup/package.xml +++ b/rosbot_bringup/package.xml @@ -25,6 +25,7 @@ robot_localization rosbot_controller + nav_msgs ament_python diff --git a/rosbot_bringup/test/test_diff_drive_ekf.py b/rosbot_bringup/test/test_diff_drive_ekf.py index c9b1bea8..5da7a3b1 100644 --- a/rosbot_bringup/test/test_diff_drive_ekf.py +++ b/rosbot_bringup/test/test_diff_drive_ekf.py @@ -58,10 +58,10 @@ def test_bringup_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.odom_tf_event.wait(timeout=10.0) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) assert ( msgs_received_flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" + ), "Expected odometry/filtered message but it was not received. Check robot_localization!" finally: rclpy.shutdown() diff --git a/rosbot_bringup/test/test_mecanum_ekf.py b/rosbot_bringup/test/test_mecanum_ekf.py index d46b29cc..fcdfc633 100644 --- a/rosbot_bringup/test/test_mecanum_ekf.py +++ b/rosbot_bringup/test/test_mecanum_ekf.py @@ -58,10 +58,10 @@ def test_bringup_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.odom_tf_event.wait(timeout=10.0) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) assert ( msgs_received_flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" + ), "Expected odometry/filtered message but it was not received. Check robot_localization!" finally: rclpy.shutdown() diff --git a/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py b/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py new file mode 100644 index 00000000..f78629a7 --- /dev/null +++ b/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py @@ -0,0 +1,68 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_bringup = get_package_share_directory("rosbot_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_bringup_startup_success(): + rclpy.init() + try: + node = BringupTestNode("test_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected odometry/filtered message but it was not received. Check robot_localization!" + + finally: + rclpy.shutdown() diff --git a/rosbot_bringup/test/test_namespaced_mecanum_ekf.py b/rosbot_bringup/test/test_namespaced_mecanum_ekf.py new file mode 100644 index 00000000..a49b8faf --- /dev/null +++ b/rosbot_bringup/test/test_namespaced_mecanum_ekf.py @@ -0,0 +1,68 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_bringup = get_package_share_directory("rosbot_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_bringup_startup_success(): + rclpy.init() + try: + node = BringupTestNode("test_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected odometry/filtered message but it was not received. Check robot_localization!" + + finally: + rclpy.shutdown() diff --git a/rosbot_bringup/test/test_utils.py b/rosbot_bringup/test/test_utils.py index 36ff36f4..cc483be1 100644 --- a/rosbot_bringup/test/test_utils.py +++ b/rosbot_bringup/test/test_utils.py @@ -21,9 +21,7 @@ from rclpy.node import Node from sensor_msgs.msg import JointState, Imu -from tf2_ros import TransformException -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener +from nav_msgs.msg import Odometry class BringupTestNode(Node): @@ -31,27 +29,20 @@ class BringupTestNode(Node): __test__ = False - def __init__(self, name="test_node"): - super().__init__(name) - self.odom_tf_event = Event() + def __init__(self, name="test_node", namespace=None): + super().__init__(name, namespace=namespace) + self.odom_msg_event = Event() def create_test_subscribers_and_publishers(self): - self.imu_publisher = self.create_publisher(Imu, "_imu/data_raw", 10) + self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10) - self.joint_states_publisher = self.create_publisher(JointState, "_motors_response", 10) - - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) + self.joint_states_publisher = self.create_publisher(JointState, "/_motors_response", 10) + self.odom_sub = self.create_subscription( + Odometry, "odometry/filtered", self.odometry_callback, 10 + ) self.timer = None - def lookup_transform_odom(self): - try: - self.tf_buffer.lookup_transform("odom", "base_link", rclpy.time.Time()) - self.odom_tf_event.set() - except TransformException as ex: - self.get_logger().error(f"Could not transform odom to base_link: {ex}") - def start_node_thread(self): self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) self.ros_spin_thread.start() @@ -64,7 +55,9 @@ def start_publishing_fake_hardware(self): def timer_callback(self): self.publish_fake_hardware_messages() - self.lookup_transform_odom() + + def odometry_callback(self, data): + self.odom_msg_event.set() def publish_fake_hardware_messages(self): imu_msg = Imu() diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index de1c2cf0..1755236d 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -13,8 +13,8 @@ /**/imu_broadcaster: ros__parameters: tf_frame_prefix_enable: false - # Overwritten in controller.launch.py - # sensor_name: imu + + sensor_name: imu frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally @@ -23,9 +23,9 @@ /**/rosbot_base_controller: ros__parameters: tf_frame_prefix_enable: false - # Overwritten in controller.launch.py - # left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] - # right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] + + left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] + right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] wheel_separation: 0.186 wheel_radius: 0.0425 diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index fb778820..9eec0f0e 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -13,8 +13,8 @@ /**/imu_broadcaster: ros__parameters: tf_frame_prefix_enable: false - # Overwritten in controller.launch.py - # sensor_name: imu + + sensor_name: imu frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive @@ -23,11 +23,11 @@ /**/rosbot_base_controller: ros__parameters: tf_frame_prefix_enable: false - # Overwritten in controller.launch.py - # front_left_wheel_name: fl_wheel_joint - # front_right_wheel_name: fr_wheel_joint - # rear_left_wheel_name: rl_wheel_joint - # rear_right_wheel_name: rr_wheel_joint + + front_left_wheel_name: fl_wheel_joint + front_right_wheel_name: fr_wheel_joint + rear_left_wheel_name: rl_wheel_joint + rear_right_wheel_name: rr_wheel_joint wheel_separation_x: 0.106 wheel_separation_y: 0.192 diff --git a/rosbot_controller/config/simulation_diff_drive_controller.yaml b/rosbot_controller/config/simulation_diff_drive_controller.yaml deleted file mode 100644 index 94c9aa89..00000000 --- a/rosbot_controller/config/simulation_diff_drive_controller.yaml +++ /dev/null @@ -1,92 +0,0 @@ -# This file is the controllers configuration for one robot without namespace in Ignition Gazebo -# This file is separated because it is not possible to override sensor_name nad wheels names with -# launch file like in the controller.launch.py. gz_ros2_control/ign_ros2_control get parameters from -# this file not from the launch file. - -/**/controller_manager: - ros__parameters: - use_sim_time: True - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: diff_drive_controller/DiffDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -/**/imu_broadcaster: - ros__parameters: - tf_frame_prefix_enable: false - - sensor_name: imu - frame_id: imu_link - static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet - static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally - static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally - -/**/rosbot_base_controller: - ros__parameters: - tf_frame_prefix_enable: false - - left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] - right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] - - wheel_separation: 0.186 - wheel_radius: 0.0425 - - # For skid drive kinematics it will act as ICR coefficient - # Kinematic model with ICR coefficient isn't totally accurate and this coefficient can differ - # for various ground types, but checking on our office floor 1.3 looked approximately alright - wheel_separation_multiplier: 1.45 - - left_wheel_radius_multiplier: 1.0 - right_wheel_radius_multiplier: 1.0 - - # Feedback from motors is published at around 20 Hz - publish_rate: 20.0 - odom_frame_id: odom - base_frame_id: base_link - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally - - # Whether to use feedback or commands for odometry calculations - open_loop: false - - # Update odometry from velocity - #! IMPORTANT at the time of writing it is bugged on deb galactic version and require fork of diff drive controller installed from sources - # in sensor fusion only velocity is used and with this setting it is more accurate - position_feedback: false - # velocity computation filtering - velocity_rolling_window_size: 1 - - enable_odom_tf: false - - cmd_vel_timeout: 0.5 - #publish_limited_velocity: true - use_stamped_vel: false - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - - angular: - z: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - max_velocity: 3.0 # rad/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 4.0 # rad/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # rad/s^3 diff --git a/rosbot_controller/config/simulation_mecanum_drive_controller.yaml b/rosbot_controller/config/simulation_mecanum_drive_controller.yaml deleted file mode 100644 index 92c3ed73..00000000 --- a/rosbot_controller/config/simulation_mecanum_drive_controller.yaml +++ /dev/null @@ -1,102 +0,0 @@ -# This file is the controllers configuration for one robot without namespace in Ignition Gazebo -# This file is separated because it is not possible to override sensor_name nad wheels names with -# launch file like in the controller.launch.py. gz_ros2_control/ign_ros2_control get parameters from -# this file not from the launch file. - -/**/controller_manager: - ros__parameters: - use_sim_time: True - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: mecanum_drive_controller/MecanumDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -/**/imu_broadcaster: - ros__parameters: - tf_frame_prefix_enable: false - - sensor_name: imu - frame_id: imu_link - static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet - static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive - static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive - -/**/rosbot_base_controller: - ros__parameters: - tf_frame_prefix_enable: false - - front_left_wheel_name: fl_wheel_joint - front_right_wheel_name: fr_wheel_joint - rear_left_wheel_name: rl_wheel_joint - rear_right_wheel_name: rr_wheel_joint - - wheel_separation_x: 0.106 - wheel_separation_y: 0.192 - wheel_radius: 0.047 - - - wheel_separation_multiplier_x: 1.0 - wheel_separation_multiplier_y: 1.0 - - wheel_radius_multiplier: 1.0 - - # Feedback from motors is published at around 20 Hz - publish_rate: 25.0 - odom_frame_id: odom - base_frame_id: base_link - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally - - # Whether to use feedback or commands for odometry calculations - open_loop: false - - # Update odometry from velocity - # in sensor fusion only velocity is used and with this setting it is more accurate - position_feedback: false - # velocity computation filtering - velocity_rolling_window_size: 1 - - enable_odom_tf: false - - cmd_vel_timeout: 0.5 - #publish_limited_velocity: true - use_stamped_vel: false - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - y: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - - angular: - z: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - max_velocity: 3.0 # rad/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 4.0 # rad/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # rad/s^3 diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index 8f6202ad..779a3b47 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -133,60 +133,6 @@ def generate_launch_description(): parameters=[ robot_description, robot_controllers, - { - "left_wheel_names": LaunchConfiguration( - "left_wheels_joints", - default=[ - "[", - namespace_ext, - "fl_wheel_joint,", - namespace_ext, - "rl_wheel_joint]", - ], - ) - }, - { - "right_wheel_names": LaunchConfiguration( - "right_wheels_joints", - default=[ - "[", - namespace_ext, - "fr_wheel_joint,", - namespace_ext, - "rr_wheel_joint]", - ], - ) - }, - { - "sensor_name": LaunchConfiguration( - "imu_sensor_name", - default=[namespace_ext, "imu"], - ) - }, - { - "front_left_wheel_name": LaunchConfiguration( - "front_left_wheel_name", - default=[namespace_ext, "fl_wheel_joint"], - ), - }, - { - "front_right_wheel_name": LaunchConfiguration( - "front_right_wheel_name", - default=[namespace_ext, "fr_wheel_joint"], - ), - }, - { - "rear_left_wheel_name": LaunchConfiguration( - "rear_left_wheel_name", - default=[namespace_ext, "rl_wheel_joint"], - ), - }, - { - "rear_right_wheel_name": LaunchConfiguration( - "rear_right_wheel_name", - default=[namespace_ext, "rr_wheel_joint"], - ), - }, ], remappings=[ ("imu_sensor_node/imu", "/_imu/data_raw"), diff --git a/rosbot_controller/test/test_diff_drive_controllers.py b/rosbot_controller/test/test_diff_drive_controllers.py index 5e15c606..3b69459c 100644 --- a/rosbot_controller/test/test_diff_drive_controllers.py +++ b/rosbot_controller/test/test_diff_drive_controllers.py @@ -43,7 +43,6 @@ def generate_test_description(): "use_sim": "False", "mecanum": "False", "use_gpu": "False", - "namespace": "rosbot2r", }.items(), ) diff --git a/rosbot_controller/test/test_mecanum_controllers.py b/rosbot_controller/test/test_mecanum_controllers.py index 6ff9e192..5f2607e3 100644 --- a/rosbot_controller/test/test_mecanum_controllers.py +++ b/rosbot_controller/test/test_mecanum_controllers.py @@ -43,7 +43,6 @@ def generate_test_description(): "use_sim": "False", "mecanum": "True", "use_gpu": "False", - "namespace": "rosbot2r", }.items(), ) diff --git a/rosbot_controller/test/test_namespaced_diff_drive_controllers.py b/rosbot_controller/test/test_namespaced_diff_drive_controllers.py new file mode 100644 index 00000000..fdab2de6 --- /dev/null +++ b/rosbot_controller/test/test_namespaced_diff_drive_controllers.py @@ -0,0 +1,102 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_controller = get_package_share_directory("rosbot_controller") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_fail(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received JointStates message that should not have appeared. Check whether other" + " robots are connected to your network.!" + ) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Odom message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Imu message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + finally: + rclpy.shutdown() + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_success(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Odom message but it was not received. Check rosbot_base_controller!" + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Imu message but it was not received. Check imu_broadcaster!" + finally: + rclpy.shutdown() diff --git a/rosbot_controller/test/test_namespaced_mecanum_controllers.py b/rosbot_controller/test/test_namespaced_mecanum_controllers.py new file mode 100644 index 00000000..e2321fd0 --- /dev/null +++ b/rosbot_controller/test/test_namespaced_mecanum_controllers.py @@ -0,0 +1,102 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_controller = get_package_share_directory("rosbot_controller") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_fail(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received JointStates message that should not have appeared. Check whether other" + " robots are connected to your network.!" + ) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Odom message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Imu message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + finally: + rclpy.shutdown() + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_success(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Odom message but it was not received. Check rosbot_base_controller!" + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Imu message but it was not received. Check imu_broadcaster!" + finally: + rclpy.shutdown() diff --git a/rosbot_controller/test/test_utils.py b/rosbot_controller/test/test_utils.py index 79e3372b..831eadad 100644 --- a/rosbot_controller/test/test_utils.py +++ b/rosbot_controller/test/test_utils.py @@ -37,16 +37,14 @@ def __init__(self, name="test_node", namespace=None): def create_test_subscribers_and_publishers(self): self.joint_state_sub = self.create_subscription( - JointState, "/rosbot2r/joint_states", self.joint_states_callback, 10 + JointState, "joint_states", self.joint_states_callback, 10 ) self.odom_sub = self.create_subscription( - Odometry, "/rosbot2r/rosbot_base_controller/odom", self.odometry_callback, 10 + Odometry, "rosbot_base_controller/odom", self.odometry_callback, 10 ) - self.imu_sub = self.create_subscription( - Imu, "/rosbot2r/imu_broadcaster/imu", self.imu_callback, 10 - ) + self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10) # TODO: @delipl namespaces have not been implemented in microros yet self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10) diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro deleted file mode 100644 index 82fddfa5..00000000 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ /dev/null @@ -1,232 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - rosbot_hardware_interfaces/RosbotImuSensor - 120000 - 500 - - - - - - - - - - - - - - - - - - - - rosbot_hardware_interfaces/RosbotSystem - 120000 - 500 - - - - - - rr_wheel_joint, - rl_wheel_joint, - fr_wheel_joint, - fl_wheel_joint - - ${namespace_ext} - - - - - ign_ros2_control/IgnitionSystem - - - webots_ros2_control::Ros2ControlSystem - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - $(find rosbot_controller)/config/${controller_config_prefix}mecanum_drive_controller.yaml - - - $(find rosbot_controller)/config/${controller_config_prefix}diff_drive_controller.yaml - - - - ${namespace_ext} - rosbot_base_controller/cmd_vel_unstamped:=cmd_vel - /tf:=tf - - - - - - - true - 25 - ${namespace_ext}imu/data_raw - false - false - imu_link - imu_link - - ${namespace_ext} - - - - - - - - - - - - 10.0 - 2 - fl_wheel_joint - fr_wheel_joint - rl_wheel_joint - rr_wheel_joint - 0.192 - 0.192 - ${wheel_radius*2.0} - ${wheel_radius*2.0} - 1.5 - 1.0 - true - true - true - odom - base_link - - - - - - - fl_wheel_joint - fr_wheel_joint - rl_wheel_joint - rr_wheel_joint - - - - - - true - 25 - - - ~/out:=imu/data/raw - - - - - - - - - - - true - /imu_broadcaster/imu - true - imu_link - imu gyro - imu accelerometer - imu inertial_unit - - - - - - diff --git a/rosbot_gazebo/launch/multirobot_simulation.launch.py b/rosbot_gazebo/launch/multirobot_simulation.launch.py new file mode 100644 index 00000000..ec846c68 --- /dev/null +++ b/rosbot_gazebo/launch/multirobot_simulation.launch.py @@ -0,0 +1,151 @@ +# Copyright 2023 Husarion +# +# 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. + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, TimerAction +from launch.substitutions import ( + PathJoinSubstitution, + PythonExpression, + LaunchConfiguration, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import SetParameter + +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + mecanum = LaunchConfiguration("mecanum") + declare_mecanum_arg = DeclareLaunchArgument( + "mecanum", + default_value="False", + description=( + "Whether to use mecanum drive controller (otherwise diff drive controller is used)" + ), + ) + + world_package = get_package_share_directory("husarion_office_gz") + world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"]) + world_cfg = LaunchConfiguration("world") + declare_world_arg = DeclareLaunchArgument( + "world", default_value=world_file, description="SDF world file" + ) + + headless = LaunchConfiguration("headless") + declare_headless_arg = DeclareLaunchArgument( + "headless", + default_value="False", + description="Run Gazebo Ignition in the headless mode", + ) + + headless_cfg = PythonExpression( + [ + "'--headless-rendering -s -v 4 -r' if ", + headless, + " else '-r -v 4 '", + ] + ) + gz_args = [headless_cfg, " ", world_cfg] + + use_gpu = LaunchConfiguration("use_gpu") + declare_use_gpu_arg = DeclareLaunchArgument( + "use_gpu", + default_value="True", + description="Whether GPU acceleration is used", + ) + + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ] + ) + ), + launch_arguments={ + "gz_args": gz_args, + "on_exit_shutdown": "True", + }.items(), + ) + + spawn_robot1_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("rosbot_gazebo"), + "launch", + "spawn.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": mecanum, + "use_multirobot_system": "True", + "use_sim": "True", + "use_gpu": use_gpu, + "simulation_engine": "ignition-gazebo", + "namespace": "rosbot1", + "x": LaunchConfiguration("x1", default="0.00"), + "y": LaunchConfiguration("y1", default="2.00"), + "z": LaunchConfiguration("z1", default="0.20"), + "roll": LaunchConfiguration("roll1", default="0.00"), + "pitch": LaunchConfiguration("pitch1", default="0.00"), + "yaw": LaunchConfiguration("yaw1", default="0.00"), + }.items(), + ) + + spawn_robot2_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("rosbot_gazebo"), + "launch", + "spawn.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": mecanum, + "use_multirobot_system": "True", + "use_sim": "True", + "use_gpu": use_gpu, + "simulation_engine": "ignition-gazebo", + "namespace": "rosbot2", + "x": LaunchConfiguration("x2", default="1.00"), + "y": LaunchConfiguration("y2", default="2.00"), + "z": LaunchConfiguration("z2", default="0.20"), + "roll": LaunchConfiguration("roll2", default="0.00"), + "pitch": LaunchConfiguration("pitch2", default="0.00"), + "yaw": LaunchConfiguration("yaw2", default="0.00"), + }.items(), + ) + + delayed_spawn_robot2 = TimerAction(period=10.0, actions=[spawn_robot2_launch]) + return LaunchDescription( + [ + declare_mecanum_arg, + declare_world_arg, + declare_headless_arg, + declare_use_gpu_arg, + # Sets use_sim_time for all nodes started below + # (doesn't work for nodes started from ignition gazebo) + SetParameter(name="use_sim_time", value=True), + gz_sim, + spawn_robot1_launch, + delayed_spawn_robot2, + ] + ) From 1e0f69d98b8a3533fe9b808f32a9a55aec788517 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 29 Nov 2023 01:47:49 +0100 Subject: [PATCH 24/79] add multicontroller test Signed-off-by: Jakub Delicat --- .../test/test_multirobot_controllers.py | 100 ++++++++++++++++++ 1 file changed, 100 insertions(+) create mode 100644 rosbot_controller/test/test_multirobot_controllers.py diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py new file mode 100644 index 00000000..5ec2da83 --- /dev/null +++ b/rosbot_controller/test/test_multirobot_controllers.py @@ -0,0 +1,100 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode + +robot_names = ["rosbot1", "rosbot2"] + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_controller = get_package_share_directory("rosbot_controller") + bringup_rosbot1_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + "namespace": robot_names[0], + }.items(), + ) + + bringup_rosbot2_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + "namespace": robot_names[1], + }.items(), + ) + + delayed_spawn_robot2 = TimerAction(period=10.0, actions=[bringup_rosbot2_launch]) + + return LaunchDescription([bringup_rosbot1_launch, delayed_spawn_robot2]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_success(): + for robot_name in robot_names: + rclpy.init() + try: + node = ControllersTestNode(f"test_{robot_name}_controllers", namespace=robot_name) + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert msgs_received_flag, ( + f"Expected JointStates message but it was not received. Check {robot_name}/" + "joint_state_broadcaster!" + ) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert msgs_received_flag, ( + f"Expected Odom message but it was not received. Check {robot_name}/" + "rosbot_base_controller!" + ) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), f"Expected Imu message but it was not received. Check {robot_name}/imu_broadcaster!" + finally: + rclpy.shutdown() From 3ea4d164dc8e9f51ae6ffa20915e2be687108c5f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 29 Nov 2023 01:55:15 +0100 Subject: [PATCH 25/79] add multibringup test Signed-off-by: Jakub Delicat --- rosbot_controller/test/test_multirobot_controllers.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py index 5ec2da83..6d369ede 100644 --- a/rosbot_controller/test/test_multirobot_controllers.py +++ b/rosbot_controller/test/test_multirobot_controllers.py @@ -61,7 +61,7 @@ def generate_test_description(): ), launch_arguments={ "use_sim": "False", - "mecanum": "True", + "mecanum": "False", "use_gpu": "False", "namespace": robot_names[1], }.items(), @@ -73,7 +73,7 @@ def generate_test_description(): @pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_success(): +def test_multirobot_controllers_startup_success(): for robot_name in robot_names: rclpy.init() try: From 568063b65e1d5964271fa24c27692b52bf6faf07 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 29 Nov 2023 01:55:36 +0100 Subject: [PATCH 26/79] add multibringup test Signed-off-by: Jakub Delicat --- rosbot_bringup/test/test_multirobot_ekf.py | 92 ++++++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 rosbot_bringup/test/test_multirobot_ekf.py diff --git a/rosbot_bringup/test/test_multirobot_ekf.py b/rosbot_bringup/test/test_multirobot_ekf.py new file mode 100644 index 00000000..8c3fff46 --- /dev/null +++ b/rosbot_bringup/test/test_multirobot_ekf.py @@ -0,0 +1,92 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import BringupTestNode + +robot_names = ["rosbot1", "rosbot2"] + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_bringup = get_package_share_directory("rosbot_bringup") + bringup_rosbot1_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + "namespace": robot_names[0], + }.items(), + ) + + bringup_rosbot2_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + "namespace": robot_names[1], + }.items(), + ) + + delayed_spawn_robot2 = TimerAction(period=10.0, actions=[bringup_rosbot2_launch]) + + return LaunchDescription([bringup_rosbot1_launch, delayed_spawn_robot2]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_bringup_startup_success(): + for robot_name in robot_names: + rclpy.init() + try: + node = BringupTestNode(f"test_bringup_{robot_name}", namespace=robot_name) + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_msg_event.wait(timeout=20.0) + assert msgs_received_flag, ( + f"Expected {robot_name}/odometry/filtered message but it was not received. " + "Check robot_localization!" + ) + + finally: + rclpy.shutdown() From 2e5b6cf648a8768327cc5f59b0d453c7d0a481e1 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 29 Nov 2023 16:17:16 +0100 Subject: [PATCH 27/79] Added remove rosbot_macro Signed-off-by: Jakub Delicat --- .../urdf/rosbot_macro.urdf.xacro | 243 ++++++++++++++++++ 1 file changed, 243 insertions(+) create mode 100644 rosbot_description/urdf/rosbot_macro.urdf.xacro diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro new file mode 100644 index 00000000..6ef76cbf --- /dev/null +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -0,0 +1,243 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + rosbot_hardware_interfaces/RosbotImuSensor + 120000 + 500 + + + + + + + + + + + + + + + + + + + + rosbot_hardware_interfaces/RosbotSystem + 120000 + 500 + + + + + + rr_wheel_joint, + rl_wheel_joint, + fr_wheel_joint, + fl_wheel_joint + + ${namespace_ext} + + + + + ign_ros2_control/IgnitionSystem + + + webots_ros2_control::Ros2ControlSystem + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(find rosbot_controller)/config/${controller_config_prefix}mecanum_drive_controller.yaml + + + $(find rosbot_controller)/config/${controller_config_prefix}diff_drive_controller.yaml + + + + + ${namespace} + + rosbot_base_controller/cmd_vel_unstamped:=cmd_vel + /tf:=tf + + + + + + + true + 25 + ${namespace_ext}imu/data_raw + false + false + imu_link + imu_link + + + ${namespace} + + + + + + + + + + + + + 10.0 + 2 + fl_wheel_joint + fr_wheel_joint + rl_wheel_joint + rr_wheel_joint + 0.192 + 0.192 + ${wheel_radius*2.0} + ${wheel_radius*2.0} + 1.5 + 1.0 + true + true + true + odom + base_link + + + + + + + fl_wheel_joint + fr_wheel_joint + rl_wheel_joint + rr_wheel_joint + + + + + + true + 25 + + + ~/out:=imu/data/raw + + + + + + + + + + + true + /imu_broadcaster/imu + true + imu_link + imu gyro + imu accelerometer + imu inertial_unit + + + + + + From cd3e10421d1160bbff5b7d815420bc25a1ac12aa Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 29 Nov 2023 18:14:38 +0100 Subject: [PATCH 28/79] fixed miss spelling Signed-off-by: Jakub Delicat --- rosbot_description/urdf/rosbot_macro.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 6ef76cbf..21deafe5 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -2,7 +2,7 @@ - From b831fd56f8e25e145edc91ae26b29a0c73990b29 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 29 Nov 2023 20:47:16 +0100 Subject: [PATCH 29/79] The change from robot_hardware_interface seems to be unnecessary Signed-off-by: Jakub Delicat --- rosbot/rosbot_hardware.repos | 2 +- rosbot_description/urdf/rosbot_macro.urdf.xacro | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index c204dd2c..e30781d4 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -10,7 +10,7 @@ repositories: rosbot_controllers: type: git url: https://github.com/husarion/rosbot_controllers - version: add-namespace + version: main # Waiting for backporting to the humble version # https://github.com/ros-controls/ros2_controllers/pull/726 and # https://github.com/ros-controls/ros2_controllers/pull/831 diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 21deafe5..697b8e93 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -67,15 +67,12 @@ 500 - - rr_wheel_joint, rl_wheel_joint, fr_wheel_joint, fl_wheel_joint - ${namespace_ext} From 11fcac83be792f1c97e498c69fe552587818342d Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 29 Nov 2023 21:11:52 +0100 Subject: [PATCH 30/79] moved multirobot tests to arrays Signed-off-by: Jakub Delicat --- rosbot_bringup/test/test_multirobot_ekf.py | 67 ++++++++----------- .../test/test_multirobot_controllers.py | 65 +++++++----------- 2 files changed, 53 insertions(+), 79 deletions(-) diff --git a/rosbot_bringup/test/test_multirobot_ekf.py b/rosbot_bringup/test/test_multirobot_ekf.py index 8c3fff46..883082b8 100644 --- a/rosbot_bringup/test/test_multirobot_ekf.py +++ b/rosbot_bringup/test/test_multirobot_ekf.py @@ -25,51 +25,38 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from test_utils import BringupTestNode -robot_names = ["rosbot1", "rosbot2"] +robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"] @launch_pytest.fixture def generate_test_description(): rosbot_bringup = get_package_share_directory("rosbot_bringup") - bringup_rosbot1_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_bringup, - "launch", - "bringup.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "False", - "use_gpu": "False", - "namespace": robot_names[0], - }.items(), - ) - - bringup_rosbot2_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_bringup, - "launch", - "bringup.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "True", - "use_gpu": "False", - "namespace": robot_names[1], - }.items(), - ) - - delayed_spawn_robot2 = TimerAction(period=10.0, actions=[bringup_rosbot2_launch]) + actions = [] + for i in range(robot_names): + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + "namespace": robot_names[i], + }.items(), + ) + if i > 0: + delayed_bringup_launch = TimerAction(period=i * 10.0, actions=[bringup_launch]) + actions.append(delayed_bringup_launch) + else: + actions.append(bringup_launch) - return LaunchDescription([bringup_rosbot1_launch, delayed_spawn_robot2]) + return LaunchDescription(actions) @pytest.mark.launch(fixture=generate_test_description) @@ -77,7 +64,7 @@ def test_multirobot_bringup_startup_success(): for robot_name in robot_names: rclpy.init() try: - node = BringupTestNode(f"test_bringup_{robot_name}", namespace=robot_name) + node = BringupTestNode("test_bringup", namespace=robot_name) node.create_test_subscribers_and_publishers() node.start_publishing_fake_hardware() diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py index 6d369ede..fedc096e 100644 --- a/rosbot_controller/test/test_multirobot_controllers.py +++ b/rosbot_controller/test/test_multirobot_controllers.py @@ -25,51 +25,38 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from test_utils import ControllersTestNode -robot_names = ["rosbot1", "rosbot2"] +robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"] @launch_pytest.fixture def generate_test_description(): rosbot_controller = get_package_share_directory("rosbot_controller") - bringup_rosbot1_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_controller, - "launch", - "controller.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "True", - "use_gpu": "False", - "namespace": robot_names[0], - }.items(), - ) - - bringup_rosbot2_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_controller, - "launch", - "controller.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "False", - "use_gpu": "False", - "namespace": robot_names[1], - }.items(), - ) - - delayed_spawn_robot2 = TimerAction(period=10.0, actions=[bringup_rosbot2_launch]) + actions = [] + for i in range(robot_names): + controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + "namespace": robot_names[i], + }.items(), + ) + if i > 0: + delayed_bringup_launch = TimerAction(period=i * 10.0, actions=[controller_launch]) + actions.append(delayed_bringup_launch) + else: + actions.append(controller_launch) - return LaunchDescription([bringup_rosbot1_launch, delayed_spawn_robot2]) + return LaunchDescription(actions) @pytest.mark.launch(fixture=generate_test_description) From 580260166c74e5efb522cafb11d5e4ad973a16d3 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 29 Nov 2023 21:22:36 +0100 Subject: [PATCH 31/79] move rosbot_hardware_interfaces to main Signed-off-by: Jakub Delicat --- rosbot/rosbot_hardware.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index e30781d4..0ffde78d 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -2,7 +2,7 @@ repositories: rosbot_hardware_interfaces: type: git url: https://github.com/husarion/rosbot_hardware_interfaces.git - version: add-namespace + version: main ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git From 9019ff3af66ca1742bf3cc770856589d48890dc2 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 29 Nov 2023 21:58:34 +0100 Subject: [PATCH 32/79] Fixed len Signed-off-by: Jakub Delicat --- rosbot_bringup/test/test_multirobot_ekf.py | 2 +- rosbot_controller/test/test_multirobot_controllers.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rosbot_bringup/test/test_multirobot_ekf.py b/rosbot_bringup/test/test_multirobot_ekf.py index 883082b8..ca3d5685 100644 --- a/rosbot_bringup/test/test_multirobot_ekf.py +++ b/rosbot_bringup/test/test_multirobot_ekf.py @@ -32,7 +32,7 @@ def generate_test_description(): rosbot_bringup = get_package_share_directory("rosbot_bringup") actions = [] - for i in range(robot_names): + for i in range(len(robot_names)): bringup_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py index fedc096e..e6117a2f 100644 --- a/rosbot_controller/test/test_multirobot_controllers.py +++ b/rosbot_controller/test/test_multirobot_controllers.py @@ -32,7 +32,7 @@ def generate_test_description(): rosbot_controller = get_package_share_directory("rosbot_controller") actions = [] - for i in range(robot_names): + for i in range(len(robot_names)): controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( From 165085b9c0a507b0f0600955291710ff95d3f8a6 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 30 Nov 2023 13:11:16 +0100 Subject: [PATCH 33/79] Added updated controllers info to readme Signed-off-by: Jakub Delicat --- .gitignore | 3 ++- README.md | 6 ++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 33aa2f87..2ef10e57 100644 --- a/.gitignore +++ b/.gitignore @@ -12,7 +12,8 @@ ros_components_description/ rosbot_controllers/ husarion/husarion_office_gz gazebosim/gz_ros2_control -ros2_controllers +diff_drive_controller +imu_sensor_broadcaster # pyspelling *.dic diff --git a/README.md b/README.md index 419f5fef..07041bc2 100644 --- a/README.md +++ b/README.md @@ -54,6 +54,9 @@ source /opt/ros/$ROS_DISTRO/setup.bash vcs import src < src/rosbot/rosbot_hardware.repos +# Build only diff_drive_controller and imu_sensor_broadcaster from ros2_controllers +cp -r src/ros2_controllers/diff_drive_controller . && cp -r src/ros2_controllers/imu_sensor_broadcaster . && rm -rf src/ros2_controllers + rm -r src/rosbot_gazebo rosdep init @@ -94,6 +97,9 @@ source /opt/ros/$ROS_DISTRO/setup.bash vcs import src < src/rosbot/rosbot_hardware.repos vcs import src < src/rosbot/rosbot_simulation.repos +# Build only diff_drive_controller and imu_sensor_broadcaster from ros2_controllers +cp -r src/ros2_controllers/diff_drive_controller . && cp -r src/ros2_controllers/imu_sensor_broadcaster . && rm -rf src/ros2_controllers + rosdep init rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y From 9afd60c80e437cf7cf49589389eae969803702e5 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Tue, 5 Dec 2023 10:18:09 +0100 Subject: [PATCH 34/79] Gazebo test (#86) * Wait for velocity stabilization * Apply suggestions from code review Co-authored-by: Jakub Delicat <109142865+delihus@users.noreply.github.com> * Apply suggestions from code review * refactor * test * headless * fix sim time * update * Update rosbot_gazebo/test/test_mecanum_simulation.py Co-authored-by: Jakub Delicat <109142865+delihus@users.noreply.github.com> --------- Co-authored-by: Jakub Delicat <109142865+delihus@users.noreply.github.com> --- .pre-commit-config.yaml | 6 +-- .../test/test_diff_drive_simulation.py | 28 ++++++++----- rosbot_gazebo/test/test_mecanum_simulation.py | 41 +++++++++++-------- rosbot_gazebo/test/test_utils.py | 37 +++++++++++------ 4 files changed, 70 insertions(+), 42 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a71dc9ba..673c0755 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ repos: args: [--pytest-test-first] - repo: https://github.com/codespell-project/codespell - rev: v1.16.0 + rev: v2.2.6 hooks: - id: codespell name: codespell @@ -26,13 +26,13 @@ repos: types: [text] - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt - rev: 0.2.1 + rev: 0.2.3 hooks: - id: yamlfmt files: ^.github|./\.yaml - repo: https://github.com/psf/black - rev: 23.10.1 + rev: 23.11.0 hooks: - id: black args: ["--line-length=99", "--experimental-string-processing"] diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py index 11f769f9..135735fa 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_diff_drive_simulation.py @@ -41,9 +41,7 @@ def generate_test_description(): ] ) ), - launch_arguments={ - "headless": "True", - }.items(), + launch_arguments={"headless": "True", "world": "empty.sdf"}.items(), ) return LaunchDescription([simulation_launch]) @@ -65,18 +63,26 @@ def test_diff_drive_simulation(): # 0.9 m/s and 3.0 rad/s are controller's limits defined in # rosbot_controller/config/diff_drive_controller.yaml node.set_destination_speed(0.9, 0.0, 0.0) - controller_flag = node.controller_odom_event.wait(timeout=20.0) - ekf_flag = node.ekf_odom_event.wait(timeout=20.0) + assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting the" + f" target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) assert ( - controller_flag + node.controller_odom_flag ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" - assert ekf_flag, "ROSbot does not move properly in x direction. Check ekf_filter_node!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" node.set_destination_speed(0.0, 0.0, 3.0) - controller_flag = node.controller_odom_event.wait(timeout=20.0) - ekf_flag = node.ekf_odom_event.wait(timeout=20.0) - assert controller_flag, "ROSbot does not rotate properly. Check rosbot_base_controller!" - assert ekf_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" + assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting the" + f" target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + 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!" flag = node.scan_event.wait(timeout=20.0) assert flag, "ROSbot's lidar does not work properly!" diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py index c6b7f9be..ed6a2627 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_mecanum_simulation.py @@ -41,10 +41,7 @@ def generate_test_description(): ] ) ), - launch_arguments={ - "mecanum": "True", - "headless": "True", - }.items(), + launch_arguments={"mecanum": "True", "headless": "True", "world": "empty.sdf"}.items(), ) return LaunchDescription([simulation_launch]) @@ -66,26 +63,38 @@ def test_mecanum_simulation(): # 0.9 m/s and 3.0 rad/s are controller's limits defined in # rosbot_controller/config/mecanum_drive_controller.yaml node.set_destination_speed(0.9, 0.0, 0.0) - controller_flag = node.controller_odom_event.wait(timeout=20.0) - ekf_flag = node.ekf_odom_event.wait(timeout=20.0) + assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting the" + f" target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) assert ( - controller_flag + node.controller_odom_flag ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" - assert ekf_flag, "ROSbot does not move properly in x direction. Check ekf_filter_node!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" node.set_destination_speed(0.0, 0.9, 0.0) - controller_flag = node.controller_odom_event.wait(timeout=20.0) - ekf_flag = node.ekf_odom_event.wait(timeout=20.0) + assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting the" + f" target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) assert ( - controller_flag + node.controller_odom_flag ), "ROSbot does not move properly in y direction. Check rosbot_base_controller!" - assert ekf_flag, "ROSbot does not move properly in y direction. Check ekf_filter_node!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not move properly in y direction. Check ekf_filter_node!" node.set_destination_speed(0.0, 0.0, 3.0) - controller_flag = node.controller_odom_event.wait(timeout=20.0) - ekf_flag = node.ekf_odom_event.wait(timeout=20.0) - assert controller_flag, "ROSbot does not rotate properly. Check rosbot_base_controller!" - assert ekf_flag, "ROSbot does not rotate properly. Check ekf_filter_node!" + assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting the" + f" target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + 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!" flag = node.scan_event.wait(timeout=20.0) assert flag, "ROSbot's lidar does not work properly!" diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index 92e2ea56..ed111f40 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -37,24 +37,35 @@ class SimulationTestNode(Node): def __init__(self, name="test_node"): super().__init__(name) + + # Use simulation time to correct run on slow machine + 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 + self.goal_received_time = 1e-9 * self.get_clock().now().nanoseconds + self.vel_stabilization_time_event = Event() + self.v_x = 0.0 self.v_y = 0.0 self.v_yaw = 0.0 - self.controller_odom_event = Event() - self.ekf_odom_event = Event() + self.controller_odom_flag = False + self.ekf_odom_flag = False self.odom_tf_event = Event() self.scan_event = Event() - def clear_odom_events(self): - self.controller_odom_event.clear() - self.ekf_odom_event.clear() + def clear_odom_flag(self): + self.controller_odom_flag = False + self.ekf_odom_flag = False def set_destination_speed(self, v_x, v_y, v_yaw): - self.clear_odom_events() + self.clear_odom_flag() self.v_x = v_x self.v_y = v_y self.v_yaw = v_yaw + self.goal_received_time = 1e-9 * self.get_clock().now().nanoseconds + self.vel_stabilization_time_event.clear() def create_test_subscribers_and_publishers(self): self.cmd_vel_publisher = self.create_publisher(Twist, "cmd_vel", 10) @@ -79,9 +90,9 @@ def start_node_thread(self): self.timer = self.create_timer(1.0 / 10.0, self.timer_callback) def is_twist_ok(self, twist: Twist): - def are_close_to_each_other(true_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(true_value - dest_value) <= acceptable_range + eps + return abs(current_value - dest_value) <= acceptable_range + eps x_ok = are_close_to_each_other(twist.linear.x, self.v_x) y_ok = are_close_to_each_other(twist.linear.y, self.v_y) @@ -90,12 +101,10 @@ def are_close_to_each_other(true_value, dest_value, tolerance=self.ACCURACY, eps return x_ok and y_ok and yaw_ok def controller_callback(self, data: Odometry): - if self.is_twist_ok(data.twist.twist): - self.controller_odom_event.set() + self.controller_odom_flag = self.is_twist_ok(data.twist.twist) def ekf_callback(self, data: Odometry): - if self.is_twist_ok(data.twist.twist): - self.ekf_odom_event.set() + self.ekf_odom_flag = self.is_twist_ok(data.twist.twist) def lookup_transform_odom(self): try: @@ -108,6 +117,10 @@ def timer_callback(self): self.publish_cmd_vel_messages() self.lookup_transform_odom() + self.current_time = 1e-9 * self.get_clock().now().nanoseconds + if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY: + self.vel_stabilization_time_event.set() + def scan_callback(self, data: LaserScan): if data.ranges: self.scan_event.set() From e05b777cd1382d734a77ed3b724d6e1d8ff5c5e0 Mon Sep 17 00:00:00 2001 From: action-bot Date: Tue, 5 Dec 2023 09:30:15 +0000 Subject: [PATCH 35/79] Update changelog --- rosbot/CHANGELOG.rst | 3 +++ rosbot_bringup/CHANGELOG.rst | 3 +++ rosbot_controller/CHANGELOG.rst | 3 +++ rosbot_description/CHANGELOG.rst | 3 +++ rosbot_gazebo/CHANGELOG.rst | 3 +++ 5 files changed, 15 insertions(+) diff --git a/rosbot/CHANGELOG.rst b/rosbot/CHANGELOG.rst index b5ed5abd..8a63ecba 100644 --- a/rosbot/CHANGELOG.rst +++ b/rosbot/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rosbot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.10.4 (2023-12-01) ------------------- diff --git a/rosbot_bringup/CHANGELOG.rst b/rosbot_bringup/CHANGELOG.rst index bdf101b9..ddb0c02f 100644 --- a/rosbot_bringup/CHANGELOG.rst +++ b/rosbot_bringup/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rosbot_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.10.4 (2023-12-01) ------------------- diff --git a/rosbot_controller/CHANGELOG.rst b/rosbot_controller/CHANGELOG.rst index 01edd293..cc610ac3 100644 --- a/rosbot_controller/CHANGELOG.rst +++ b/rosbot_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rosbot_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.10.4 (2023-12-01) ------------------- diff --git a/rosbot_description/CHANGELOG.rst b/rosbot_description/CHANGELOG.rst index d0bdbf08..5f39943b 100644 --- a/rosbot_description/CHANGELOG.rst +++ b/rosbot_description/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rosbot_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.10.4 (2023-12-01) ------------------- diff --git a/rosbot_gazebo/CHANGELOG.rst b/rosbot_gazebo/CHANGELOG.rst index 5410af32..85454a55 100644 --- a/rosbot_gazebo/CHANGELOG.rst +++ b/rosbot_gazebo/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rosbot_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.10.4 (2023-12-01) ------------------- From 74ff66ef37b01228a68499a5271c8b9b47666d61 Mon Sep 17 00:00:00 2001 From: action-bot Date: Tue, 5 Dec 2023 09:30:15 +0000 Subject: [PATCH 36/79] 0.10.5 --- rosbot/CHANGELOG.rst | 4 ++-- rosbot/package.xml | 2 +- rosbot_bringup/CHANGELOG.rst | 4 ++-- rosbot_bringup/package.xml | 2 +- rosbot_controller/CHANGELOG.rst | 4 ++-- rosbot_controller/package.xml | 2 +- rosbot_description/CHANGELOG.rst | 4 ++-- rosbot_description/package.xml | 2 +- rosbot_gazebo/CHANGELOG.rst | 4 ++-- rosbot_gazebo/package.xml | 2 +- 10 files changed, 15 insertions(+), 15 deletions(-) diff --git a/rosbot/CHANGELOG.rst b/rosbot/CHANGELOG.rst index 8a63ecba..96a501b0 100644 --- a/rosbot/CHANGELOG.rst +++ b/rosbot/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rosbot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.10.5 (2023-12-05) +------------------- 0.10.4 (2023-12-01) ------------------- diff --git a/rosbot/package.xml b/rosbot/package.xml index 1e66910c..d799ba1f 100644 --- a/rosbot/package.xml +++ b/rosbot/package.xml @@ -2,7 +2,7 @@ rosbot - 0.10.4 + 0.10.5 Meta package that contains all packages of Rosbot 2 2R PRO Apache License 2.0 diff --git a/rosbot_bringup/CHANGELOG.rst b/rosbot_bringup/CHANGELOG.rst index ddb0c02f..f9fb45d4 100644 --- a/rosbot_bringup/CHANGELOG.rst +++ b/rosbot_bringup/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rosbot_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.10.5 (2023-12-05) +------------------- 0.10.4 (2023-12-01) ------------------- diff --git a/rosbot_bringup/package.xml b/rosbot_bringup/package.xml index 8f383f38..292a074b 100644 --- a/rosbot_bringup/package.xml +++ b/rosbot_bringup/package.xml @@ -2,7 +2,7 @@ rosbot_bringup - 0.10.4 + 0.10.5 ROSbot 2, 2R, PRO bringup package Apache License 2.0 diff --git a/rosbot_controller/CHANGELOG.rst b/rosbot_controller/CHANGELOG.rst index cc610ac3..9fd72ec4 100644 --- a/rosbot_controller/CHANGELOG.rst +++ b/rosbot_controller/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rosbot_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.10.5 (2023-12-05) +------------------- 0.10.4 (2023-12-01) ------------------- diff --git a/rosbot_controller/package.xml b/rosbot_controller/package.xml index d05a4e8b..a6db97d8 100644 --- a/rosbot_controller/package.xml +++ b/rosbot_controller/package.xml @@ -2,7 +2,7 @@ rosbot_controller - 0.10.4 + 0.10.5 Hardware configuration for ROSbot 2, 2R, PRO Apache License 2.0 diff --git a/rosbot_description/CHANGELOG.rst b/rosbot_description/CHANGELOG.rst index 5f39943b..18ac455f 100644 --- a/rosbot_description/CHANGELOG.rst +++ b/rosbot_description/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rosbot_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.10.5 (2023-12-05) +------------------- 0.10.4 (2023-12-01) ------------------- diff --git a/rosbot_description/package.xml b/rosbot_description/package.xml index 6e6078ee..25cafc2c 100644 --- a/rosbot_description/package.xml +++ b/rosbot_description/package.xml @@ -2,7 +2,7 @@ rosbot_description - 0.10.4 + 0.10.5 ROSbot 2, 2R, PRO description package Apache License 2.0 diff --git a/rosbot_gazebo/CHANGELOG.rst b/rosbot_gazebo/CHANGELOG.rst index 85454a55..bac68c99 100644 --- a/rosbot_gazebo/CHANGELOG.rst +++ b/rosbot_gazebo/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rosbot_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.10.5 (2023-12-05) +------------------- 0.10.4 (2023-12-01) ------------------- diff --git a/rosbot_gazebo/package.xml b/rosbot_gazebo/package.xml index 45c43767..dc2e4cae 100644 --- a/rosbot_gazebo/package.xml +++ b/rosbot_gazebo/package.xml @@ -2,7 +2,7 @@ rosbot_gazebo - 0.10.4 + 0.10.5 Gazebo Ignition simulation for ROSbot 2, 2R, PRO Apache License 2.0 From 7eab9e8832453f969568a20ca330d44bad203843 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 5 Dec 2023 13:24:24 +0100 Subject: [PATCH 37/79] Added gazebo remappings file Signed-off-by: Jakub Delicat --- .../urdf/components/vl53lox.urdf.xacro | 39 ------------------- rosbot_description/urdf/rosbot.urdf.xacro | 11 +----- .../urdf/rosbot_macro.urdf.xacro | 1 - rosbot_gazebo/config/gz_remappings.yaml | 39 +++++++++++++++++++ rosbot_gazebo/launch/spawn.launch.py | 27 +++---------- rosbot_gazebo/setup.py | 1 + 6 files changed, 47 insertions(+), 71 deletions(-) create mode 100644 rosbot_gazebo/config/gz_remappings.yaml diff --git a/rosbot_description/urdf/components/vl53lox.urdf.xacro b/rosbot_description/urdf/components/vl53lox.urdf.xacro index 83d60c8b..ad1fd22c 100644 --- a/rosbot_description/urdf/components/vl53lox.urdf.xacro +++ b/rosbot_description/urdf/components/vl53lox.urdf.xacro @@ -11,45 +11,6 @@ - - - - - - /range/${prefix} - ${prefix}_range - ${prefix}_range - - 5.0 - - - - 1 - 1 - -0.01 - 0.01 - - - - 0.01 - 0.90 - 0.02 - - - gaussian - 0.1 - 0.005 - - - true - false - - - ogre2 - - - diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index 1fdaee18..67ebfdbc 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -17,20 +17,13 @@ namespace="$(arg namespace)" use_multirobot_system="$(arg use_multirobot_system)" /> - - + - - - diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 697b8e93..3be8b361 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -155,7 +155,6 @@ - true 25 ${namespace_ext}imu/data_raw diff --git a/rosbot_gazebo/config/gz_remappings.yaml b/rosbot_gazebo/config/gz_remappings.yaml new file mode 100644 index 00000000..3fa1b7ed --- /dev/null +++ b/rosbot_gazebo/config/gz_remappings.yaml @@ -0,0 +1,39 @@ +- topic_name: "/scan" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "ignition.msgs.LaserScan" + +- topic_name: "/camera/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "ignition.msgs.CameraInfo" + +- topic_name: "/camera/depth_image" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "ignition.msgs.Image" + +- topic_name: "/camera/image" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "ignition.msgs.Image" + +- topic_name: "/camera/points" + ros_type_name: "sensor_msgs/msg/PointCloud2" + gz_type_name: "ignition.msgs.PointCloudPacked" + +- topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "ignition.msgs.Clock" + +- topic_name: "/range/fl" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "ignition.msgs.LaserScan" + +- topic_name: "/range/fr" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "ignition.msgs.LaserScan" + +- topic_name: "/range/rl" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "ignition.msgs.LaserScan" + +- topic_name: "/range/rr" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "ignition.msgs.LaserScan" diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py index fc8baab9..5d399e92 100644 --- a/rosbot_gazebo/launch/spawn.launch.py +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -60,6 +60,10 @@ def generate_launch_description(): ["'rosbot'", " if '", namespace, "' == '' ", "else ", "'", namespace, "'"] ) + gz_remappings_file = PathJoinSubstitution( + [get_package_share_directory("rosbot_gazebo"), "config", "gz_remappings.yaml"] + ) + gz_spawn_entity = Node( package="ros_gz_sim", executable="create", @@ -91,29 +95,8 @@ def generate_launch_description(): package="ros_gz_bridge", executable="parameter_bridge", name="ros_gz_bridge", - arguments=[ - "/scan@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - "/camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo", - "/camera/depth_image@sensor_msgs/msg/Image[ignition.msgs.Image", - "/camera/image@sensor_msgs/msg/Image[ignition.msgs.Image", - "/camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked", - "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", - # an IR sensor is not implemented yet https://github.com/gazebosim/gz-sensors/issues/19 - "/range/fl@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - "/range/fr@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - "/range/rl@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - "/range/rr@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan", - ], + parameters=[{"config_file": gz_remappings_file}], remappings=[ - ("/camera/camera_info", "camera/color/camera_info"), - ("/camera/image", "camera/color/image_raw"), - ("/camera/depth_image", "camera/depth/image_raw"), - ("/camera/points", "camera/depth/points"), - ("/scan", "scan"), - ("/range/fl", "range/fl"), - ("/range/fr", "range/fr"), - ("/range/rl", "range/rl"), - ("/range/rr", "range/rr"), ("/tf", "tf"), ("/tf_static", "tf_static"), ], diff --git a/rosbot_gazebo/setup.py b/rosbot_gazebo/setup.py index 0d432bd4..26443e75 100644 --- a/rosbot_gazebo/setup.py +++ b/rosbot_gazebo/setup.py @@ -26,6 +26,7 @@ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), + (os.path.join("share", package_name, "config"), glob("config/*.yaml")), ], install_requires=["setuptools"], zip_safe=True, From 967df74bed2304e40fd52d8ba5955d462f3337b3 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 5 Dec 2023 14:17:54 +0100 Subject: [PATCH 38/79] Added namespace to mappings Signed-off-by: Jakub Delicat --- rosbot_bringup/launch/bringup.launch.py | 2 +- rosbot_gazebo/config/gz_remappings.yaml | 20 ++++++++++---------- rosbot_gazebo/launch/spawn.launch.py | 18 ++++++++++++++---- rosbot_gazebo/package.xml | 1 + 4 files changed, 26 insertions(+), 15 deletions(-) diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index 4225276b..4240a09a 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -94,9 +94,9 @@ def generate_launch_description(): robot_localization_node = Node( package="robot_localization", executable="ekf_node", - name="ekf_filter_node", output="screen", parameters=[ekf_config], + arguments=["--ros-args --log-level debug"], remappings=[ ("/tf", "tf"), ("/tf_static", "tf_static"), diff --git a/rosbot_gazebo/config/gz_remappings.yaml b/rosbot_gazebo/config/gz_remappings.yaml index 3fa1b7ed..ae2790ac 100644 --- a/rosbot_gazebo/config/gz_remappings.yaml +++ b/rosbot_gazebo/config/gz_remappings.yaml @@ -1,39 +1,39 @@ -- topic_name: "/scan" +- topic_name: "/scan" ros_type_name: "sensor_msgs/msg/LaserScan" gz_type_name: "ignition.msgs.LaserScan" -- topic_name: "/camera/camera_info" +- topic_name: "/camera/camera_info" ros_type_name: "sensor_msgs/msg/CameraInfo" gz_type_name: "ignition.msgs.CameraInfo" -- topic_name: "/camera/depth_image" +- topic_name: "/camera/depth_image" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "ignition.msgs.Image" -- topic_name: "/camera/image" +- topic_name: "/camera/image" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "ignition.msgs.Image" -- topic_name: "/camera/points" +- topic_name: "/camera/points" ros_type_name: "sensor_msgs/msg/PointCloud2" gz_type_name: "ignition.msgs.PointCloudPacked" -- topic_name: "/clock" +- topic_name: "/clock" ros_type_name: "rosgraph_msgs/msg/Clock" gz_type_name: "ignition.msgs.Clock" -- topic_name: "/range/fl" +- topic_name: "/range/fl" ros_type_name: "sensor_msgs/msg/LaserScan" gz_type_name: "ignition.msgs.LaserScan" -- topic_name: "/range/fr" +- topic_name: "/range/fr" ros_type_name: "sensor_msgs/msg/LaserScan" gz_type_name: "ignition.msgs.LaserScan" -- topic_name: "/range/rl" +- topic_name: "/range/rl" ros_type_name: "sensor_msgs/msg/LaserScan" gz_type_name: "ignition.msgs.LaserScan" -- topic_name: "/range/rr" +- topic_name: "/range/rr" ros_type_name: "sensor_msgs/msg/LaserScan" gz_type_name: "ignition.msgs.LaserScan" diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py index 5d399e92..e65e92eb 100644 --- a/rosbot_gazebo/launch/spawn.launch.py +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -19,7 +19,7 @@ ) 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 from ament_index_python.packages import get_package_share_directory @@ -33,6 +33,10 @@ def generate_launch_description(): description="Namespace for all topics and tfs", ) + namespace_ext = PythonExpression( + ["''", " if '", namespace, "' == '' ", "else ", "'/", namespace, "'"] + ) + mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", @@ -64,6 +68,11 @@ def generate_launch_description(): [get_package_share_directory("rosbot_gazebo"), "config", "gz_remappings.yaml"] ) + namespaced_gz_remappings_file = ReplaceString( + source_file=gz_remappings_file, + replacements={"": (namespace_ext)}, + ) + gz_spawn_entity = Node( package="ros_gz_sim", executable="create", @@ -95,7 +104,7 @@ def generate_launch_description(): package="ros_gz_bridge", executable="parameter_bridge", name="ros_gz_bridge", - parameters=[{"config_file": gz_remappings_file}], + parameters=[{"config_file": namespaced_gz_remappings_file}], remappings=[ ("/tf", "tf"), ("/tf_static", "tf_static"), @@ -138,9 +147,10 @@ def generate_launch_description(): "1.57", "-1.57", "0.0", - # Here should be namespace "camera_depth_optical_frame", - "rosbot/base_link/camera_orbbec_astra_camera", + LaunchConfiguration( + "gazebo_frame", default=[robot_name, "/base_link/camera_orbbec_astra_camera"] + ), ], remappings=[ ("/tf", "tf"), diff --git a/rosbot_gazebo/package.xml b/rosbot_gazebo/package.xml index 17cd2223..4d08ec4b 100644 --- a/rosbot_gazebo/package.xml +++ b/rosbot_gazebo/package.xml @@ -26,6 +26,7 @@ for details refer to the ros_gz_sim package --> ros_gz_bridge ign_ros2_control + nav2_common python3-pytest launch From 4a3db5cb2af7c13beaea161b9df3a53f1c6e559f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 5 Dec 2023 15:36:20 +0100 Subject: [PATCH 39/79] fixed clock | added camera Signed-off-by: Jakub Delicat --- rosbot_description/urdf/rosbot.urdf.xacro | 3 ++- rosbot_gazebo/config/gz_remappings.yaml | 9 ++++---- rosbot_gazebo/launch/spawn.launch.py | 27 ----------------------- 3 files changed, 7 insertions(+), 32 deletions(-) diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index 0437a355..2af6796f 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -31,6 +31,7 @@ parent_link="camera_link" xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" + namespace="$(arg namespace)" simulation_engine="$(arg simulation_engine)" /> - + diff --git a/rosbot_gazebo/config/gz_remappings.yaml b/rosbot_gazebo/config/gz_remappings.yaml index ae2790ac..6f7a6730 100644 --- a/rosbot_gazebo/config/gz_remappings.yaml +++ b/rosbot_gazebo/config/gz_remappings.yaml @@ -1,3 +1,8 @@ +- topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "ignition.msgs.Clock" + direction: GZ_TO_ROS + - topic_name: "/scan" ros_type_name: "sensor_msgs/msg/LaserScan" gz_type_name: "ignition.msgs.LaserScan" @@ -18,10 +23,6 @@ ros_type_name: "sensor_msgs/msg/PointCloud2" gz_type_name: "ignition.msgs.PointCloudPacked" -- topic_name: "/clock" - ros_type_name: "rosgraph_msgs/msg/Clock" - gz_type_name: "ignition.msgs.Clock" - - topic_name: "/range/fl" ros_type_name: "sensor_msgs/msg/LaserScan" gz_type_name: "ignition.msgs.LaserScan" diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py index e65e92eb..e2efc611 100644 --- a/rosbot_gazebo/launch/spawn.launch.py +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -133,32 +133,6 @@ def generate_launch_description(): }.items(), ) - # The frame of the pointcloud from ignition gazebo 6 isn't provided by . - # See https://github.com/gazebosim/gz-sensors/issues/239 - depth_cam_frame_fixer = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="depth_to_camera", - output="log", - arguments=[ - "0.0", - "0.0", - "0.0", - "1.57", - "-1.57", - "0.0", - "camera_depth_optical_frame", - LaunchConfiguration( - "gazebo_frame", default=[robot_name, "/base_link/camera_orbbec_astra_camera"] - ), - ], - remappings=[ - ("/tf", "tf"), - ("/tf_static", "tf_static"), - ], - namespace=namespace, - ) - return LaunchDescription( [ declare_namespace_arg, @@ -171,6 +145,5 @@ def generate_launch_description(): ign_bridge, gz_spawn_entity, bringup_launch, - depth_cam_frame_fixer, ] ) From d4abcc573a86bc44ee107487c3cfde16b6b4331f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 5 Dec 2023 15:56:07 +0100 Subject: [PATCH 40/79] working without multisystem Signed-off-by: Jakub Delicat --- .../config/multi_diff_drive_controller.yaml | 8 ++--- rosbot_description/urdf/body.urdf.xacro | 2 +- .../urdf/rosbot_macro.urdf.xacro | 35 +++++++------------ rosbot_description/urdf/wheel.urdf.xacro | 11 ++---- .../launch/multirobot_simulation.launch.py | 2 +- 5 files changed, 21 insertions(+), 37 deletions(-) diff --git a/rosbot_controller/config/multi_diff_drive_controller.yaml b/rosbot_controller/config/multi_diff_drive_controller.yaml index 9b0e3a6a..cbc2e907 100644 --- a/rosbot_controller/config/multi_diff_drive_controller.yaml +++ b/rosbot_controller/config/multi_diff_drive_controller.yaml @@ -28,8 +28,8 @@ use_sim_time: True tf_frame_prefix_enable: False - left_wheel_names: ["rosbot1/fl_wheel_joint", "rosbot1/rl_wheel_joint"] - right_wheel_names: ["rosbot1/fr_wheel_joint", "rosbot1/rr_wheel_joint"] + left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] + right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] wheel_separation: 0.186 wheel_radius: 0.0425 @@ -117,8 +117,8 @@ use_sim_time: True tf_frame_prefix_enable: False - left_wheel_names: ["rosbot2/fl_wheel_joint", "rosbot2/rl_wheel_joint"] - right_wheel_names: ["rosbot2/fr_wheel_joint", "rosbot2/rr_wheel_joint"] + left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] + right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] wheel_separation: 0.186 wheel_radius: 0.0425 diff --git a/rosbot_description/urdf/body.urdf.xacro b/rosbot_description/urdf/body.urdf.xacro index 16ec6563..c178f1f8 100644 --- a/rosbot_description/urdf/body.urdf.xacro +++ b/rosbot_description/urdf/body.urdf.xacro @@ -1,7 +1,7 @@ - + diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 3be8b361..d00aa026 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -2,13 +2,6 @@ - - - - - @@ -28,13 +21,13 @@ - + - - - - + + + + @@ -85,22 +78,22 @@ - + - + - + - + @@ -131,23 +124,21 @@ - + - $(find rosbot_controller)/config/${controller_config_prefix}mecanum_drive_controller.yaml + $(find rosbot_controller)/config/mecanum_drive_controller.yaml - $(find rosbot_controller)/config/${controller_config_prefix}diff_drive_controller.yaml + $(find rosbot_controller)/config/diff_drive_controller.yaml - - ${namespace} - + ${namespace} rosbot_base_controller/cmd_vel_unstamped:=cmd_vel /tf:=tf diff --git a/rosbot_description/urdf/wheel.urdf.xacro b/rosbot_description/urdf/wheel.urdf.xacro index 770bb2e0..e9d5057e 100644 --- a/rosbot_description/urdf/wheel.urdf.xacro +++ b/rosbot_description/urdf/wheel.urdf.xacro @@ -1,14 +1,7 @@ - - - - - - - - + @@ -65,7 +58,7 @@ - + diff --git a/rosbot_gazebo/launch/multirobot_simulation.launch.py b/rosbot_gazebo/launch/multirobot_simulation.launch.py index ec846c68..1028b7f4 100644 --- a/rosbot_gazebo/launch/multirobot_simulation.launch.py +++ b/rosbot_gazebo/launch/multirobot_simulation.launch.py @@ -120,7 +120,7 @@ def generate_launch_description(): ), launch_arguments={ "mecanum": mecanum, - "use_multirobot_system": "True", + "use_multirobot_system": "False", "use_sim": "True", "use_gpu": use_gpu, "simulation_engine": "ignition-gazebo", From cba7f16b6201c4cbe961c35c4d716d11193579e0 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 27 Dec 2023 11:58:54 +0100 Subject: [PATCH 41/79] parse robots arg | removed delays from rosbot_controller Signed-off-by: Jakub Delicat --- rosbot_bringup/launch/bringup.launch.py | 9 - .../config/multi_diff_drive_controller.yaml | 180 ----------------- .../multi_mecanum_drive_controller.yaml | 191 ------------------ rosbot_controller/launch/controller.launch.py | 35 +--- rosbot_description/urdf/rosbot.urdf.xacro | 4 +- .../urdf/rosbot_macro.urdf.xacro | 17 +- .../launch/multirobot_simulation.launch.py | 109 +++++----- rosbot_gazebo/launch/simulation.launch.py | 11 +- rosbot_gazebo/launch/spawn.launch.py | 9 - 9 files changed, 66 insertions(+), 499 deletions(-) delete mode 100644 rosbot_controller/config/multi_diff_drive_controller.yaml delete mode 100644 rosbot_controller/config/multi_mecanum_drive_controller.yaml diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index b1e88081..dcf8eade 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -62,13 +62,6 @@ def generate_launch_description(): ), ) - use_multirobot_system = LaunchConfiguration("use_multirobot_system") - declare_use_multirobot_system_arg = DeclareLaunchArgument( - "use_multirobot_system", - default_value="false", - description="Enable correct Ignition Gazebo configuration in URDF", - ) - controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -85,7 +78,6 @@ def generate_launch_description(): "use_gpu": use_gpu, "simulation_engine": simulation_engine, "namespace": namespace, - "use_multirobot_system": use_multirobot_system, }.items(), ) @@ -109,7 +101,6 @@ def generate_launch_description(): declare_use_sim_arg, declare_use_gpu_arg, declare_simulation_engine_arg, - declare_use_multirobot_system_arg, SetParameter(name="use_sim_time", value=use_sim), controller_launch, robot_localization_node, diff --git a/rosbot_controller/config/multi_diff_drive_controller.yaml b/rosbot_controller/config/multi_diff_drive_controller.yaml deleted file mode 100644 index cbc2e907..00000000 --- a/rosbot_controller/config/multi_diff_drive_controller.yaml +++ /dev/null @@ -1,180 +0,0 @@ -# This file has controllers configuration for 2 robots because the ign_ros2_control::IgnitionROS2ControlPlugin -# parameter parses only one file. -# -/rosbot1/controller_manager: - ros__parameters: - use_sim_time: True - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: diff_drive_controller/DiffDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -/rosbot1/imu_broadcaster: - ros__parameters: - use_sim_time: True - sensor_name: rosbot1/imu - tf_frame_prefix_enable: False - frame_id: imu_link - static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet - static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally - static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally - -/rosbot1/rosbot_base_controller: - ros__parameters: - use_sim_time: True - tf_frame_prefix_enable: False - - left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] - right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] - - wheel_separation: 0.186 - wheel_radius: 0.0425 - - # For skid drive kinematics it will act as ICR coefficient - # Kinematic model with ICR coefficient isn't totally accurate and this coefficient can differ - # for various ground types, but checking on our office floor 1.3 looked approximately alright - wheel_separation_multiplier: 1.45 - - left_wheel_radius_multiplier: 1.0 - right_wheel_radius_multiplier: 1.0 - - # Feedback from motors is published at around 20 Hz - publish_rate: 20.0 - odom_frame_id: odom - base_frame_id: base_link - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally - - # Whether to use feedback or commands for odometry calculations - open_loop: false - - # Update odometry from velocity - #! IMPORTANT at the time of writing it is bugged on deb galactic version and require fork of diff drive controller installed from sources - # in sensor fusion only velocity is used and with this setting it is more accurate - position_feedback: false - # velocity computation filtering - velocity_rolling_window_size: 1 - - enable_odom_tf: false - - cmd_vel_timeout: 0.5 - #publish_limited_velocity: true - use_stamped_vel: false - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - - angular: - z: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - max_velocity: 3.0 # rad/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 4.0 # rad/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # rad/s^3 - -/rosbot2/controller_manager: - ros__parameters: - use_sim_time: True - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: diff_drive_controller/DiffDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -/rosbot2/imu_broadcaster: - ros__parameters: - use_sim_time: True - sensor_name: rosbot2/imu - tf_frame_prefix_enable: False - frame_id: imu_link - static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet - static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally - static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally - -/rosbot2/rosbot_base_controller: - ros__parameters: - use_sim_time: True - tf_frame_prefix_enable: False - - left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] - right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] - - wheel_separation: 0.186 - wheel_radius: 0.0425 - - # For skid drive kinematics it will act as ICR coefficient - # Kinematic model with ICR coefficient isn't totally accurate and this coefficient can differ - # for various ground types, but checking on our office floor 1.3 looked approximately alright - wheel_separation_multiplier: 1.45 - - left_wheel_radius_multiplier: 1.0 - right_wheel_radius_multiplier: 1.0 - - # Feedback from motors is published at around 20 Hz - publish_rate: 20.0 - odom_frame_id: odom - base_frame_id: base_link - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally - - # Whether to use feedback or commands for odometry calculations - open_loop: false - - # Update odometry from velocity - #! IMPORTANT at the time of writing it is bugged on deb galactic version and require fork of diff drive controller installed from sources - # in sensor fusion only velocity is used and with this setting it is more accurate - position_feedback: false - # velocity computation filtering - velocity_rolling_window_size: 1 - - enable_odom_tf: false - - cmd_vel_timeout: 0.5 - #publish_limited_velocity: true - use_stamped_vel: false - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - - angular: - z: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - max_velocity: 3.0 # rad/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 4.0 # rad/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # rad/s^3 diff --git a/rosbot_controller/config/multi_mecanum_drive_controller.yaml b/rosbot_controller/config/multi_mecanum_drive_controller.yaml deleted file mode 100644 index f352830e..00000000 --- a/rosbot_controller/config/multi_mecanum_drive_controller.yaml +++ /dev/null @@ -1,191 +0,0 @@ -/rosbot1/controller_manager: - ros__parameters: - use_sim_time: false - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: mecanum_drive_controller/MecanumDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -/rosbot1/imu_broadcaster: - ros__parameters: - sensor_name: rosbot1/imu - tf_frame_prefix_enable: False - frame_id: imu_link - static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet - static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive - static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive - -/rosbot1/rosbot_base_controller: - ros__parameters: - tf_frame_prefix_enable: False - front_left_wheel_name: rosbot1/fl_wheel_joint - front_right_wheel_name: rosbot1/fr_wheel_joint - rear_left_wheel_name: rosbot1/rl_wheel_joint - rear_right_wheel_name: rosbot1/rr_wheel_joint - - wheel_separation_x: 0.106 - wheel_separation_y: 0.192 - wheel_radius: 0.047 - - - wheel_separation_multiplier_x: 1.0 - wheel_separation_multiplier_y: 1.0 - - wheel_radius_multiplier: 1.0 - - # Feedback from motors is published at around 20 Hz - publish_rate: 25.0 - odom_frame_id: odom - base_frame_id: base_link - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally - - # Whether to use feedback or commands for odometry calculations - open_loop: false - - # Update odometry from velocity - # in sensor fusion only velocity is used and with this setting it is more accurate - position_feedback: false - # velocity computation filtering - velocity_rolling_window_size: 1 - - enable_odom_tf: false - - cmd_vel_timeout: 0.5 - #publish_limited_velocity: true - use_stamped_vel: false - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - y: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - - angular: - z: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - max_velocity: 3.0 # rad/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 4.0 # rad/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # rad/s^3 - -/rosbot2/controller_manager: - ros__parameters: - use_sim_time: false - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: mecanum_drive_controller/MecanumDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - -/rosbot2/imu_broadcaster: - ros__parameters: - sensor_name: rosbot2/imu - tf_frame_prefix_enable: False - frame_id: imu_link - static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet - static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values based on diff_drive - static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values based on diff_drive - -/rosbot2/rosbot_base_controller: - ros__parameters: - tf_frame_prefix_enable: False - front_left_wheel_name: rosbot2/fl_wheel_joint - front_right_wheel_name: rosbot2/fr_wheel_joint - rear_left_wheel_name: rosbot2/rl_wheel_joint - rear_right_wheel_name: rosbot2/rr_wheel_joint - - wheel_separation_x: 0.106 - wheel_separation_y: 0.192 - wheel_radius: 0.047 - - - wheel_separation_multiplier_x: 1.0 - wheel_separation_multiplier_y: 1.0 - - wheel_radius_multiplier: 1.0 - - # Feedback from motors is published at around 20 Hz - publish_rate: 25.0 - odom_frame_id: odom - base_frame_id: base_link - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] - twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally - - # Whether to use feedback or commands for odometry calculations - open_loop: false - - # Update odometry from velocity - # in sensor fusion only velocity is used and with this setting it is more accurate - position_feedback: false - # velocity computation filtering - velocity_rolling_window_size: 1 - - enable_odom_tf: false - - cmd_vel_timeout: 0.5 - #publish_limited_velocity: true - use_stamped_vel: false - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - y: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - # top speed is around 1.2m/s - max_velocity: 0.9 # m/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 1.0 # m/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # m/s^3 - - angular: - z: - has_velocity_limits: true - has_acceleration_limits: true - has_jerk_limits: false - max_velocity: 3.0 # rad/s - # min_velocity - When unspecified, -max_velocity is used - max_acceleration: 4.0 # rad/s^2 - # min_acceleration - When unspecified, -max_acceleration is used. - max_jerk: 0.0 # rad/s^3 diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index 779a3b47..e94a3b9a 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -14,9 +14,8 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import RegisterEventHandler, DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument from launch.conditions import UnlessCondition -from launch.event_handlers import OnProcessExit from launch.substitutions import ( Command, PythonExpression, @@ -68,13 +67,6 @@ def generate_launch_description(): choices=["ignition-gazebo", "gazebo-classic", "webots"], ) - use_multirobot_system = LaunchConfiguration("use_multirobot_system") - declare_use_multirobot_system_arg = DeclareLaunchArgument( - "use_multirobot_system", - default_value="false", - description="Enable correct Ignition Gazebo configuration in URDF", - ) - controller_config_name = PythonExpression( [ "'mecanum_drive_controller.yaml' if ", @@ -86,6 +78,7 @@ def generate_launch_description(): namespace_ext = PythonExpression( ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"] ) + controller_manager_name = LaunchConfiguration( "controller_manager_name", default=[namespace_ext, "controller_manager"], @@ -113,8 +106,6 @@ def generate_launch_description(): simulation_engine, " namespace:=", namespace, - " use_multirobot_system:=", - use_multirobot_system, ] ) robot_description = {"robot_description": robot_description_content} @@ -182,14 +173,6 @@ def generate_launch_description(): ], ) - # Delay start of robot_controller after joint_state_broadcaster - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) - ) - imu_broadcaster_spawner = Node( package="controller_manager", executable="spawner", @@ -204,15 +187,6 @@ def generate_launch_description(): ], ) - # Delay start of imu_broadcaster after robot_controller - # when spawning without delay ros2_control_node sometimes crashed - delay_imu_broadcaster_spawner_after_robot_controller_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=robot_controller_spawner, - on_exit=[imu_broadcaster_spawner], - ) - ) - return LaunchDescription( [ declare_namespace_arg, @@ -220,12 +194,11 @@ def generate_launch_description(): declare_use_sim_arg, declare_use_gpu_arg, declare_simulation_engine_arg, - declare_use_multirobot_system_arg, SetParameter("use_sim_time", value=use_sim), control_node, robot_state_pub_node, joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - delay_imu_broadcaster_spawner_after_robot_controller_spawner, + robot_controller_spawner, + imu_broadcaster_spawner, ] ) diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index 2af6796f..78ca5b19 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -6,7 +6,6 @@ - + namespace="$(arg namespace)" /> - + - + + + @@ -123,13 +125,6 @@ - - - - - - - $(find rosbot_controller)/config/mecanum_drive_controller.yaml @@ -138,7 +133,7 @@ - ${namespace} + ${gz_control_namespace} rosbot_base_controller/cmd_vel_unstamped:=cmd_vel /tf:=tf @@ -155,7 +150,7 @@ imu_link - ${namespace} + ${namespace_ext} diff --git a/rosbot_gazebo/launch/multirobot_simulation.launch.py b/rosbot_gazebo/launch/multirobot_simulation.launch.py index 1028b7f4..b1bf9aa1 100644 --- a/rosbot_gazebo/launch/multirobot_simulation.launch.py +++ b/rosbot_gazebo/launch/multirobot_simulation.launch.py @@ -13,11 +13,17 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, TimerAction +from launch.actions import ( + IncludeLaunchDescription, + DeclareLaunchArgument, + LogInfo, + GroupAction, +) from launch.substitutions import ( PathJoinSubstitution, PythonExpression, LaunchConfiguration, + TextSubstitution, ) from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -25,6 +31,8 @@ from ament_index_python.packages import get_package_share_directory +from nav2_common.launch import ParseMultiRobotPose + def generate_launch_description(): mecanum = LaunchConfiguration("mecanum") @@ -54,7 +62,7 @@ def generate_launch_description(): [ "'--headless-rendering -s -v 4 -r' if ", headless, - " else '-r -v 4 '", + " else '-r'", ] ) gz_args = [headless_cfg, " ", world_cfg] @@ -82,59 +90,51 @@ def generate_launch_description(): }.items(), ) - spawn_robot1_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("rosbot_gazebo"), - "launch", - "spawn.launch.py", - ] - ) - ), - launch_arguments={ - "mecanum": mecanum, - "use_multirobot_system": "True", - "use_sim": "True", - "use_gpu": use_gpu, - "simulation_engine": "ignition-gazebo", - "namespace": "rosbot1", - "x": LaunchConfiguration("x1", default="0.00"), - "y": LaunchConfiguration("y1", default="2.00"), - "z": LaunchConfiguration("z1", default="0.20"), - "roll": LaunchConfiguration("roll1", default="0.00"), - "pitch": LaunchConfiguration("pitch1", default="0.00"), - "yaw": LaunchConfiguration("yaw1", default="0.00"), - }.items(), - ) + robots_list = ParseMultiRobotPose("robots").value() - spawn_robot2_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("rosbot_gazebo"), - "launch", - "spawn.launch.py", - ] - ) - ), - launch_arguments={ - "mecanum": mecanum, - "use_multirobot_system": "False", - "use_sim": "True", - "use_gpu": use_gpu, - "simulation_engine": "ignition-gazebo", - "namespace": "rosbot2", - "x": LaunchConfiguration("x2", default="1.00"), - "y": LaunchConfiguration("y2", default="2.00"), - "z": LaunchConfiguration("z2", default="0.20"), - "roll": LaunchConfiguration("roll2", default="0.00"), - "pitch": LaunchConfiguration("pitch2", default="0.00"), - "yaw": LaunchConfiguration("yaw2", default="0.00"), - }.items(), - ) + # Define commands for launching the navigation instances + bringup_cmd_group = [] + for robot_name in robots_list: + init_pose = robots_list[robot_name] + group = GroupAction( + [ + LogInfo( + msg=[ + "Launching namespace=", + robot_name, + " init_pose=", + str(init_pose), + ] + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("rosbot_gazebo"), + "launch", + "spawn.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": mecanum, + "use_sim": "True", + "use_gpu": use_gpu, + "simulation_engine": "ignition-gazebo", + "namespace": TextSubstitution(text=robot_name), + "x": TextSubstitution(text=str(init_pose["x"])), + "y": TextSubstitution(text=str(init_pose["y"])), + "z": TextSubstitution(text=str(init_pose["z"])), + "roll": TextSubstitution(text=str(init_pose["roll"])), + "pitch": TextSubstitution(text=str(init_pose["pitch"])), + "yaw": TextSubstitution(text=str(init_pose["yaw"])), + }.items(), + ), + ] + ) + + bringup_cmd_group.append(group) - delayed_spawn_robot2 = TimerAction(period=10.0, actions=[spawn_robot2_launch]) return LaunchDescription( [ declare_mecanum_arg, @@ -145,7 +145,6 @@ def generate_launch_description(): # (doesn't work for nodes started from ignition gazebo) SetParameter(name="use_sim_time", value=True), gz_sim, - spawn_robot1_launch, - delayed_spawn_robot2, ] + + bringup_cmd_group ) diff --git a/rosbot_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py index be66c2d5..220a8947 100644 --- a/rosbot_gazebo/launch/simulation.launch.py +++ b/rosbot_gazebo/launch/simulation.launch.py @@ -46,13 +46,6 @@ def generate_launch_description(): ), ) - use_multirobot_system = LaunchConfiguration("use_multirobot_system") - declare_use_multirobot_system_arg = DeclareLaunchArgument( - "use_multirobot_system", - default_value="false", - description="Enable correct Ignition Gazebo configuration in URDF", - ) - world_package = get_package_share_directory("husarion_office_gz") world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"]) world_cfg = LaunchConfiguration("world") @@ -71,7 +64,7 @@ def generate_launch_description(): [ "'--headless-rendering -s -v 4 -r' if ", headless, - " else '-r -v 4 '", + " else '-r'", ] ) gz_args = [headless_cfg, " ", world_cfg] @@ -111,7 +104,6 @@ def generate_launch_description(): ), launch_arguments={ "mecanum": mecanum, - "use_multirobot_system": use_multirobot_system, "use_sim": "True", "use_gpu": use_gpu, "simulation_engine": "ignition-gazebo", @@ -128,7 +120,6 @@ def generate_launch_description(): return LaunchDescription( [ declare_namespace_arg, - declare_use_multirobot_system_arg, declare_mecanum_arg, declare_world_arg, declare_headless_arg, diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py index e2efc611..fb6723f7 100644 --- a/rosbot_gazebo/launch/spawn.launch.py +++ b/rosbot_gazebo/launch/spawn.launch.py @@ -46,13 +46,6 @@ def generate_launch_description(): ), ) - use_multirobot_system = LaunchConfiguration("use_multirobot_system") - declare_use_multirobot_system_arg = DeclareLaunchArgument( - "use_multirobot_system", - default_value="false", - description="Enable correct Ignition Gazebo configuration in URDF", - ) - use_gpu = LaunchConfiguration("use_gpu") declare_use_gpu_arg = DeclareLaunchArgument( "use_gpu", @@ -129,7 +122,6 @@ def generate_launch_description(): "use_gpu": use_gpu, "simulation_engine": "ignition-gazebo", "namespace": namespace, - "use_multirobot_system": use_multirobot_system, }.items(), ) @@ -137,7 +129,6 @@ def generate_launch_description(): [ declare_namespace_arg, declare_mecanum_arg, - declare_use_multirobot_system_arg, declare_use_gpu_arg, # Sets use_sim_time for all nodes started below # (doesn't work for nodes started from ignition gazebo) From d11a464d01b9257e0b1221263d792e9cd33c1019 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 27 Dec 2023 15:06:23 +0100 Subject: [PATCH 42/79] TEsting Signed-off-by: Jakub Delicat --- rosbot_gazebo/test/test_diff_drive_simulation.py | 5 ++++- rosbot_gazebo/test/test_mecanum_simulation.py | 6 +++++- rosbot_gazebo/test/test_multirobot_simulation.py | 4 +++- .../test/test_namespaced_diff_drive_simulation.py | 2 +- ...copy.py => test_namespaced_mecanum_simulation.py} | 2 +- rosbot_gazebo/test/test_utils.py | 12 +++++++----- 6 files changed, 21 insertions(+), 10 deletions(-) rename rosbot_gazebo/test/{test_namespaced_mecanum_simulation copy.py => test_namespaced_mecanum_simulation.py} (99%) diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py index 135735fa..453c07ca 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_diff_drive_simulation.py @@ -41,7 +41,10 @@ def generate_test_description(): ] ) ), - launch_arguments={"headless": "True", "world": "empty.sdf"}.items(), + launch_arguments={ + # "headless": "True", + "world": "empty.sdf" + }.items(), ) return LaunchDescription([simulation_launch]) diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py index ed6a2627..8e496c2c 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_mecanum_simulation.py @@ -41,7 +41,11 @@ def generate_test_description(): ] ) ), - launch_arguments={"mecanum": "True", "headless": "True", "world": "empty.sdf"}.items(), + launch_arguments={ + "mecanum": "True", + # "headless": "True", + "world": "empty.sdf", + }.items(), ) return LaunchDescription([simulation_launch]) diff --git a/rosbot_gazebo/test/test_multirobot_simulation.py b/rosbot_gazebo/test/test_multirobot_simulation.py index 466e1e68..59385ab9 100644 --- a/rosbot_gazebo/test/test_multirobot_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_simulation.py @@ -42,7 +42,9 @@ def generate_test_description(): ) ), launch_arguments={ - "robots": "robot1={x: 0.0}; robot2={x: 1.0}; robot3={x: 2.0}; robot4={x: 3.0}" + "robots": "robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}", + "world": "empty.sdf", + # "headless": True }.items(), ) diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py index bee20309..7ca7d506 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py @@ -42,7 +42,7 @@ def generate_test_description(): ) ), launch_arguments={ - "headless": "True", + # "headless": "True", "world": "empty.sdf", "namespace": "rosbot2r", }.items(), diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation copy.py b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py similarity index 99% rename from rosbot_gazebo/test/test_namespaced_mecanum_simulation copy.py rename to rosbot_gazebo/test/test_namespaced_mecanum_simulation.py index e477cd5a..f63a9bbb 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum_simulation copy.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py @@ -43,7 +43,7 @@ def generate_test_description(): ), launch_arguments={ "mecanum": "True", - "headless": "True", + # "headless": "True", "world": "empty.sdf", "namespace": "rosbot2r", }.items(), diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index ed111f40..8fd78093 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -35,14 +35,15 @@ class SimulationTestNode(Node): # cause the rosbot_base_controller to determine inaccurate odometry. ACCURACY = 0.10 # 10% accuracy - def __init__(self, name="test_node"): - super().__init__(name) + 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) self.set_parameters([use_sim_time]) self.VELOCITY_STABILIZATION_DELAY = 3 + self.current_time = 1e-9 * self.get_clock().now().nanoseconds self.goal_received_time = 1e-9 * self.get_clock().now().nanoseconds self.vel_stabilization_time_event = Event() @@ -71,11 +72,11 @@ def create_test_subscribers_and_publishers(self): self.cmd_vel_publisher = self.create_publisher(Twist, "cmd_vel", 10) self.controller_odom_sub = self.create_subscription( - Odometry, "/rosbot_base_controller/odom", self.controller_callback, 10 + Odometry, "rosbot_base_controller/odom", self.controller_callback, 10 ) self.ekf_odom_sub = self.create_subscription( - Odometry, "/odometry/filtered", self.ekf_callback, 10 + Odometry, "odometry/filtered", self.ekf_callback, 10 ) self.scan_sub = self.create_subscription(LaserScan, "/scan", self.scan_callback, 10) @@ -104,6 +105,7 @@ def controller_callback(self, data: Odometry): self.controller_odom_flag = self.is_twist_ok(data.twist.twist) def ekf_callback(self, data: Odometry): + self.odom_tf_event.set() self.ekf_odom_flag = self.is_twist_ok(data.twist.twist) def lookup_transform_odom(self): @@ -115,7 +117,7 @@ def lookup_transform_odom(self): def timer_callback(self): self.publish_cmd_vel_messages() - self.lookup_transform_odom() + # self.lookup_transform_odom() self.current_time = 1e-9 * self.get_clock().now().nanoseconds if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY: From aee0756d360b856dbd68c36074cba9d79cd14e7f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 28 Dec 2023 09:59:39 +0100 Subject: [PATCH 43/79] fixed imu while testing Signed-off-by: Jakub Delicat --- .../test/test_diff_drive_simulation.py | 8 ++++++- rosbot_gazebo/test/test_mecanum_simulation.py | 8 ++++++- .../test/test_multirobot_simulation.py | 8 ++++++- .../test_namespaced_diff_drive_simulation.py | 8 ++++++- .../test_namespaced_mecanum_simulation.py | 8 ++++++- rosbot_gazebo/test/test_utils.py | 21 ++++++------------- 6 files changed, 41 insertions(+), 20 deletions(-) diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py index 453c07ca..835da43b 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_diff_drive_simulation.py @@ -43,7 +43,13 @@ def generate_test_description(): ), launch_arguments={ # "headless": "True", - "world": "empty.sdf" + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ) }.items(), ) diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py index 8e496c2c..11a7b21b 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_mecanum_simulation.py @@ -44,7 +44,13 @@ def generate_test_description(): launch_arguments={ "mecanum": "True", # "headless": "True", - "world": "empty.sdf", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), }.items(), ) diff --git a/rosbot_gazebo/test/test_multirobot_simulation.py b/rosbot_gazebo/test/test_multirobot_simulation.py index 59385ab9..54ca5c25 100644 --- a/rosbot_gazebo/test/test_multirobot_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_simulation.py @@ -43,7 +43,13 @@ def generate_test_description(): ), launch_arguments={ "robots": "robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}", - "world": "empty.sdf", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ) # "headless": True }.items(), ) diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py index 7ca7d506..c1e0bd5c 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py @@ -43,7 +43,13 @@ def generate_test_description(): ), launch_arguments={ # "headless": "True", - "world": "empty.sdf", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), "namespace": "rosbot2r", }.items(), ) diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py index f63a9bbb..381d9196 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py @@ -43,8 +43,14 @@ def generate_test_description(): ), launch_arguments={ "mecanum": "True", + "world": PathJoinSubstitution( + [ + get_package_share_directory("husarion_office_gz"), + "worlds", + "empty_with_plugins.sdf", + ] + ), # "headless": "True", - "world": "empty.sdf", "namespace": "rosbot2r", }.items(), ) diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index 8fd78093..24a5687f 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -24,10 +24,6 @@ from nav_msgs.msg import Odometry from sensor_msgs.msg import LaserScan -from tf2_ros import TransformException -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener - class SimulationTestNode(Node): __test__ = False @@ -79,9 +75,7 @@ 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.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) + self.scan_sub = self.create_subscription(LaserScan, "scan", self.scan_callback, 10) self.timer = None @@ -102,28 +96,24 @@ def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, return x_ok and y_ok and yaw_ok def controller_callback(self, data: Odometry): + self.get_logger().info(f"Received twist from controller: {data.twist.twist}") self.controller_odom_flag = self.is_twist_ok(data.twist.twist) def ekf_callback(self, data: Odometry): + self.get_logger().info(f"Received twist filtered: {data.twist.twist}") + self.odom_tf_event.set() self.ekf_odom_flag = self.is_twist_ok(data.twist.twist) - def lookup_transform_odom(self): - try: - self.tf_buffer.lookup_transform("odom", "base_link", rclpy.time.Time()) - self.odom_tf_event.set() - except TransformException as ex: - self.get_logger().error(f"Could not transform odom to base_link: {ex}") - def timer_callback(self): self.publish_cmd_vel_messages() - # self.lookup_transform_odom() self.current_time = 1e-9 * self.get_clock().now().nanoseconds if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY: self.vel_stabilization_time_event.set() def scan_callback(self, data: LaserScan): + self.get_logger().info(f"Received scan length: {len(data.ranges)}") if data.ranges: self.scan_event.set() @@ -134,4 +124,5 @@ def publish_cmd_vel_messages(self): twist_msg.linear.y = self.v_y twist_msg.angular.z = self.v_yaw + self.get_logger().info(f"Publishing twist: {twist_msg}") self.cmd_vel_publisher.publish(twist_msg) From b88cbd928a87e61d06af041f017fc0d06970a520 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 28 Dec 2023 14:20:47 +0100 Subject: [PATCH 44/79] Multirobots tests Signed-off-by: Jakub Delicat --- .../test_multirobot_diff_drive_simulation.py | 110 ++++++++++++++++++ ... => test_multirobot_mecanum_simulation.py} | 102 +++++++++------- .../test_namespaced_diff_drive_simulation.py | 4 +- .../test_namespaced_mecanum_simulation.py | 6 +- rosbot_gazebo/test/test_utils.py | 12 +- 5 files changed, 179 insertions(+), 55 deletions(-) create mode 100644 rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py rename rosbot_gazebo/test/{test_multirobot_simulation.py => test_multirobot_mecanum_simulation.py} (59%) diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py new file mode 100644 index 00000000..ba30c54d --- /dev/null +++ b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -0,0 +1,110 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from rclpy.executors import MultiThreadedExecutor +from threading import Thread +from test_utils import SimulationTestNode +from test_ign_kill_utils import kill_ign_linux_processes + + +@launch_pytest.fixture +def generate_test_description(): + # IncludeLaunchDescription does not work with robots argument + simulation_launch = ExecuteProcess( + cmd=[ + "ros2", + "launch", + "rosbot_gazebo", + "multirobot_simulation.launch.py", + ( + f'world:={get_package_share_directory("husarion_office_gz")}' + "/worlds/empty_with_plugins.sdf" + ), + "robots:=robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}", + ], + output="screen", + ) + + return LaunchDescription([simulation_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_simulation(): + robot_names = ["robot1", "robot2", "robot3", "robot4"] + rclpy.init() + try: + nodes = [] + executor = MultiThreadedExecutor(num_threads=len(robot_names)) + + for robot_name in robot_names: + node = SimulationTestNode("test_simulation", namespace=robot_name) + node.create_test_subscribers_and_publishers() + nodes.append(node) + executor.add_node(node) + + ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) + ros_spin_thread.start() + + for node in nodes: + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/mecanum_drive_controller.yaml + node.set_destination_speed(0.9, 0.0, 0.0) + + for node in nodes: + assert node.vel_stabilization_time_event.wait(timeout=60.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting" + f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" + assert ( + node.ekf_odom_flag + ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" + + for node in nodes: + node.set_destination_speed(0.0, 0.0, 3.0) + + for node in nodes: + assert node.vel_stabilization_time_event.wait(timeout=60.0), ( + "The simulation is running slowly or has crashed! The time elapsed since setting" + f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), f"{robot_name} does not rotate properly. Check rosbot_base_controller!" + assert ( + node.ekf_odom_flag + ), f"{robot_name} does not rotate properly. Check ekf_filter_node!" + + flag = node.scan_event.wait(timeout=20.0) + assert flag, f"{robot_name}'s lidar does not work properly!" + + node.destroy_node() + + finally: + rclpy.shutdown() + + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() diff --git a/rosbot_gazebo/test/test_multirobot_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py similarity index 59% rename from rosbot_gazebo/test/test_multirobot_simulation.py rename to rosbot_gazebo/test/test_multirobot_mecanum_simulation.py index 54ca5c25..8beb02d7 100644 --- a/rosbot_gazebo/test/test_multirobot_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py @@ -20,57 +20,59 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource - +from launch.actions import ExecuteProcess +from rclpy.executors import MultiThreadedExecutor +from threading import Thread from test_utils import SimulationTestNode from test_ign_kill_utils import kill_ign_linux_processes @launch_pytest.fixture def generate_test_description(): - rosbot_gazebo = get_package_share_directory("rosbot_gazebo") - simulation_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_gazebo, - "launch", - "multirobot_simulation.launch.py", - ] - ) - ), - launch_arguments={ - "robots": "robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}", - "world": PathJoinSubstitution( - [ - get_package_share_directory("husarion_office_gz"), - "worlds", - "empty_with_plugins.sdf", - ] - ) - # "headless": True - }.items(), + # IncludeLaunchDescription does not work with robots argument + simulation_launch = ExecuteProcess( + cmd=[ + "ros2", + "launch", + "rosbot_gazebo", + "multirobot_simulation.launch.py", + ( + f'world:={get_package_share_directory("husarion_office_gz")}' + "/worlds/empty_with_plugins.sdf" + ), + "robots:=robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}", + "mecanum:=True", + ], + output="screen", ) return LaunchDescription([simulation_launch]) @pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_mecanum_simulation(): +def test_multirobot_simulation(): robot_names = ["robot1", "robot2", "robot3", "robot4"] - for robot_name in robot_names: - rclpy.init() - try: + rclpy.init() + try: + nodes = [] + executor = MultiThreadedExecutor(num_threads=len(robot_names)) + + for robot_name in robot_names: node = SimulationTestNode("test_simulation", namespace=robot_name) node.create_test_subscribers_and_publishers() - node.start_node_thread() + nodes.append(node) + executor.add_node(node) + ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) + ros_spin_thread.start() + + for node in nodes: # 0.9 m/s and 3.0 rad/s are controller's limits defined in # rosbot_controller/config/mecanum_drive_controller.yaml node.set_destination_speed(0.9, 0.0, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + + for node in nodes: + assert node.vel_stabilization_time_event.wait(timeout=60.0), ( "The simulation is running slowly or has crashed! The time elapsed since setting" f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." ) @@ -81,34 +83,46 @@ def test_namespaced_mecanum_simulation(): node.ekf_odom_flag ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" + for node in nodes: + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/mecanum_drive_controller.yaml node.set_destination_speed(0.0, 0.9, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + + for node in nodes: + assert node.vel_stabilization_time_event.wait(timeout=60.0), ( "The simulation is running slowly or has crashed! The time elapsed since setting" f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." ) assert ( node.controller_odom_flag - ), "ROSbot does not move properly in y direction. Check rosbot_base_controller!" + ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" assert ( node.ekf_odom_flag - ), "ROSbot does not move properly in y direction. Check ekf_filter_node!" + ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" + for node in nodes: node.set_destination_speed(0.0, 0.0, 3.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + + for node in nodes: + assert node.vel_stabilization_time_event.wait(timeout=60.0), ( "The simulation is running slowly or has crashed! The time elapsed since setting" f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." ) 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!" + ), f"{robot_name} does not rotate properly. Check rosbot_base_controller!" + assert ( + node.ekf_odom_flag + ), f"{robot_name} 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!" + assert flag, f"{robot_name}'s lidar does not work properly!" + + node.destroy_node() - finally: - # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching - # several tests in a row. - rclpy.shutdown() + finally: + rclpy.shutdown() - kill_ign_linux_processes() + # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching + # several tests in a row. + kill_ign_linux_processes() diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py index c1e0bd5c..ff1847fd 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py @@ -68,7 +68,7 @@ def test_namespaced_diff_drive_simulation(): # 0.9 m/s and 3.0 rad/s are controller's limits defined in # rosbot_controller/config/diff_drive_controller.yaml node.set_destination_speed(0.9, 0.0, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + assert node.vel_stabilization_time_event.wait(timeout=40.0), ( "The simulation is running slowly or has crashed! The time elapsed since setting the" f" target speed is: {(node.current_time - node.goal_received_time):.1f}." ) @@ -80,7 +80,7 @@ def test_namespaced_diff_drive_simulation(): ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" node.set_destination_speed(0.0, 0.0, 3.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + assert node.vel_stabilization_time_event.wait(timeout=40.0), ( "The simulation is running slowly or has crashed! The time elapsed since setting the" f" target speed is: {(node.current_time - node.goal_received_time):.1f}." ) diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py index 381d9196..1414c9dc 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py @@ -69,7 +69,7 @@ def test_namespaced_mecanum_simulation(): # 0.9 m/s and 3.0 rad/s are controller's limits defined in # rosbot_controller/config/mecanum_drive_controller.yaml node.set_destination_speed(0.9, 0.0, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + assert node.vel_stabilization_time_event.wait(timeout=40.0), ( "The simulation is running slowly or has crashed! The time elapsed since setting the" f" target speed is: {(node.current_time - node.goal_received_time):.1f}." ) @@ -81,7 +81,7 @@ def test_namespaced_mecanum_simulation(): ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" node.set_destination_speed(0.0, 0.9, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + assert node.vel_stabilization_time_event.wait(timeout=40.0), ( "The simulation is running slowly or has crashed! The time elapsed since setting the" f" target speed is: {(node.current_time - node.goal_received_time):.1f}." ) @@ -93,7 +93,7 @@ def test_namespaced_mecanum_simulation(): ), "ROSbot does not move properly in y direction. Check ekf_filter_node!" node.set_destination_speed(0.0, 0.0, 3.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( + assert node.vel_stabilization_time_event.wait(timeout=40.0), ( "The simulation is running slowly or has crashed! The time elapsed since setting the" f" target speed is: {(node.current_time - node.goal_received_time):.1f}." ) diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index 24a5687f..91eded67 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -51,6 +51,7 @@ def __init__(self, name="test_node", namespace=None): self.ekf_odom_flag = False self.odom_tf_event = Event() self.scan_event = Event() + self.ros_node_spin_event = Event() def clear_odom_flag(self): self.controller_odom_flag = False @@ -77,12 +78,11 @@ def create_test_subscribers_and_publishers(self): self.scan_sub = self.create_subscription(LaserScan, "scan", self.scan_callback, 10) - self.timer = None + 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.start() - self.timer = self.create_timer(1.0 / 10.0, self.timer_callback) def is_twist_ok(self, twist: Twist): def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, eps=0.01): @@ -96,11 +96,11 @@ def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, return x_ok and y_ok and yaw_ok def controller_callback(self, data: Odometry): - self.get_logger().info(f"Received twist from controller: {data.twist.twist}") + self.get_logger().debug(f"Received twist from controller: {data.twist.twist}") self.controller_odom_flag = self.is_twist_ok(data.twist.twist) def ekf_callback(self, data: Odometry): - self.get_logger().info(f"Received twist filtered: {data.twist.twist}") + self.get_logger().debug(f"Received twist filtered: {data.twist.twist}") self.odom_tf_event.set() self.ekf_odom_flag = self.is_twist_ok(data.twist.twist) @@ -113,7 +113,7 @@ def timer_callback(self): self.vel_stabilization_time_event.set() def scan_callback(self, data: LaserScan): - self.get_logger().info(f"Received scan length: {len(data.ranges)}") + self.get_logger().debug(f"Received scan length: {len(data.ranges)}") if data.ranges: self.scan_event.set() @@ -124,5 +124,5 @@ def publish_cmd_vel_messages(self): twist_msg.linear.y = self.v_y twist_msg.angular.z = self.v_yaw - self.get_logger().info(f"Publishing twist: {twist_msg}") + self.get_logger().debug(f"Publishing twist: {twist_msg}") self.cmd_vel_publisher.publish(twist_msg) From ab53ca9ea77a249a1b7a41bf612d61ddad085200 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 28 Dec 2023 15:59:39 +0100 Subject: [PATCH 45/79] all gazebo tests Signed-off-by: Jakub Delicat --- rosbot_gazebo/test/test_ign_kill_utils.py | 22 ++++++++++--------- .../test_multirobot_mecanum_simulation.py | 14 +++++++----- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/rosbot_gazebo/test/test_ign_kill_utils.py b/rosbot_gazebo/test/test_ign_kill_utils.py index 7f62f936..271c4310 100644 --- a/rosbot_gazebo/test/test_ign_kill_utils.py +++ b/rosbot_gazebo/test/test_ign_kill_utils.py @@ -20,16 +20,18 @@ def kill_ign_linux_processes(): try: - result = subprocess.run( - ["pgrep", "-f", "ign gazebo"], - capture_output=True, - text=True, - check=True, - ) - - pids = result.stdout.strip().split("\n") - for pid in pids: - subprocess.run(["kill", pid], check=True) + for process_name in ["ign gazebo", "ros_gz"]: + result = subprocess.run( + ["pgrep", "-f", process_name], + capture_output=True, + text=True, + check=True, + ) + + pids = result.stdout.strip().split("\n") + for pid in pids: + subprocess.run(["kill", pid], check=True) + print("Killed all Ignition Gazebo processes") except subprocess.CalledProcessError as e: print(e) diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py index 8beb02d7..fefe933a 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py @@ -26,6 +26,8 @@ from test_utils import SimulationTestNode from test_ign_kill_utils import kill_ign_linux_processes +robot_names = ["robot1", "robot2", "robot3", "robot4"] + @launch_pytest.fixture def generate_test_description(): @@ -50,7 +52,7 @@ def generate_test_description(): @pytest.mark.launch(fixture=generate_test_description) -def test_multirobot_simulation(): +def test_multirobot_mecanum_simulation(): robot_names = ["robot1", "robot2", "robot3", "robot4"] rclpy.init() try: @@ -72,7 +74,7 @@ def test_multirobot_simulation(): node.set_destination_speed(0.9, 0.0, 0.0) for node in nodes: - assert node.vel_stabilization_time_event.wait(timeout=60.0), ( + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( "The simulation is running slowly or has crashed! The time elapsed since setting" f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." ) @@ -89,22 +91,22 @@ def test_multirobot_simulation(): node.set_destination_speed(0.0, 0.9, 0.0) for node in nodes: - assert node.vel_stabilization_time_event.wait(timeout=60.0), ( + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( "The simulation is running slowly or has crashed! The time elapsed since setting" f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." ) assert ( node.controller_odom_flag - ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" + ), "ROSbot does not move properly in y direction. Check rosbot_base_controller!" assert ( node.ekf_odom_flag - ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" + ), "ROSbot does not move properly in y direction. Check ekf_filter_node!" for node in nodes: node.set_destination_speed(0.0, 0.0, 3.0) for node in nodes: - assert node.vel_stabilization_time_event.wait(timeout=60.0), ( + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( "The simulation is running slowly or has crashed! The time elapsed since setting" f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." ) From cf197badccc37470d4464019891b20fac847380a Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 28 Dec 2023 16:10:46 +0100 Subject: [PATCH 46/79] changed to namespaced repositories Signed-off-by: Jakub Delicat --- rosbot/rosbot_hardware.repos | 2 +- rosbot/rosbot_simulation.repos | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index 80f61e7d..c64e1ff5 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -6,7 +6,7 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: ros2 + version: gazebo-ignition-multirobots rosbot_controllers: type: git url: https://github.com/husarion/rosbot_controllers diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos index affc3289..838189e6 100644 --- a/rosbot/rosbot_simulation.repos +++ b/rosbot/rosbot_simulation.repos @@ -7,4 +7,4 @@ repositories: husarion/husarion_office_gz: type: git url: https://github.com/husarion/husarion_office_gz - version: main + version: add-gazebo-plugins From 273ddeb13cf8cedf404e22f082f545452591f9a7 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 28 Dec 2023 16:20:25 +0100 Subject: [PATCH 47/79] revert headless to tests Signed-off-by: Jakub Delicat --- rosbot_gazebo/test/test_diff_drive_simulation.py | 4 ++-- rosbot_gazebo/test/test_mecanum_simulation.py | 2 +- rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py | 1 + rosbot_gazebo/test/test_multirobot_mecanum_simulation.py | 1 + rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py | 2 +- rosbot_gazebo/test/test_namespaced_mecanum_simulation.py | 2 +- 6 files changed, 7 insertions(+), 5 deletions(-) diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py index 835da43b..1fc26695 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_diff_drive_simulation.py @@ -42,14 +42,14 @@ def generate_test_description(): ) ), launch_arguments={ - # "headless": "True", + "headless": "True", "world": PathJoinSubstitution( [ get_package_share_directory("husarion_office_gz"), "worlds", "empty_with_plugins.sdf", ] - ) + ), }.items(), ) diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py index 11a7b21b..f528d644 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_mecanum_simulation.py @@ -43,7 +43,7 @@ def generate_test_description(): ), launch_arguments={ "mecanum": "True", - # "headless": "True", + "headless": "True", "world": PathJoinSubstitution( [ get_package_share_directory("husarion_office_gz"), diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py index ba30c54d..df5f16e7 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -41,6 +41,7 @@ def generate_test_description(): "/worlds/empty_with_plugins.sdf" ), "robots:=robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}", + "headless:=True", ], output="screen", ) diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py index fefe933a..d68ed6ed 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py @@ -43,6 +43,7 @@ def generate_test_description(): "/worlds/empty_with_plugins.sdf" ), "robots:=robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}", + "headless:=True", "mecanum:=True", ], output="screen", diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py index ff1847fd..d9030aca 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py @@ -42,7 +42,7 @@ def generate_test_description(): ) ), launch_arguments={ - # "headless": "True", + "headless": "True", "world": PathJoinSubstitution( [ get_package_share_directory("husarion_office_gz"), diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py index 1414c9dc..a6301ada 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py @@ -50,7 +50,7 @@ def generate_test_description(): "empty_with_plugins.sdf", ] ), - # "headless": "True", + "headless": "True", "namespace": "rosbot2r", }.items(), ) From 61825c3e67f2143321211ea496cf2899f0c694fa Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 28 Dec 2023 17:13:26 +0100 Subject: [PATCH 48/79] IMU name with slash Signed-off-by: Jakub Delicat --- rosbot_controller/config/diff_drive_controller.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index 1755236d..efa97f70 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -14,7 +14,7 @@ ros__parameters: tf_frame_prefix_enable: false - sensor_name: imu + sensor_name: /imu frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally From 712cd9dc6ce4c88c20040037e6a8a6b2e5d7bc87 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 28 Dec 2023 17:26:40 +0100 Subject: [PATCH 49/79] removed delay from spawning Signed-off-by: Jakub Delicat --- rosbot_bringup/test/test_multirobot_ekf.py | 9 +++------ rosbot_controller/config/diff_drive_controller.yaml | 2 +- rosbot_controller/test/test_multirobot_controllers.py | 9 +++------ 3 files changed, 7 insertions(+), 13 deletions(-) diff --git a/rosbot_bringup/test/test_multirobot_ekf.py b/rosbot_bringup/test/test_multirobot_ekf.py index ca3d5685..64a38731 100644 --- a/rosbot_bringup/test/test_multirobot_ekf.py +++ b/rosbot_bringup/test/test_multirobot_ekf.py @@ -20,7 +20,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from test_utils import BringupTestNode @@ -50,11 +50,8 @@ def generate_test_description(): "namespace": robot_names[i], }.items(), ) - if i > 0: - delayed_bringup_launch = TimerAction(period=i * 10.0, actions=[bringup_launch]) - actions.append(delayed_bringup_launch) - else: - actions.append(bringup_launch) + + actions.append(bringup_launch) return LaunchDescription(actions) diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index efa97f70..1755236d 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -14,7 +14,7 @@ ros__parameters: tf_frame_prefix_enable: false - sensor_name: /imu + sensor_name: imu frame_id: imu_link static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py index e6117a2f..8f067081 100644 --- a/rosbot_controller/test/test_multirobot_controllers.py +++ b/rosbot_controller/test/test_multirobot_controllers.py @@ -20,7 +20,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from test_utils import ControllersTestNode @@ -50,11 +50,8 @@ def generate_test_description(): "namespace": robot_names[i], }.items(), ) - if i > 0: - delayed_bringup_launch = TimerAction(period=i * 10.0, actions=[controller_launch]) - actions.append(delayed_bringup_launch) - else: - actions.append(controller_launch) + + actions.append(controller_launch) return LaunchDescription(actions) From ec1ce0a01ab1d8f6f38eac5a5694fd334d1c930e Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 29 Dec 2023 13:56:21 +0100 Subject: [PATCH 50/79] added name for ign_ros2_controller Signed-off-by: Jakub Delicat --- rosbot/rosbot_hardware.repos | 2 +- rosbot/rosbot_simulation.repos | 3 ++- .../config/diff_drive_controller.yaml | 13 ++++++++++++ .../config/mecanum_drive_controller.yaml | 20 +++++++++++++++---- rosbot_controller/launch/controller.launch.py | 6 +++++- .../urdf/rosbot_macro.urdf.xacro | 4 +++- 6 files changed, 40 insertions(+), 8 deletions(-) diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index c64e1ff5..6310761b 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -10,7 +10,7 @@ repositories: rosbot_controllers: type: git url: https://github.com/husarion/rosbot_controllers - version: main + version: update-params # Waiting for backporting to the humble version # https://github.com/ros-controls/ros2_controllers/pull/726 and # https://github.com/ros-controls/ros2_controllers/pull/831 diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos index 838189e6..cb238b1b 100644 --- a/rosbot/rosbot_simulation.repos +++ b/rosbot/rosbot_simulation.repos @@ -1,7 +1,8 @@ repositories: gazebosim/gz_ros2_control: type: git - url: https://github.com/ros-controls/gz_ros2_control.git + # Added feature with control manager name. Waiting on https://github.com/ros-controls/gz_ros2_control/pull/212 + url: https://github.com/delihus/gz_ros2_control/tree/humble version: humble husarion/husarion_office_gz: diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index 1755236d..481fd076 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -1,3 +1,15 @@ +/**/simulation_controller_manager: + ros__parameters: + use_sim_time: True + update_rate: 20 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + rosbot_base_controller: + type: diff_drive_controller/DiffDriveController + imu_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + /**/controller_manager: ros__parameters: use_sim_time: False @@ -13,6 +25,7 @@ /**/imu_broadcaster: ros__parameters: tf_frame_prefix_enable: false + use_namespace_as_sensor_name_prefix: true sensor_name: imu frame_id: imu_link diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index 9eec0f0e..79d43a3e 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -1,6 +1,18 @@ +/**/simulation_controller_manager: + ros__parameters: + use_sim_time: True + update_rate: 20 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + rosbot_base_controller: + type: mecanum_drive_controller/MecanumDriveController + imu_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + /**/controller_manager: ros__parameters: - use_sim_time: false + use_sim_time: False update_rate: 20 # Hz joint_state_broadcaster: @@ -13,6 +25,7 @@ /**/imu_broadcaster: ros__parameters: tf_frame_prefix_enable: false + use_namespace_as_sensor_name_prefix: true sensor_name: imu frame_id: imu_link @@ -33,9 +46,8 @@ wheel_separation_y: 0.192 wheel_radius: 0.047 - - wheel_separation_multiplier_x: 1.0 - wheel_separation_multiplier_y: 1.0 + wheel_separation_x_multiplier: 1.0 + wheel_separation_y_multiplier: 1.0 wheel_radius_multiplier: 1.0 diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index e94a3b9a..0d4a380d 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -79,9 +79,13 @@ def generate_launch_description(): ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"] ) + in_case_simulation_controller_name = PythonExpression( + ["'simulation_controller_manager'", " if '", use_sim, "' else 'controller_manager'"] + ) + controller_manager_name = LaunchConfiguration( "controller_manager_name", - default=[namespace_ext, "controller_manager"], + default=[namespace_ext, in_case_simulation_controller_name], ) # Get URDF via xacro diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 8afa8a99..40966ce5 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -3,7 +3,7 @@ - + @@ -132,6 +132,8 @@ $(find rosbot_controller)/config/diff_drive_controller.yaml + simulation_controller_manager + ${gz_control_namespace} rosbot_base_controller/cmd_vel_unstamped:=cmd_vel From 25009f11c799023a317142efa847e387e82e4c4b Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 29 Dec 2023 13:58:53 +0100 Subject: [PATCH 51/79] fixed submodule Signed-off-by: Jakub Delicat --- rosbot/rosbot_simulation.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos index cb238b1b..deedc368 100644 --- a/rosbot/rosbot_simulation.repos +++ b/rosbot/rosbot_simulation.repos @@ -2,7 +2,7 @@ repositories: gazebosim/gz_ros2_control: type: git # Added feature with control manager name. Waiting on https://github.com/ros-controls/gz_ros2_control/pull/212 - url: https://github.com/delihus/gz_ros2_control/tree/humble + url: https://github.com/delihus/gz_ros2_control version: humble husarion/husarion_office_gz: From 79d82ffa537fefb28c9e3b6926fe157f4edd8a34 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 29 Dec 2023 15:10:14 +0100 Subject: [PATCH 52/79] checking mecanum controller sim time true Signed-off-by: Jakub Delicat --- rosbot/rosbot_hardware.repos | 2 +- rosbot_controller/config/diff_drive_controller.yaml | 13 ------------- .../config/mecanum_drive_controller.yaml | 13 ------------- rosbot_controller/launch/controller.launch.py | 6 +----- rosbot_description/urdf/rosbot_macro.urdf.xacro | 2 -- 5 files changed, 2 insertions(+), 34 deletions(-) diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index 6310761b..1d9e14c7 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -17,7 +17,7 @@ repositories: ros2_controllers: type: git url: https://github.com/delihus/ros2_controllers - version: humble-backport + version: imu-sensor-broadcaster-features micro_ros_msgs: type: git url: https://github.com/micro-ROS/micro_ros_msgs.git diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index 481fd076..df7a0af4 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -1,18 +1,5 @@ -/**/simulation_controller_manager: - ros__parameters: - use_sim_time: True - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: diff_drive_controller/DiffDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - /**/controller_manager: ros__parameters: - use_sim_time: False update_rate: 20 # Hz joint_state_broadcaster: diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index 79d43a3e..4ee7d87e 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -1,18 +1,5 @@ -/**/simulation_controller_manager: - ros__parameters: - use_sim_time: True - update_rate: 20 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - rosbot_base_controller: - type: mecanum_drive_controller/MecanumDriveController - imu_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - /**/controller_manager: ros__parameters: - use_sim_time: False update_rate: 20 # Hz joint_state_broadcaster: diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index 0d4a380d..e94a3b9a 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -79,13 +79,9 @@ def generate_launch_description(): ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"] ) - in_case_simulation_controller_name = PythonExpression( - ["'simulation_controller_manager'", " if '", use_sim, "' else 'controller_manager'"] - ) - controller_manager_name = LaunchConfiguration( "controller_manager_name", - default=[namespace_ext, in_case_simulation_controller_name], + default=[namespace_ext, "controller_manager"], ) # Get URDF via xacro diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 40966ce5..3ef9db0b 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -132,8 +132,6 @@ $(find rosbot_controller)/config/diff_drive_controller.yaml - simulation_controller_manager - ${gz_control_namespace} rosbot_base_controller/cmd_vel_unstamped:=cmd_vel From ad156c139bab38a9de915627876c3e76f6f666e7 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Tue, 2 Jan 2024 12:25:34 +0100 Subject: [PATCH 53/79] Update industrial_ci.yaml --- .github/workflows/industrial_ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index 87c6c5f1..ea138b6e 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -29,7 +29,7 @@ jobs: ros_industrial_ci: name: ROS Industrial CI runs-on: ubuntu-22.04 - timeout-minutes: 30 # Set the job timeout to 30 minutes + timeout-minutes: 60 needs: - black - spellcheck From 50c90e5028450640f9b2ce78584a76bceb3e6989 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 2 Jan 2024 15:26:26 +0100 Subject: [PATCH 54/79] removed coltroller test for testing timeout Signed-off-by: Jakub Delicat --- .github/workflows/industrial_ci.yaml | 2 +- .../test/test_diff_drive_controllers.py | 101 ----------------- .../test/test_mecanum_controllers.py | 101 ----------------- .../test/test_multirobot_controllers.py | 84 --------------- .../test_namespaced_diff_drive_controllers.py | 102 ------------------ .../test_namespaced_mecanum_controllers.py | 102 ------------------ rosbot_controller/test/test_utils.py | 93 ---------------- rosbot_controller/test/test_xacro.py | 55 ---------- 8 files changed, 1 insertion(+), 639 deletions(-) delete mode 100644 rosbot_controller/test/test_diff_drive_controllers.py delete mode 100644 rosbot_controller/test/test_mecanum_controllers.py delete mode 100644 rosbot_controller/test/test_multirobot_controllers.py delete mode 100644 rosbot_controller/test/test_namespaced_diff_drive_controllers.py delete mode 100644 rosbot_controller/test/test_namespaced_mecanum_controllers.py delete mode 100644 rosbot_controller/test/test_utils.py delete mode 100644 rosbot_controller/test/test_xacro.py diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index ea138b6e..7fe1ebb4 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -29,7 +29,7 @@ jobs: ros_industrial_ci: name: ROS Industrial CI runs-on: ubuntu-22.04 - timeout-minutes: 60 + timeout-minutes: 20 needs: - black - spellcheck diff --git a/rosbot_controller/test/test_diff_drive_controllers.py b/rosbot_controller/test/test_diff_drive_controllers.py deleted file mode 100644 index 3b69459c..00000000 --- a/rosbot_controller/test/test_diff_drive_controllers.py +++ /dev/null @@ -1,101 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion -# -# 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 launch_pytest -import pytest -import rclpy - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_controller = get_package_share_directory("rosbot_controller") - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_controller, - "launch", - "controller.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "False", - "use_gpu": "False", - }.items(), - ) - - return LaunchDescription([bringup_launch]) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_controllers_startup_fail(): - rclpy.init() - try: - node = ControllersTestNode("test_controllers_bringup") - node.create_test_subscribers_and_publishers() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received JointStates message that should not have appeared. Check whether other" - " robots are connected to your network.!" - ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Odom message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Imu message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - finally: - rclpy.shutdown() - - -@pytest.mark.launch(fixture=generate_test_description) -def test_controllers_startup_success(): - rclpy.init() - try: - node = ControllersTestNode("test_controllers_bringup") - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" - finally: - rclpy.shutdown() diff --git a/rosbot_controller/test/test_mecanum_controllers.py b/rosbot_controller/test/test_mecanum_controllers.py deleted file mode 100644 index 5f2607e3..00000000 --- a/rosbot_controller/test/test_mecanum_controllers.py +++ /dev/null @@ -1,101 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion -# -# 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 launch_pytest -import pytest -import rclpy - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_controller = get_package_share_directory("rosbot_controller") - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_controller, - "launch", - "controller.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "True", - "use_gpu": "False", - }.items(), - ) - - return LaunchDescription([bringup_launch]) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_controllers_startup_fail(): - rclpy.init() - try: - node = ControllersTestNode("test_controllers_bringup") - node.create_test_subscribers_and_publishers() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received JointStates message that should not have appeared. Check whether other" - " robots are connected to your network.!" - ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Odom message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Imu message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - finally: - rclpy.shutdown() - - -@pytest.mark.launch(fixture=generate_test_description) -def test_controllers_startup_success(): - rclpy.init() - try: - node = ControllersTestNode("test_controllers_bringup") - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" - finally: - rclpy.shutdown() diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py deleted file mode 100644 index 8f067081..00000000 --- a/rosbot_controller/test/test_multirobot_controllers.py +++ /dev/null @@ -1,84 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion -# -# 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 launch_pytest -import pytest -import rclpy - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode - -robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"] - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_controller = get_package_share_directory("rosbot_controller") - actions = [] - for i in range(len(robot_names)): - controller_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_controller, - "launch", - "controller.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "True", - "use_gpu": "False", - "namespace": robot_names[i], - }.items(), - ) - - actions.append(controller_launch) - - return LaunchDescription(actions) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_multirobot_controllers_startup_success(): - for robot_name in robot_names: - rclpy.init() - try: - node = ControllersTestNode(f"test_{robot_name}_controllers", namespace=robot_name) - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert msgs_received_flag, ( - f"Expected JointStates message but it was not received. Check {robot_name}/" - "joint_state_broadcaster!" - ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert msgs_received_flag, ( - f"Expected Odom message but it was not received. Check {robot_name}/" - "rosbot_base_controller!" - ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), f"Expected Imu message but it was not received. Check {robot_name}/imu_broadcaster!" - finally: - rclpy.shutdown() diff --git a/rosbot_controller/test/test_namespaced_diff_drive_controllers.py b/rosbot_controller/test/test_namespaced_diff_drive_controllers.py deleted file mode 100644 index fdab2de6..00000000 --- a/rosbot_controller/test/test_namespaced_diff_drive_controllers.py +++ /dev/null @@ -1,102 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion -# -# 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 launch_pytest -import pytest -import rclpy - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_controller = get_package_share_directory("rosbot_controller") - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_controller, - "launch", - "controller.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "False", - "use_gpu": "False", - "namespace": "rosbot2r", - }.items(), - ) - - return LaunchDescription([bringup_launch]) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_fail(): - rclpy.init() - try: - node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") - node.create_test_subscribers_and_publishers() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received JointStates message that should not have appeared. Check whether other" - " robots are connected to your network.!" - ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Odom message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Imu message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - finally: - rclpy.shutdown() - - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_success(): - rclpy.init() - try: - node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" - finally: - rclpy.shutdown() diff --git a/rosbot_controller/test/test_namespaced_mecanum_controllers.py b/rosbot_controller/test/test_namespaced_mecanum_controllers.py deleted file mode 100644 index e2321fd0..00000000 --- a/rosbot_controller/test/test_namespaced_mecanum_controllers.py +++ /dev/null @@ -1,102 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion -# -# 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 launch_pytest -import pytest -import rclpy - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_controller = get_package_share_directory("rosbot_controller") - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_controller, - "launch", - "controller.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "True", - "use_gpu": "False", - "namespace": "rosbot2r", - }.items(), - ) - - return LaunchDescription([bringup_launch]) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_fail(): - rclpy.init() - try: - node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") - node.create_test_subscribers_and_publishers() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received JointStates message that should not have appeared. Check whether other" - " robots are connected to your network.!" - ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Odom message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert not msgs_received_flag, ( - "Received Imu message that should not have appeared. Check whether other robots are" - " connected to your network.!" - ) - finally: - rclpy.shutdown() - - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_controllers_startup_success(): - rclpy.init() - try: - node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" - finally: - rclpy.shutdown() diff --git a/rosbot_controller/test/test_utils.py b/rosbot_controller/test/test_utils.py deleted file mode 100644 index 831eadad..00000000 --- a/rosbot_controller/test/test_utils.py +++ /dev/null @@ -1,93 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Husarion -# -# 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 rclpy - -from threading import Event -from threading import Thread - -from rclpy.node import Node - -from sensor_msgs.msg import JointState, Imu -from nav_msgs.msg import Odometry - - -class ControllersTestNode(Node): - ROSBOT_HARDWARE_PUBLISHERS_RATE = 10.0 - - __test__ = False - - def __init__(self, name="test_node", namespace=None): - super().__init__(name, namespace=namespace) - self.joint_state_msg_event = Event() - self.odom_msg_event = Event() - self.imu_msg_event = Event() - - def create_test_subscribers_and_publishers(self): - self.joint_state_sub = self.create_subscription( - JointState, "joint_states", self.joint_states_callback, 10 - ) - - self.odom_sub = self.create_subscription( - Odometry, "rosbot_base_controller/odom", self.odometry_callback, 10 - ) - - self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10) - - # TODO: @delipl namespaces have not been implemented in microros yet - self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10) - - self.joint_states_publisher = self.create_publisher(JointState, "/_motors_response", 10) - - self.timer = None - - def start_node_thread(self): - self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) - self.ros_spin_thread.start() - - def joint_states_callback(self, data): - self.joint_state_msg_event.set() - - def odometry_callback(self, data): - self.odom_msg_event.set() - - def imu_callback(self, data): - self.imu_msg_event.set() - - def start_publishing_fake_hardware(self): - self.timer = self.create_timer( - 1.0 / self.ROSBOT_HARDWARE_PUBLISHERS_RATE, - self.publish_fake_hardware_messages, - ) - - # TODO: @delipl namespaces have not been implemented in microros yet - def publish_fake_hardware_messages(self): - imu_msg = Imu() - imu_msg.header.stamp = self.get_clock().now().to_msg() - imu_msg.header.frame_id = "imu_link" - - joint_state_msg = JointState() - joint_state_msg.header.stamp = self.get_clock().now().to_msg() - joint_state_msg.name = [ - "fl_wheel_joint", - "fr_wheel_joint", - "rl_wheel_joint", - "rr_wheel_joint", - ] - joint_state_msg.position = [0.0, 0.0, 0.0, 0.0] - joint_state_msg.velocity = [0.0, 0.0, 0.0, 0.0] - - self.imu_publisher.publish(imu_msg) - self.joint_states_publisher.publish(joint_state_msg) diff --git a/rosbot_controller/test/test_xacro.py b/rosbot_controller/test/test_xacro.py deleted file mode 100644 index 0e52cfe6..00000000 --- a/rosbot_controller/test/test_xacro.py +++ /dev/null @@ -1,55 +0,0 @@ -# Copyright 2023 Husarion -# -# 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 os -import xacro -import itertools -from ament_index_python.packages import get_package_share_directory - - -def test_rosbot_description_parsing(): - mecanum_values = ["true", "false"] - use_sim_values = ["true", "false"] - use_gpu_values = ["true", "false"] - simulation_engine_values = ["ignition-gazebo", "webots"] # 'gazebo-classic' - use_multirobot_system_values = ["true", "false"] - - all_combinations = list( - itertools.product( - mecanum_values, - use_sim_values, - use_gpu_values, - simulation_engine_values, - use_multirobot_system_values, - ) - ) - - for combination in all_combinations: - mecanum, use_sim, use_gpu, simulation_engine, use_multirobot_system = combination - mappings = { - "mecanum": mecanum, - "use_sim": use_sim, - "use_gpu": use_gpu, - "simulation_engine": simulation_engine, - "use_multirobot_system": use_multirobot_system, - } - rosbot_description = get_package_share_directory("rosbot_description") - xacro_path = os.path.join(rosbot_description, "urdf/rosbot.urdf.xacro") - try: - xacro.process_file(xacro_path, mappings=mappings) - except xacro.XacroException as e: - assert False, ( - f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, use_sim:" - f" {use_sim}, use_gpu: {use_gpu}, simulation_engine: {simulation_engine}" - ) From f58a96780568bc4edd9cd976dc4f4d8bb29ab554 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 2 Jan 2024 15:43:31 +0100 Subject: [PATCH 55/79] removed bringup tests for testing timeout Signed-off-by: Jakub Delicat --- rosbot_bringup/test/test_copyright.py | 23 ------ rosbot_bringup/test/test_diff_drive_ekf.py | 67 ---------------- rosbot_bringup/test/test_flake8.py | 23 ------ rosbot_bringup/test/test_mecanum_ekf.py | 67 ---------------- rosbot_bringup/test/test_multirobot_ekf.py | 76 ------------------ .../test/test_namespaced_diff_drive_ekf.py | 68 ---------------- .../test/test_namespaced_mecanum_ekf.py | 68 ---------------- rosbot_bringup/test/test_pep257.py | 23 ------ rosbot_bringup/test/test_utils.py | 79 ------------------- 9 files changed, 494 deletions(-) delete mode 100644 rosbot_bringup/test/test_copyright.py delete mode 100644 rosbot_bringup/test/test_diff_drive_ekf.py delete mode 100644 rosbot_bringup/test/test_flake8.py delete mode 100644 rosbot_bringup/test/test_mecanum_ekf.py delete mode 100644 rosbot_bringup/test/test_multirobot_ekf.py delete mode 100644 rosbot_bringup/test/test_namespaced_diff_drive_ekf.py delete mode 100644 rosbot_bringup/test/test_namespaced_mecanum_ekf.py delete mode 100644 rosbot_bringup/test/test_pep257.py delete mode 100644 rosbot_bringup/test/test_utils.py diff --git a/rosbot_bringup/test/test_copyright.py b/rosbot_bringup/test/test_copyright.py deleted file mode 100644 index f46f861d..00000000 --- a/rosbot_bringup/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# 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. - -from ament_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/rosbot_bringup/test/test_diff_drive_ekf.py b/rosbot_bringup/test/test_diff_drive_ekf.py deleted file mode 100644 index 5da7a3b1..00000000 --- a/rosbot_bringup/test/test_diff_drive_ekf.py +++ /dev/null @@ -1,67 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion -# -# 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 launch_pytest -import pytest -import rclpy - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import BringupTestNode - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_bringup = get_package_share_directory("rosbot_bringup") - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_bringup, - "launch", - "bringup.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "False", - "use_gpu": "False", - }.items(), - ) - - return LaunchDescription([bringup_launch]) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_bringup_startup_success(): - rclpy.init() - try: - node = BringupTestNode("test_bringup") - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected odometry/filtered message but it was not received. Check robot_localization!" - - finally: - rclpy.shutdown() diff --git a/rosbot_bringup/test/test_flake8.py b/rosbot_bringup/test/test_flake8.py deleted file mode 100644 index 49c1644f..00000000 --- a/rosbot_bringup/test/test_flake8.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# 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. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@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) diff --git a/rosbot_bringup/test/test_mecanum_ekf.py b/rosbot_bringup/test/test_mecanum_ekf.py deleted file mode 100644 index fcdfc633..00000000 --- a/rosbot_bringup/test/test_mecanum_ekf.py +++ /dev/null @@ -1,67 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion -# -# 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 launch_pytest -import pytest -import rclpy - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import BringupTestNode - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_bringup = get_package_share_directory("rosbot_bringup") - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_bringup, - "launch", - "bringup.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "True", - "use_gpu": "False", - }.items(), - ) - - return LaunchDescription([bringup_launch]) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_bringup_startup_success(): - rclpy.init() - try: - node = BringupTestNode("test_bringup") - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected odometry/filtered message but it was not received. Check robot_localization!" - - finally: - rclpy.shutdown() diff --git a/rosbot_bringup/test/test_multirobot_ekf.py b/rosbot_bringup/test/test_multirobot_ekf.py deleted file mode 100644 index 64a38731..00000000 --- a/rosbot_bringup/test/test_multirobot_ekf.py +++ /dev/null @@ -1,76 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion -# -# 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 launch_pytest -import pytest -import rclpy - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import BringupTestNode - -robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"] - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_bringup = get_package_share_directory("rosbot_bringup") - actions = [] - for i in range(len(robot_names)): - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_bringup, - "launch", - "bringup.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "False", - "use_gpu": "False", - "namespace": robot_names[i], - }.items(), - ) - - actions.append(bringup_launch) - - return LaunchDescription(actions) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_multirobot_bringup_startup_success(): - for robot_name in robot_names: - rclpy.init() - try: - node = BringupTestNode("test_bringup", namespace=robot_name) - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.odom_msg_event.wait(timeout=20.0) - assert msgs_received_flag, ( - f"Expected {robot_name}/odometry/filtered message but it was not received. " - "Check robot_localization!" - ) - - finally: - rclpy.shutdown() diff --git a/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py b/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py deleted file mode 100644 index f78629a7..00000000 --- a/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py +++ /dev/null @@ -1,68 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion -# -# 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 launch_pytest -import pytest -import rclpy - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import BringupTestNode - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_bringup = get_package_share_directory("rosbot_bringup") - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_bringup, - "launch", - "bringup.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "False", - "use_gpu": "False", - "namespace": "rosbot2r", - }.items(), - ) - - return LaunchDescription([bringup_launch]) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_bringup_startup_success(): - rclpy.init() - try: - node = BringupTestNode("test_bringup", namespace="rosbot2r") - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected odometry/filtered message but it was not received. Check robot_localization!" - - finally: - rclpy.shutdown() diff --git a/rosbot_bringup/test/test_namespaced_mecanum_ekf.py b/rosbot_bringup/test/test_namespaced_mecanum_ekf.py deleted file mode 100644 index a49b8faf..00000000 --- a/rosbot_bringup/test/test_namespaced_mecanum_ekf.py +++ /dev/null @@ -1,68 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Intel Corporation. All Rights Reserved. -# Copyright 2023 Husarion -# -# 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 launch_pytest -import pytest -import rclpy - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import BringupTestNode - - -@launch_pytest.fixture -def generate_test_description(): - rosbot_bringup = get_package_share_directory("rosbot_bringup") - bringup_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - rosbot_bringup, - "launch", - "bringup.launch.py", - ] - ) - ), - launch_arguments={ - "use_sim": "False", - "mecanum": "True", - "use_gpu": "False", - "namespace": "rosbot2r", - }.items(), - ) - - return LaunchDescription([bringup_launch]) - - -@pytest.mark.launch(fixture=generate_test_description) -def test_namespaced_bringup_startup_success(): - rclpy.init() - try: - node = BringupTestNode("test_bringup", namespace="rosbot2r") - node.create_test_subscribers_and_publishers() - node.start_publishing_fake_hardware() - - node.start_node_thread() - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected odometry/filtered message but it was not received. Check robot_localization!" - - finally: - rclpy.shutdown() diff --git a/rosbot_bringup/test/test_pep257.py b/rosbot_bringup/test/test_pep257.py deleted file mode 100644 index a2c3deb8..00000000 --- a/rosbot_bringup/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# 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. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" diff --git a/rosbot_bringup/test/test_utils.py b/rosbot_bringup/test/test_utils.py deleted file mode 100644 index cc483be1..00000000 --- a/rosbot_bringup/test/test_utils.py +++ /dev/null @@ -1,79 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# Copyright 2023 Husarion -# -# 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 rclpy - -from threading import Event -from threading import Thread - -from rclpy.node import Node - -from sensor_msgs.msg import JointState, Imu -from nav_msgs.msg import Odometry - - -class BringupTestNode(Node): - ROSBOT_HARDWARE_PUBLISHERS_RATE = 10.0 - - __test__ = False - - def __init__(self, name="test_node", namespace=None): - super().__init__(name, namespace=namespace) - self.odom_msg_event = Event() - - def create_test_subscribers_and_publishers(self): - self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10) - - self.joint_states_publisher = self.create_publisher(JointState, "/_motors_response", 10) - - self.odom_sub = self.create_subscription( - Odometry, "odometry/filtered", self.odometry_callback, 10 - ) - self.timer = None - - def start_node_thread(self): - self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) - self.ros_spin_thread.start() - - def start_publishing_fake_hardware(self): - self.timer = self.create_timer( - 1.0 / self.ROSBOT_HARDWARE_PUBLISHERS_RATE, - self.timer_callback, - ) - - def timer_callback(self): - self.publish_fake_hardware_messages() - - def odometry_callback(self, data): - self.odom_msg_event.set() - - def publish_fake_hardware_messages(self): - imu_msg = Imu() - imu_msg.header.stamp = self.get_clock().now().to_msg() - imu_msg.header.frame_id = "imu_link" - - joint_state_msg = JointState() - joint_state_msg.header.stamp = self.get_clock().now().to_msg() - joint_state_msg.name = [ - "fl_wheel_joint", - "fr_wheel_joint", - "rl_wheel_joint", - "rr_wheel_joint", - ] - joint_state_msg.position = [0.0, 0.0, 0.0, 0.0] - joint_state_msg.velocity = [0.0, 0.0, 0.0, 0.0] - - self.imu_publisher.publish(imu_msg) - self.joint_states_publisher.publish(joint_state_msg) From 23f6432ec6d37163d268cc47e08c0a305402991f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 2 Jan 2024 16:44:17 +0100 Subject: [PATCH 56/79] used launch_ros features Signed-off-by: Jakub Delicat --- .github/workflows/industrial_ci.yaml | 4 ++++ .../config/mecanum_drive_controller.yaml | 2 ++ .../test/test_diff_drive_simulation.py | 16 ++++++++++++- rosbot_gazebo/test/test_ign_kill_utils.py | 24 ++++++------------- rosbot_gazebo/test/test_mecanum_simulation.py | 16 ++++++++++++- .../test_multirobot_diff_drive_simulation.py | 17 ++++++++++++- .../test_multirobot_mecanum_simulation.py | 17 ++++++++++++- .../test_namespaced_diff_drive_simulation.py | 16 ++++++++++++- .../test_namespaced_mecanum_simulation.py | 16 ++++++++++++- 9 files changed, 105 insertions(+), 23 deletions(-) diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index 7fe1ebb4..73a71d04 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -63,6 +63,10 @@ jobs: cp -r src/ros2_controllers/imu_sensor_broadcaster src/ rm -rf src/ros2_controllers + - name: Remove ign_ros2_control demo + shell: bash + run: rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos + # Package micro_ros_msgs does not have industrial ci and tests does not pass. # For more information see https://github.com/micro-ROS/micro_ros_msgs/issues/7 - name: Remove tests from micro_ros_msgs diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index 4ee7d87e..de40cc07 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -22,6 +22,8 @@ /**/rosbot_base_controller: ros__parameters: + # TODO: (@delihus) use_sim_time is parsing here. + use_sim_time: True tf_frame_prefix_enable: false front_left_wheel_name: fl_wheel_joint diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py index 1fc26695..2c8c58f0 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_diff_drive_simulation.py @@ -17,12 +17,15 @@ import launch_pytest import pytest import rclpy +import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc from test_utils import SimulationTestNode from test_ign_kill_utils import kill_ign_linux_processes @@ -30,6 +33,10 @@ @launch_pytest.fixture def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + rosbot_gazebo = get_package_share_directory("rosbot_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -53,7 +60,14 @@ def generate_test_description(): }.items(), ) - return LaunchDescription([simulation_launch]) + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) @pytest.mark.launch(fixture=generate_test_description) diff --git a/rosbot_gazebo/test/test_ign_kill_utils.py b/rosbot_gazebo/test/test_ign_kill_utils.py index 271c4310..c961d22d 100644 --- a/rosbot_gazebo/test/test_ign_kill_utils.py +++ b/rosbot_gazebo/test/test_ign_kill_utils.py @@ -1,4 +1,5 @@ # Copyright 2023 Husarion +# Copyright 2023 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,29 +13,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -import subprocess +import psutil # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching # several tests in a row. +# https://github.com/ros-controls/gz_ros2_control/blob/master/gz_ros2_control_tests/tests/position_test.py def kill_ign_linux_processes(): - try: - for process_name in ["ign gazebo", "ros_gz"]: - result = subprocess.run( - ["pgrep", "-f", process_name], - capture_output=True, - text=True, - check=True, - ) - - pids = result.stdout.strip().split("\n") - for pid in pids: - subprocess.run(["kill", pid], check=True) - - print("Killed all Ignition Gazebo processes") - except subprocess.CalledProcessError as e: - print(e) + for proc in psutil.process_iter(): + # check whether the process name matches + if proc.name() == "ruby": + proc.kill() kill_ign_linux_processes() diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py index f528d644..537288c9 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_mecanum_simulation.py @@ -17,12 +17,15 @@ import launch_pytest import pytest import rclpy +import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc from test_utils import SimulationTestNode from test_ign_kill_utils import kill_ign_linux_processes @@ -30,6 +33,10 @@ @launch_pytest.fixture def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + rosbot_gazebo = get_package_share_directory("rosbot_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -54,7 +61,14 @@ def generate_test_description(): }.items(), ) - return LaunchDescription([simulation_launch]) + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) @pytest.mark.launch(fixture=generate_test_description) diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py index df5f16e7..572c5b47 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -17,18 +17,26 @@ import launch_pytest import pytest import rclpy +import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ExecuteProcess from rclpy.executors import MultiThreadedExecutor from threading import Thread +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc + from test_utils import SimulationTestNode from test_ign_kill_utils import kill_ign_linux_processes @launch_pytest.fixture def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + # IncludeLaunchDescription does not work with robots argument simulation_launch = ExecuteProcess( cmd=[ @@ -46,7 +54,14 @@ def generate_test_description(): output="screen", ) - return LaunchDescription([simulation_launch]) + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) @pytest.mark.launch(fixture=generate_test_description) diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py index d68ed6ed..906cb700 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py @@ -17,12 +17,16 @@ import launch_pytest import pytest import rclpy +import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ExecuteProcess +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc from rclpy.executors import MultiThreadedExecutor from threading import Thread + from test_utils import SimulationTestNode from test_ign_kill_utils import kill_ign_linux_processes @@ -31,6 +35,10 @@ @launch_pytest.fixture def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + # IncludeLaunchDescription does not work with robots argument simulation_launch = ExecuteProcess( cmd=[ @@ -49,7 +57,14 @@ def generate_test_description(): output="screen", ) - return LaunchDescription([simulation_launch]) + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) @pytest.mark.launch(fixture=generate_test_description) diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py index d9030aca..7a757cef 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py @@ -17,12 +17,15 @@ import launch_pytest import pytest import rclpy +import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc from test_utils import SimulationTestNode from test_ign_kill_utils import kill_ign_linux_processes @@ -30,6 +33,10 @@ @launch_pytest.fixture def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + rosbot_gazebo = get_package_share_directory("rosbot_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -54,7 +61,14 @@ def generate_test_description(): }.items(), ) - return LaunchDescription([simulation_launch]) + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) @pytest.mark.launch(fixture=generate_test_description) diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py index a6301ada..92ff2570 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py @@ -17,12 +17,15 @@ import launch_pytest import pytest import rclpy +import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc from test_utils import SimulationTestNode from test_ign_kill_utils import kill_ign_linux_processes @@ -30,6 +33,10 @@ @launch_pytest.fixture def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + rosbot_gazebo = get_package_share_directory("rosbot_gazebo") simulation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -55,7 +62,14 @@ def generate_test_description(): }.items(), ) - return LaunchDescription([simulation_launch]) + return LaunchDescription( + [ + simulation_launch, + KeepAliveProc(), + # Tell launch to start the test + ReadyToTest(), + ] + ) @pytest.mark.launch(fixture=generate_test_description) From d796c384bbd8620bcc982a18ad8bd1480c9ceb88 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 2 Jan 2024 16:51:12 +0100 Subject: [PATCH 57/79] Added unbuffered to skip list Signed-off-by: Jakub Delicat --- .wordlist.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/.wordlist.txt b/.wordlist.txt index cb6fa9be..62f3753d 100644 --- a/.wordlist.txt +++ b/.wordlist.txt @@ -148,3 +148,4 @@ stm ttyUSB subprocess cbus +unbuffered From 18a94855d5b00fb7fa956198a432a1aaed36eaae Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 3 Jan 2024 08:17:49 +0100 Subject: [PATCH 58/79] added microros submodules to ignore and removed ros2 control demos from tests Signed-off-by: Jakub Delicat --- .github/workflows/industrial_ci.yaml | 2 +- .gitignore | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index 73a71d04..3230d73f 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -65,7 +65,7 @@ jobs: - name: Remove ign_ros2_control demo shell: bash - run: rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos + run: rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos src/gazebosim/gz_ros2_control/gz_ros2_control_tests # Package micro_ros_msgs does not have industrial ci and tests does not pass. # For more information see https://github.com/micro-ROS/micro_ros_msgs/issues/7 diff --git a/.gitignore b/.gitignore index 2ef10e57..35a1da3b 100644 --- a/.gitignore +++ b/.gitignore @@ -14,6 +14,8 @@ husarion/husarion_office_gz gazebosim/gz_ros2_control diff_drive_controller imu_sensor_broadcaster +micro-ROS-Agent/ +micro_ros_msgs/ # pyspelling *.dic From bd0d0260ac471bfe52bc99019423ed5bf60e43cd Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 9 Jan 2024 15:58:01 +0100 Subject: [PATCH 59/79] Revert "removed bringup tests for testing timeout" This reverts commit f58a96780568bc4edd9cd976dc4f4d8bb29ab554. --- rosbot_bringup/test/test_copyright.py | 23 ++++++ rosbot_bringup/test/test_diff_drive_ekf.py | 67 ++++++++++++++++ rosbot_bringup/test/test_flake8.py | 23 ++++++ rosbot_bringup/test/test_mecanum_ekf.py | 67 ++++++++++++++++ rosbot_bringup/test/test_multirobot_ekf.py | 76 ++++++++++++++++++ .../test/test_namespaced_diff_drive_ekf.py | 68 ++++++++++++++++ .../test/test_namespaced_mecanum_ekf.py | 68 ++++++++++++++++ rosbot_bringup/test/test_pep257.py | 23 ++++++ rosbot_bringup/test/test_utils.py | 79 +++++++++++++++++++ 9 files changed, 494 insertions(+) create mode 100644 rosbot_bringup/test/test_copyright.py create mode 100644 rosbot_bringup/test/test_diff_drive_ekf.py create mode 100644 rosbot_bringup/test/test_flake8.py create mode 100644 rosbot_bringup/test/test_mecanum_ekf.py create mode 100644 rosbot_bringup/test/test_multirobot_ekf.py create mode 100644 rosbot_bringup/test/test_namespaced_diff_drive_ekf.py create mode 100644 rosbot_bringup/test/test_namespaced_mecanum_ekf.py create mode 100644 rosbot_bringup/test/test_pep257.py create mode 100644 rosbot_bringup/test/test_utils.py diff --git a/rosbot_bringup/test/test_copyright.py b/rosbot_bringup/test/test_copyright.py new file mode 100644 index 00000000..f46f861d --- /dev/null +++ b/rosbot_bringup/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/rosbot_bringup/test/test_diff_drive_ekf.py b/rosbot_bringup/test/test_diff_drive_ekf.py new file mode 100644 index 00000000..5da7a3b1 --- /dev/null +++ b/rosbot_bringup/test/test_diff_drive_ekf.py @@ -0,0 +1,67 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_bringup = get_package_share_directory("rosbot_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_bringup_startup_success(): + rclpy.init() + try: + node = BringupTestNode("test_bringup") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected odometry/filtered message but it was not received. Check robot_localization!" + + finally: + rclpy.shutdown() diff --git a/rosbot_bringup/test/test_flake8.py b/rosbot_bringup/test/test_flake8.py new file mode 100644 index 00000000..49c1644f --- /dev/null +++ b/rosbot_bringup/test/test_flake8.py @@ -0,0 +1,23 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@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) diff --git a/rosbot_bringup/test/test_mecanum_ekf.py b/rosbot_bringup/test/test_mecanum_ekf.py new file mode 100644 index 00000000..fcdfc633 --- /dev/null +++ b/rosbot_bringup/test/test_mecanum_ekf.py @@ -0,0 +1,67 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_bringup = get_package_share_directory("rosbot_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_bringup_startup_success(): + rclpy.init() + try: + node = BringupTestNode("test_bringup") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected odometry/filtered message but it was not received. Check robot_localization!" + + finally: + rclpy.shutdown() diff --git a/rosbot_bringup/test/test_multirobot_ekf.py b/rosbot_bringup/test/test_multirobot_ekf.py new file mode 100644 index 00000000..64a38731 --- /dev/null +++ b/rosbot_bringup/test/test_multirobot_ekf.py @@ -0,0 +1,76 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import BringupTestNode + +robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"] + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_bringup = get_package_share_directory("rosbot_bringup") + actions = [] + for i in range(len(robot_names)): + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + "namespace": robot_names[i], + }.items(), + ) + + actions.append(bringup_launch) + + return LaunchDescription(actions) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_bringup_startup_success(): + for robot_name in robot_names: + rclpy.init() + try: + node = BringupTestNode("test_bringup", namespace=robot_name) + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_msg_event.wait(timeout=20.0) + assert msgs_received_flag, ( + f"Expected {robot_name}/odometry/filtered message but it was not received. " + "Check robot_localization!" + ) + + finally: + rclpy.shutdown() diff --git a/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py b/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py new file mode 100644 index 00000000..f78629a7 --- /dev/null +++ b/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py @@ -0,0 +1,68 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_bringup = get_package_share_directory("rosbot_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_bringup_startup_success(): + rclpy.init() + try: + node = BringupTestNode("test_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected odometry/filtered message but it was not received. Check robot_localization!" + + finally: + rclpy.shutdown() diff --git a/rosbot_bringup/test/test_namespaced_mecanum_ekf.py b/rosbot_bringup/test/test_namespaced_mecanum_ekf.py new file mode 100644 index 00000000..a49b8faf --- /dev/null +++ b/rosbot_bringup/test/test_namespaced_mecanum_ekf.py @@ -0,0 +1,68 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_bringup = get_package_share_directory("rosbot_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_bringup_startup_success(): + rclpy.init() + try: + node = BringupTestNode("test_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected odometry/filtered message but it was not received. Check robot_localization!" + + finally: + rclpy.shutdown() diff --git a/rosbot_bringup/test/test_pep257.py b/rosbot_bringup/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/rosbot_bringup/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/rosbot_bringup/test/test_utils.py b/rosbot_bringup/test/test_utils.py new file mode 100644 index 00000000..cc483be1 --- /dev/null +++ b/rosbot_bringup/test/test_utils.py @@ -0,0 +1,79 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Husarion +# +# 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 rclpy + +from threading import Event +from threading import Thread + +from rclpy.node import Node + +from sensor_msgs.msg import JointState, Imu +from nav_msgs.msg import Odometry + + +class BringupTestNode(Node): + ROSBOT_HARDWARE_PUBLISHERS_RATE = 10.0 + + __test__ = False + + def __init__(self, name="test_node", namespace=None): + super().__init__(name, namespace=namespace) + self.odom_msg_event = Event() + + def create_test_subscribers_and_publishers(self): + self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10) + + self.joint_states_publisher = self.create_publisher(JointState, "/_motors_response", 10) + + self.odom_sub = self.create_subscription( + Odometry, "odometry/filtered", self.odometry_callback, 10 + ) + self.timer = None + + def start_node_thread(self): + self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def start_publishing_fake_hardware(self): + self.timer = self.create_timer( + 1.0 / self.ROSBOT_HARDWARE_PUBLISHERS_RATE, + self.timer_callback, + ) + + def timer_callback(self): + self.publish_fake_hardware_messages() + + def odometry_callback(self, data): + self.odom_msg_event.set() + + def publish_fake_hardware_messages(self): + imu_msg = Imu() + imu_msg.header.stamp = self.get_clock().now().to_msg() + imu_msg.header.frame_id = "imu_link" + + joint_state_msg = JointState() + joint_state_msg.header.stamp = self.get_clock().now().to_msg() + joint_state_msg.name = [ + "fl_wheel_joint", + "fr_wheel_joint", + "rl_wheel_joint", + "rr_wheel_joint", + ] + joint_state_msg.position = [0.0, 0.0, 0.0, 0.0] + joint_state_msg.velocity = [0.0, 0.0, 0.0, 0.0] + + self.imu_publisher.publish(imu_msg) + self.joint_states_publisher.publish(joint_state_msg) From 98f3d48e4bbab5477b08c724a383039aaf91bf8e Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 9 Jan 2024 15:58:26 +0100 Subject: [PATCH 60/79] Revert "removed coltroller test for testing timeout" This reverts commit 50c90e5028450640f9b2ce78584a76bceb3e6989. --- .github/workflows/industrial_ci.yaml | 2 +- .../test/test_diff_drive_controllers.py | 101 +++++++++++++++++ .../test/test_mecanum_controllers.py | 101 +++++++++++++++++ .../test/test_multirobot_controllers.py | 84 +++++++++++++++ .../test_namespaced_diff_drive_controllers.py | 102 ++++++++++++++++++ .../test_namespaced_mecanum_controllers.py | 102 ++++++++++++++++++ rosbot_controller/test/test_utils.py | 93 ++++++++++++++++ rosbot_controller/test/test_xacro.py | 55 ++++++++++ 8 files changed, 639 insertions(+), 1 deletion(-) create mode 100644 rosbot_controller/test/test_diff_drive_controllers.py create mode 100644 rosbot_controller/test/test_mecanum_controllers.py create mode 100644 rosbot_controller/test/test_multirobot_controllers.py create mode 100644 rosbot_controller/test/test_namespaced_diff_drive_controllers.py create mode 100644 rosbot_controller/test/test_namespaced_mecanum_controllers.py create mode 100644 rosbot_controller/test/test_utils.py create mode 100644 rosbot_controller/test/test_xacro.py diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index 3230d73f..bdd815b7 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -29,7 +29,7 @@ jobs: ros_industrial_ci: name: ROS Industrial CI runs-on: ubuntu-22.04 - timeout-minutes: 20 + timeout-minutes: 60 needs: - black - spellcheck diff --git a/rosbot_controller/test/test_diff_drive_controllers.py b/rosbot_controller/test/test_diff_drive_controllers.py new file mode 100644 index 00000000..3b69459c --- /dev/null +++ b/rosbot_controller/test/test_diff_drive_controllers.py @@ -0,0 +1,101 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_controller = get_package_share_directory("rosbot_controller") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_controllers_startup_fail(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup") + node.create_test_subscribers_and_publishers() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received JointStates message that should not have appeared. Check whether other" + " robots are connected to your network.!" + ) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Odom message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Imu message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + finally: + rclpy.shutdown() + + +@pytest.mark.launch(fixture=generate_test_description) +def test_controllers_startup_success(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Odom message but it was not received. Check rosbot_base_controller!" + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Imu message but it was not received. Check imu_broadcaster!" + finally: + rclpy.shutdown() diff --git a/rosbot_controller/test/test_mecanum_controllers.py b/rosbot_controller/test/test_mecanum_controllers.py new file mode 100644 index 00000000..5f2607e3 --- /dev/null +++ b/rosbot_controller/test/test_mecanum_controllers.py @@ -0,0 +1,101 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_controller = get_package_share_directory("rosbot_controller") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_controllers_startup_fail(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup") + node.create_test_subscribers_and_publishers() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received JointStates message that should not have appeared. Check whether other" + " robots are connected to your network.!" + ) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Odom message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Imu message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + finally: + rclpy.shutdown() + + +@pytest.mark.launch(fixture=generate_test_description) +def test_controllers_startup_success(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Odom message but it was not received. Check rosbot_base_controller!" + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Imu message but it was not received. Check imu_broadcaster!" + finally: + rclpy.shutdown() diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py new file mode 100644 index 00000000..8f067081 --- /dev/null +++ b/rosbot_controller/test/test_multirobot_controllers.py @@ -0,0 +1,84 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode + +robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"] + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_controller = get_package_share_directory("rosbot_controller") + actions = [] + for i in range(len(robot_names)): + controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + "namespace": robot_names[i], + }.items(), + ) + + actions.append(controller_launch) + + return LaunchDescription(actions) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_multirobot_controllers_startup_success(): + for robot_name in robot_names: + rclpy.init() + try: + node = ControllersTestNode(f"test_{robot_name}_controllers", namespace=robot_name) + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert msgs_received_flag, ( + f"Expected JointStates message but it was not received. Check {robot_name}/" + "joint_state_broadcaster!" + ) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert msgs_received_flag, ( + f"Expected Odom message but it was not received. Check {robot_name}/" + "rosbot_base_controller!" + ) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), f"Expected Imu message but it was not received. Check {robot_name}/imu_broadcaster!" + finally: + rclpy.shutdown() diff --git a/rosbot_controller/test/test_namespaced_diff_drive_controllers.py b/rosbot_controller/test/test_namespaced_diff_drive_controllers.py new file mode 100644 index 00000000..fdab2de6 --- /dev/null +++ b/rosbot_controller/test/test_namespaced_diff_drive_controllers.py @@ -0,0 +1,102 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_controller = get_package_share_directory("rosbot_controller") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_fail(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received JointStates message that should not have appeared. Check whether other" + " robots are connected to your network.!" + ) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Odom message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Imu message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + finally: + rclpy.shutdown() + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_success(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Odom message but it was not received. Check rosbot_base_controller!" + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Imu message but it was not received. Check imu_broadcaster!" + finally: + rclpy.shutdown() diff --git a/rosbot_controller/test/test_namespaced_mecanum_controllers.py b/rosbot_controller/test/test_namespaced_mecanum_controllers.py new file mode 100644 index 00000000..e2321fd0 --- /dev/null +++ b/rosbot_controller/test/test_namespaced_mecanum_controllers.py @@ -0,0 +1,102 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# 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 launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from test_utils import ControllersTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_controller = get_package_share_directory("rosbot_controller") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_controller, + "launch", + "controller.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + "namespace": "rosbot2r", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_fail(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received JointStates message that should not have appeared. Check whether other" + " robots are connected to your network.!" + ) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Odom message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert not msgs_received_flag, ( + "Received Imu message that should not have appeared. Check whether other robots are" + " connected to your network.!" + ) + finally: + rclpy.shutdown() + + +@pytest.mark.launch(fixture=generate_test_description) +def test_namespaced_controllers_startup_success(): + rclpy.init() + try: + node = ControllersTestNode("test_controllers_bringup", namespace="rosbot2r") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Odom message but it was not received. Check rosbot_base_controller!" + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected Imu message but it was not received. Check imu_broadcaster!" + finally: + rclpy.shutdown() diff --git a/rosbot_controller/test/test_utils.py b/rosbot_controller/test/test_utils.py new file mode 100644 index 00000000..831eadad --- /dev/null +++ b/rosbot_controller/test/test_utils.py @@ -0,0 +1,93 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Husarion +# +# 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 rclpy + +from threading import Event +from threading import Thread + +from rclpy.node import Node + +from sensor_msgs.msg import JointState, Imu +from nav_msgs.msg import Odometry + + +class ControllersTestNode(Node): + ROSBOT_HARDWARE_PUBLISHERS_RATE = 10.0 + + __test__ = False + + def __init__(self, name="test_node", namespace=None): + super().__init__(name, namespace=namespace) + self.joint_state_msg_event = Event() + self.odom_msg_event = Event() + self.imu_msg_event = Event() + + def create_test_subscribers_and_publishers(self): + self.joint_state_sub = self.create_subscription( + JointState, "joint_states", self.joint_states_callback, 10 + ) + + self.odom_sub = self.create_subscription( + Odometry, "rosbot_base_controller/odom", self.odometry_callback, 10 + ) + + self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10) + + # TODO: @delipl namespaces have not been implemented in microros yet + self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10) + + self.joint_states_publisher = self.create_publisher(JointState, "/_motors_response", 10) + + self.timer = None + + def start_node_thread(self): + self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def joint_states_callback(self, data): + self.joint_state_msg_event.set() + + def odometry_callback(self, data): + self.odom_msg_event.set() + + def imu_callback(self, data): + self.imu_msg_event.set() + + def start_publishing_fake_hardware(self): + self.timer = self.create_timer( + 1.0 / self.ROSBOT_HARDWARE_PUBLISHERS_RATE, + self.publish_fake_hardware_messages, + ) + + # TODO: @delipl namespaces have not been implemented in microros yet + def publish_fake_hardware_messages(self): + imu_msg = Imu() + imu_msg.header.stamp = self.get_clock().now().to_msg() + imu_msg.header.frame_id = "imu_link" + + joint_state_msg = JointState() + joint_state_msg.header.stamp = self.get_clock().now().to_msg() + joint_state_msg.name = [ + "fl_wheel_joint", + "fr_wheel_joint", + "rl_wheel_joint", + "rr_wheel_joint", + ] + joint_state_msg.position = [0.0, 0.0, 0.0, 0.0] + joint_state_msg.velocity = [0.0, 0.0, 0.0, 0.0] + + self.imu_publisher.publish(imu_msg) + self.joint_states_publisher.publish(joint_state_msg) diff --git a/rosbot_controller/test/test_xacro.py b/rosbot_controller/test/test_xacro.py new file mode 100644 index 00000000..0e52cfe6 --- /dev/null +++ b/rosbot_controller/test/test_xacro.py @@ -0,0 +1,55 @@ +# Copyright 2023 Husarion +# +# 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 os +import xacro +import itertools +from ament_index_python.packages import get_package_share_directory + + +def test_rosbot_description_parsing(): + mecanum_values = ["true", "false"] + use_sim_values = ["true", "false"] + use_gpu_values = ["true", "false"] + simulation_engine_values = ["ignition-gazebo", "webots"] # 'gazebo-classic' + use_multirobot_system_values = ["true", "false"] + + all_combinations = list( + itertools.product( + mecanum_values, + use_sim_values, + use_gpu_values, + simulation_engine_values, + use_multirobot_system_values, + ) + ) + + for combination in all_combinations: + mecanum, use_sim, use_gpu, simulation_engine, use_multirobot_system = combination + mappings = { + "mecanum": mecanum, + "use_sim": use_sim, + "use_gpu": use_gpu, + "simulation_engine": simulation_engine, + "use_multirobot_system": use_multirobot_system, + } + rosbot_description = get_package_share_directory("rosbot_description") + xacro_path = os.path.join(rosbot_description, "urdf/rosbot.urdf.xacro") + try: + xacro.process_file(xacro_path, mappings=mappings) + except xacro.XacroException as e: + assert False, ( + f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, use_sim:" + f" {use_sim}, use_gpu: {use_gpu}, simulation_engine: {simulation_engine}" + ) From ccba3cb544a9d73ab347ded54dd8425aac7b1cae Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 9 Jan 2024 16:07:16 +0100 Subject: [PATCH 61/79] Added nav2_common to dependencies Signed-off-by: Jakub Delicat --- rosbot_gazebo/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/rosbot_gazebo/package.xml b/rosbot_gazebo/package.xml index 7c3d509a..0f0b0e42 100644 --- a/rosbot_gazebo/package.xml +++ b/rosbot_gazebo/package.xml @@ -26,6 +26,7 @@ for details refer to the ros_gz_sim package --> ros_gz_bridge ign_ros2_control + nav2_common python3-pytest launch From a65155383b8dd06463a32786de3d0fe62ab476f5 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 9 Jan 2024 16:14:20 +0100 Subject: [PATCH 62/79] added readme Signed-off-by: Jakub Delicat --- ROS_API.md | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/ROS_API.md b/ROS_API.md index 63336694..6e02cdbb 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -11,7 +11,7 @@ Use `bringup.launch.py` from `rosbot_bringup` to start all base functionalities - `/odometry/filtered` (_nav_msgs/Odometry_) -- `controller.launch.py` from `rosbot_controller`, it loads robot model defined in `rosbot_description` as well as ros2 control [rosbot_hardware_interfaces](https://github.com/husarion/rosbot_hardware_interfaces). It also starts controllers: +Use `controller.launch.py` from `rosbot_controller`, it loads robot model defined in `rosbot_description` as well as ros2 control [rosbot_hardware_interfaces](https://github.com/husarion/rosbot_hardware_interfaces). It also starts controllers: * `joint_state_broadcaster` * `rosbot_base_controller` * `imu_broadcaster` @@ -29,5 +29,16 @@ Use `bringup.launch.py` from `rosbot_bringup` to start all base functionalities - `/imu_broadcaster/imu` (_sensor_msgs/Imu_) Use `simulation.launch.py` from `rosbot_gazebo` to start all base functionalities for ROSbot 2, 2 PRO, 2R in the Gazebo simulator. +If you want to spawn multiple robots use `multirobot_simulation.launch.py` with the `robots` argument e. g.: +```bash +ros2 launch rosbot_gazebo multirobot_simulation.launch.py robots:="robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}" +``` + +If you want to use your own world add to the world's sdf file gazebo sensors plugins inside any `` tag: +```xml + + +``` + > **Warning** > The distance sensors' topics types from Gazebo simulation mismatch with the real ones. The range sensors are not implemented yet in the Gazebo Ignition (for more information look [here](https://github.com/gazebosim/gz-sensors/issues/19)). The real type is [sensor_msgs/msg/Range](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/Range.msg) but simulated [sensor_msgs/msg/LaserScan](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/LaserScan.msg). The first value of the `ranges` in [sensor_msgs/msg/LaserScan](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/LaserScan.msg) is the `range` field of [sensor_msgs/msg/Range](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/Range.msg). From ef727ed80ddfc37fd2d9b1f182d34c16bd047ca8 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 9 Jan 2024 16:17:33 +0100 Subject: [PATCH 63/79] Get the newest version of gz_ros2_control Signed-off-by: Jakub Delicat --- rosbot/rosbot_simulation.repos | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos index deedc368..3fedb2c5 100644 --- a/rosbot/rosbot_simulation.repos +++ b/rosbot/rosbot_simulation.repos @@ -1,8 +1,7 @@ repositories: gazebosim/gz_ros2_control: type: git - # Added feature with control manager name. Waiting on https://github.com/ros-controls/gz_ros2_control/pull/212 - url: https://github.com/delihus/gz_ros2_control + url: https://github.com/ros-controls/gz_ros2_control version: humble husarion/husarion_office_gz: From 1862b5b7eb428690834f90c082d86260d678ca81 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 9 Jan 2024 16:33:43 +0100 Subject: [PATCH 64/79] removed sim_time from yaml Signed-off-by: Jakub Delicat --- rosbot_controller/config/mecanum_drive_controller.yaml | 2 -- 1 file changed, 2 deletions(-) diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml index de40cc07..4ee7d87e 100644 --- a/rosbot_controller/config/mecanum_drive_controller.yaml +++ b/rosbot_controller/config/mecanum_drive_controller.yaml @@ -22,8 +22,6 @@ /**/rosbot_base_controller: ros__parameters: - # TODO: (@delihus) use_sim_time is parsing here. - use_sim_time: True tf_frame_prefix_enable: false front_left_wheel_name: fl_wheel_joint From 1329f399d3e4b8f199eb56edb66bd4ed64ebc99f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 10 Jan 2024 15:03:20 +0100 Subject: [PATCH 65/79] Added range sensor Signed-off-by: Jakub Delicat --- rosbot_description/urdf/body.urdf.xacro | 10 ++-- .../urdf/components/vl53lox.urdf.xacro | 53 ++++++++++++++++++- .../urdf/rosbot_macro.urdf.xacro | 2 +- 3 files changed, 58 insertions(+), 7 deletions(-) diff --git a/rosbot_description/urdf/body.urdf.xacro b/rosbot_description/urdf/body.urdf.xacro index c178f1f8..647087b0 100644 --- a/rosbot_description/urdf/body.urdf.xacro +++ b/rosbot_description/urdf/body.urdf.xacro @@ -1,7 +1,7 @@ - + @@ -79,10 +79,10 @@ - - - - + + + + diff --git a/rosbot_description/urdf/components/vl53lox.urdf.xacro b/rosbot_description/urdf/components/vl53lox.urdf.xacro index ad1fd22c..39365688 100644 --- a/rosbot_description/urdf/components/vl53lox.urdf.xacro +++ b/rosbot_description/urdf/components/vl53lox.urdf.xacro @@ -1,7 +1,16 @@ - + + + + + + + + + + @@ -11,6 +20,48 @@ + + + + + + ${gz_control_namespace}range/${prefix} + ${prefix}_range + ${prefix}_range + + 5.0 + + + + 1 + 1 + -0.01 + 0.01 + + + + 0.01 + 0.90 + 0.02 + + + gaussian + 0.1 + 0.005 + + + true + false + + + + + + ogre2 + + + diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index 3ef9db0b..bd892ac7 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -23,7 +23,7 @@ - + From f00b6d9b4dbb2072427552feb1e63e64197658a6 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 10 Jan 2024 16:10:05 +0100 Subject: [PATCH 66/79] Added tests for ranges and camera Signed-off-by: Jakub Delicat --- .../urdf/components/vl53lox.urdf.xacro | 9 +-- rosbot_description/urdf/rosbot.urdf.xacro | 28 ++++----- rosbot_gazebo/launch/spawn.launch.py | 6 +- .../test/test_diff_drive_simulation.py | 16 +++++- rosbot_gazebo/test/test_flake8.py | 4 +- rosbot_gazebo/test/test_mecanum_simulation.py | 16 +++++- .../test_multirobot_diff_drive_simulation.py | 16 +++++- .../test_multirobot_mecanum_simulation.py | 16 +++++- .../test_namespaced_diff_drive_simulation.py | 16 +++++- .../test_namespaced_mecanum_simulation.py | 16 +++++- rosbot_gazebo/test/test_utils.py | 57 +++++++++++++++++-- 11 files changed, 165 insertions(+), 35 deletions(-) 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() From 8c9882e50e113164c20e3876e855574bbf76a4d6 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 11 Jan 2024 08:24:49 +0100 Subject: [PATCH 67/79] removed unnecesary if Signed-off-by: Jakub Delicat --- rosbot_description/urdf/rosbot_macro.urdf.xacro | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index bd892ac7..cc42ab12 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -148,11 +148,9 @@ false imu_link imu_link - - - ${namespace_ext} - - + + ${namespace_ext} + From 711bf16f4f595b48c5220592039f2ec8923068ee Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 11 Jan 2024 09:25:16 +0100 Subject: [PATCH 68/79] Added me to the authors Signed-off-by: Jakub Delicat --- rosbot_gazebo/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/rosbot_gazebo/package.xml b/rosbot_gazebo/package.xml index 0f0b0e42..fd526e9e 100644 --- a/rosbot_gazebo/package.xml +++ b/rosbot_gazebo/package.xml @@ -7,6 +7,7 @@ Gazebo Ignition simulation for ROSbot 2, 2R, PRO Apache License 2.0 + Jakub Delicat Krzysztof Wojciechowski Husarion From 095d4a8e8e042d19ea82fe597e1718efbd0cbb30 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Thu, 11 Jan 2024 11:55:17 +0100 Subject: [PATCH 69/79] Update rosbot_hardware.repos --- rosbot/rosbot_hardware.repos | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rosbot/rosbot_hardware.repos b/rosbot/rosbot_hardware.repos index 1d9e14c7..d94215af 100644 --- a/rosbot/rosbot_hardware.repos +++ b/rosbot/rosbot_hardware.repos @@ -6,18 +6,18 @@ repositories: ros_components_description: type: git url: https://github.com/husarion/ros_components_description.git - version: gazebo-ignition-multirobots + version: ros2 rosbot_controllers: type: git url: https://github.com/husarion/rosbot_controllers - version: update-params + version: main # Waiting for backporting to the humble version # https://github.com/ros-controls/ros2_controllers/pull/726 and # https://github.com/ros-controls/ros2_controllers/pull/831 ros2_controllers: type: git url: https://github.com/delihus/ros2_controllers - version: imu-sensor-broadcaster-features + version: humble micro_ros_msgs: type: git url: https://github.com/micro-ROS/micro_ros_msgs.git From c2c27ac98ff3662cdd0c5322f74b66192e97da16 Mon Sep 17 00:00:00 2001 From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> Date: Thu, 11 Jan 2024 11:56:11 +0100 Subject: [PATCH 70/79] Update rosbot_simulation.repos --- rosbot/rosbot_simulation.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos index 3fedb2c5..40ba729f 100644 --- a/rosbot/rosbot_simulation.repos +++ b/rosbot/rosbot_simulation.repos @@ -7,4 +7,4 @@ repositories: husarion/husarion_office_gz: type: git url: https://github.com/husarion/husarion_office_gz - version: add-gazebo-plugins + version: main From 1b93658515cef0854024b9e3758a84f42eb81ecc Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Thu, 11 Jan 2024 14:19:49 +0100 Subject: [PATCH 71/79] Update .github/workflows/industrial_ci.yaml Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- .github/workflows/industrial_ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index bdd815b7..51fb5b1c 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -29,7 +29,7 @@ jobs: ros_industrial_ci: name: ROS Industrial CI runs-on: ubuntu-22.04 - timeout-minutes: 60 + timeout-minutes: 30 needs: - black - spellcheck From f300449168744ff63313a7680b73f83e55ea62af Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Thu, 11 Jan 2024 14:20:01 +0100 Subject: [PATCH 72/79] Update .github/workflows/industrial_ci.yaml Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- .github/workflows/industrial_ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index 51fb5b1c..eb68cd17 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -63,7 +63,7 @@ jobs: cp -r src/ros2_controllers/imu_sensor_broadcaster src/ rm -rf src/ros2_controllers - - name: Remove ign_ros2_control demo + - name: Remove ign_ros2_control demo and tests shell: bash run: rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos src/gazebosim/gz_ros2_control/gz_ros2_control_tests From c3663c96365ce9d4ffa9dfbb7b4e487811c536e6 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 11 Jan 2024 14:22:24 +0100 Subject: [PATCH 73/79] default simulator is ignition-gazebo Signed-off-by: Jakub Delicat --- rosbot_description/urdf/rosbot.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro index 97b6d170..eccac3db 100644 --- a/rosbot_description/urdf/rosbot.urdf.xacro +++ b/rosbot_description/urdf/rosbot.urdf.xacro @@ -4,7 +4,7 @@ - + From 69c4e9b04b9a8bd2349770b0179f761bea9f0728 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 11 Jan 2024 15:14:16 +0100 Subject: [PATCH 74/79] merged multiple_simulation.launch.py with simulation.launch.py Signed-off-by: Jakub Delicat --- .github/workflows/industrial_ci.yaml | 3 +- README.md | 3 + ROS_API.md | 4 +- .../urdf/rosbot_macro.urdf.xacro | 6 +- .../launch/multirobot_simulation.launch.py | 150 ------------------ rosbot_gazebo/launch/simulation.launch.py | 118 +++++++++++--- .../test/test_diff_drive_simulation.py | 4 +- rosbot_gazebo/test/test_flake8.py | 4 +- rosbot_gazebo/test/test_mecanum_simulation.py | 4 +- .../test_multirobot_diff_drive_simulation.py | 6 +- .../test_multirobot_mecanum_simulation.py | 6 +- .../test_namespaced_diff_drive_simulation.py | 4 +- .../test_namespaced_mecanum_simulation.py | 4 +- rosbot_gazebo/test/test_utils.py | 25 +-- 14 files changed, 118 insertions(+), 223 deletions(-) delete mode 100644 rosbot_gazebo/launch/multirobot_simulation.launch.py diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml index bdd815b7..d06c14f6 100644 --- a/.github/workflows/industrial_ci.yaml +++ b/.github/workflows/industrial_ci.yaml @@ -54,7 +54,8 @@ jobs: shell: bash run: | python3 -m pip install -U vcstool - vcs import src < src/rosbot/rosbot_hardware.repos && vcs import src < src/rosbot/rosbot_simulation.repos + vcs import src < src/rosbot/rosbot_hardware.repos && + vcs import src < src/rosbot/rosbot_simulation.repos - name: Copy only diff_drive_controller and imu_sensor_broadcaster, waits for features from ros2-control shell: bash diff --git a/README.md b/README.md index 26aea31f..daf3ab27 100644 --- a/README.md +++ b/README.md @@ -100,6 +100,9 @@ vcs import src < src/rosbot/rosbot_simulation.repos # Build only diff_drive_controller and imu_sensor_broadcaster from ros2_controllers cp -r src/ros2_controllers/diff_drive_controller src && cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_controllers +# Remove ign_ros2_control demo and test +rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos src/gazebosim/gz_ros2_control/gz_ros2_control_tests + sudo rosdep init rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y diff --git a/ROS_API.md b/ROS_API.md index 6e02cdbb..3889c04d 100644 --- a/ROS_API.md +++ b/ROS_API.md @@ -29,9 +29,9 @@ Use `controller.launch.py` from `rosbot_controller`, it loads robot model define - `/imu_broadcaster/imu` (_sensor_msgs/Imu_) Use `simulation.launch.py` from `rosbot_gazebo` to start all base functionalities for ROSbot 2, 2 PRO, 2R in the Gazebo simulator. -If you want to spawn multiple robots use `multirobot_simulation.launch.py` with the `robots` argument e. g.: +If you want to spawn multiple robots use `simulation.launch.py` with the `robots` argument e. g.: ```bash -ros2 launch rosbot_gazebo multirobot_simulation.launch.py robots:="robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}" +ros2 launch rosbot_gazebo simulation.launch.py robots:='robot1={x: 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0}; robot4={x: 3.0, y: -1.0}' ``` If you want to use your own world add to the world's sdf file gazebo sensors plugins inside any `` tag: diff --git a/rosbot_description/urdf/rosbot_macro.urdf.xacro b/rosbot_description/urdf/rosbot_macro.urdf.xacro index cc42ab12..48b30f2c 100644 --- a/rosbot_description/urdf/rosbot_macro.urdf.xacro +++ b/rosbot_description/urdf/rosbot_macro.urdf.xacro @@ -4,11 +4,9 @@ - - @@ -133,7 +131,9 @@ - ${gz_control_namespace} + + ${namespace} + rosbot_base_controller/cmd_vel_unstamped:=cmd_vel /tf:=tf diff --git a/rosbot_gazebo/launch/multirobot_simulation.launch.py b/rosbot_gazebo/launch/multirobot_simulation.launch.py deleted file mode 100644 index b1bf9aa1..00000000 --- a/rosbot_gazebo/launch/multirobot_simulation.launch.py +++ /dev/null @@ -1,150 +0,0 @@ -# Copyright 2023 Husarion -# -# 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. - -from launch import LaunchDescription -from launch.actions import ( - IncludeLaunchDescription, - DeclareLaunchArgument, - LogInfo, - GroupAction, -) -from launch.substitutions import ( - PathJoinSubstitution, - PythonExpression, - LaunchConfiguration, - TextSubstitution, -) -from launch.launch_description_sources import PythonLaunchDescriptionSource - -from launch_ros.actions import SetParameter - -from ament_index_python.packages import get_package_share_directory - -from nav2_common.launch import ParseMultiRobotPose - - -def generate_launch_description(): - mecanum = LaunchConfiguration("mecanum") - declare_mecanum_arg = DeclareLaunchArgument( - "mecanum", - default_value="False", - description=( - "Whether to use mecanum drive controller (otherwise diff drive controller is used)" - ), - ) - - world_package = get_package_share_directory("husarion_office_gz") - world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"]) - world_cfg = LaunchConfiguration("world") - declare_world_arg = DeclareLaunchArgument( - "world", default_value=world_file, description="SDF world file" - ) - - headless = LaunchConfiguration("headless") - declare_headless_arg = DeclareLaunchArgument( - "headless", - default_value="False", - description="Run Gazebo Ignition in the headless mode", - ) - - headless_cfg = PythonExpression( - [ - "'--headless-rendering -s -v 4 -r' if ", - headless, - " else '-r'", - ] - ) - gz_args = [headless_cfg, " ", world_cfg] - - use_gpu = LaunchConfiguration("use_gpu") - declare_use_gpu_arg = DeclareLaunchArgument( - "use_gpu", - default_value="True", - description="Whether GPU acceleration is used", - ) - - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("ros_gz_sim"), - "launch", - "gz_sim.launch.py", - ] - ) - ), - launch_arguments={ - "gz_args": gz_args, - "on_exit_shutdown": "True", - }.items(), - ) - - robots_list = ParseMultiRobotPose("robots").value() - - # Define commands for launching the navigation instances - bringup_cmd_group = [] - for robot_name in robots_list: - init_pose = robots_list[robot_name] - group = GroupAction( - [ - LogInfo( - msg=[ - "Launching namespace=", - robot_name, - " init_pose=", - str(init_pose), - ] - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("rosbot_gazebo"), - "launch", - "spawn.launch.py", - ] - ) - ), - launch_arguments={ - "mecanum": mecanum, - "use_sim": "True", - "use_gpu": use_gpu, - "simulation_engine": "ignition-gazebo", - "namespace": TextSubstitution(text=robot_name), - "x": TextSubstitution(text=str(init_pose["x"])), - "y": TextSubstitution(text=str(init_pose["y"])), - "z": TextSubstitution(text=str(init_pose["z"])), - "roll": TextSubstitution(text=str(init_pose["roll"])), - "pitch": TextSubstitution(text=str(init_pose["pitch"])), - "yaw": TextSubstitution(text=str(init_pose["yaw"])), - }.items(), - ), - ] - ) - - bringup_cmd_group.append(group) - - return LaunchDescription( - [ - declare_mecanum_arg, - declare_world_arg, - declare_headless_arg, - declare_use_gpu_arg, - # Sets use_sim_time for all nodes started below - # (doesn't work for nodes started from ignition gazebo) - SetParameter(name="use_sim_time", value=True), - gz_sim, - ] - + bringup_cmd_group - ) diff --git a/rosbot_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py index 220a8947..7ff79d72 100644 --- a/rosbot_gazebo/launch/simulation.launch.py +++ b/rosbot_gazebo/launch/simulation.launch.py @@ -16,11 +16,15 @@ from launch.actions import ( IncludeLaunchDescription, DeclareLaunchArgument, + LogInfo, + LogWarn, + GroupAction, ) from launch.substitutions import ( PathJoinSubstitution, PythonExpression, LaunchConfiguration, + TextSubstitution, ) from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -28,6 +32,8 @@ from ament_index_python.packages import get_package_share_directory +from nav2_common.launch import ParseMultiRobotPose + def generate_launch_description(): namespace = LaunchConfiguration("namespace") @@ -76,6 +82,16 @@ def generate_launch_description(): description="Whether GPU acceleration is used", ) + declare_robots_arg = DeclareLaunchArgument( + "robots", + default_value="", + description=( + "Spawning multiple robots at positions with yaw orientations e. g. robots:='robot1={x:" + " 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0}; robot4={x: 3.0," + " y: -1.0}'" + ), + ) + gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -92,30 +108,81 @@ def generate_launch_description(): }.items(), ) - spawn_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("rosbot_gazebo"), - "launch", - "spawn.launch.py", - ] - ) - ), - launch_arguments={ - "mecanum": mecanum, - "use_sim": "True", - "use_gpu": use_gpu, - "simulation_engine": "ignition-gazebo", - "namespace": namespace, - "x": LaunchConfiguration("x", default="0.00"), - "y": LaunchConfiguration("y", default="2.00"), - "z": LaunchConfiguration("z", default="0.20"), - "roll": LaunchConfiguration("roll", default="0.00"), - "pitch": LaunchConfiguration("pitch", default="0.00"), - "yaw": LaunchConfiguration("yaw", default="0.00"), - }.items(), - ) + robots_list = ParseMultiRobotPose("robots").value() + spawn_group = [] + if len(robots_list) == 0: + spawn_single_robot = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("rosbot_gazebo"), + "launch", + "spawn.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": mecanum, + "use_sim": "True", + "use_gpu": use_gpu, + "simulation_engine": "ignition-gazebo", + "namespace": namespace, + "x": LaunchConfiguration("x", default="0.00"), + "y": LaunchConfiguration("y", default="2.00"), + "z": LaunchConfiguration("z", default="0.20"), + "roll": LaunchConfiguration("roll", default="0.00"), + "pitch": LaunchConfiguration("pitch", default="0.00"), + "yaw": LaunchConfiguration("yaw", default="0.00"), + }.items(), + ) + spawn_group.append(spawn_single_robot) + + for robot_name in robots_list: + init_pose = robots_list[robot_name] + group = GroupAction( + [ + LogInfo( + msg=[ + "Launching namespace=", + robot_name, + " init_pose=", + str(init_pose), + ] + ), + LogWarn( + msg=[ + "Parameter 'namespace' should not be used with robots parameter. Skipping" + " namespace..." + ], + condition=namespace, + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("rosbot_gazebo"), + "launch", + "spawn.launch.py", + ] + ) + ), + launch_arguments={ + "mecanum": mecanum, + "use_sim": "True", + "use_gpu": use_gpu, + "simulation_engine": "ignition-gazebo", + "namespace": TextSubstitution(text=robot_name), + "x": TextSubstitution(text=str(init_pose["x"])), + "y": TextSubstitution(text=str(init_pose["y"])), + "z": TextSubstitution(text=str(init_pose["z"])), + "roll": TextSubstitution(text=str(init_pose["roll"])), + "pitch": TextSubstitution(text=str(init_pose["pitch"])), + "yaw": TextSubstitution(text=str(init_pose["yaw"])), + }.items(), + ), + ] + ) + spawn_group.append(group) return LaunchDescription( [ @@ -124,10 +191,11 @@ def generate_launch_description(): declare_world_arg, declare_headless_arg, declare_use_gpu_arg, + declare_robots_arg, # Sets use_sim_time for all nodes started below # (doesn't work for nodes started from ignition gazebo) SetParameter(name="use_sim_time", value=True), gz_sim, - spawn_launch, ] + + spawn_group, ) diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py index e941c497..bcbc83ff 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_diff_drive_simulation.py @@ -105,9 +105,7 @@ 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!" diff --git a/rosbot_gazebo/test/test_flake8.py b/rosbot_gazebo/test/test_flake8.py index ee79f31a..49c1644f 100644 --- a/rosbot_gazebo/test/test_flake8.py +++ b/rosbot_gazebo/test/test_flake8.py @@ -20,6 +20,4 @@ @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 0e51fc49..9a20adb6 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_mecanum_simulation.py @@ -118,9 +118,7 @@ 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!" diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py index 50799df1..02607d8d 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -43,7 +43,7 @@ def generate_test_description(): "ros2", "launch", "rosbot_gazebo", - "multirobot_simulation.launch.py", + "simulation.launch.py", ( f'world:={get_package_share_directory("husarion_office_gz")}' "/worlds/empty_with_plugins.sdf" @@ -78,9 +78,7 @@ 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: diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py index 2c1b035d..c99bbc9b 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py @@ -45,7 +45,7 @@ def generate_test_description(): "ros2", "launch", "rosbot_gazebo", - "multirobot_simulation.launch.py", + "simulation.launch.py", ( f'world:={get_package_share_directory("husarion_office_gz")}' "/worlds/empty_with_plugins.sdf" @@ -81,9 +81,7 @@ 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: diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py index 93e36c3a..f68440cc 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py @@ -101,9 +101,7 @@ 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!" diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py index ea2d364a..a8ea08eb 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py @@ -114,9 +114,7 @@ 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!" diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index a9e0c44d..4c80c43a 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -38,9 +38,7 @@ 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 @@ -84,15 +82,11 @@ 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 - ) + sub = self.create_subscription(LaserScan, range_topic_name, self.ranges_callback, 10) self.range_subs.append(sub) self.camera_color_sub = self.create_subscription( @@ -106,15 +100,11 @@ def create_test_subscribers_and_publishers(self): 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 @@ -138,10 +128,7 @@ 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): From 8e7287541e8589ef89847cd3a1a2f0f3f201eaed Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 11 Jan 2024 15:30:21 +0100 Subject: [PATCH 75/79] unnecessary event Signed-off-by: Jakub Delicat --- rosbot_gazebo/test/test_utils.py | 1 - 1 file changed, 1 deletion(-) diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index 4c80c43a..a4a3a071 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -57,7 +57,6 @@ def __init__(self, name="test_node", namespace=None): 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): self.controller_odom_flag = False From 47700e658674414eb24ba559b6b09df94fc72097 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 11 Jan 2024 17:39:25 +0100 Subject: [PATCH 76/79] fix simulation.launch.py --- rosbot_gazebo/launch/simulation.launch.py | 168 ++++++++++------------ 1 file changed, 73 insertions(+), 95 deletions(-) diff --git a/rosbot_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py index 7ff79d72..c8706de6 100644 --- a/rosbot_gazebo/launch/simulation.launch.py +++ b/rosbot_gazebo/launch/simulation.launch.py @@ -17,80 +17,34 @@ IncludeLaunchDescription, DeclareLaunchArgument, LogInfo, - LogWarn, GroupAction, + OpaqueFunction, ) from launch.substitutions import ( PathJoinSubstitution, - PythonExpression, LaunchConfiguration, TextSubstitution, ) from launch.launch_description_sources import PythonLaunchDescriptionSource - from launch_ros.actions import SetParameter - from ament_index_python.packages import get_package_share_directory - from nav2_common.launch import ParseMultiRobotPose -def generate_launch_description(): - namespace = LaunchConfiguration("namespace") - declare_namespace_arg = DeclareLaunchArgument( - "namespace", - default_value="", - description="Namespace for all topics and tfs", - ) - - mecanum = LaunchConfiguration("mecanum") - declare_mecanum_arg = DeclareLaunchArgument( - "mecanum", - default_value="False", - description=( - "Whether to use mecanum drive controller (otherwise diff drive controller is used)" - ), - ) - - world_package = get_package_share_directory("husarion_office_gz") - world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"]) - world_cfg = LaunchConfiguration("world") - declare_world_arg = DeclareLaunchArgument( - "world", default_value=world_file, description="SDF world file" - ) - - headless = LaunchConfiguration("headless") - declare_headless_arg = DeclareLaunchArgument( - "headless", - default_value="False", - description="Run Gazebo Ignition in the headless mode", - ) - - headless_cfg = PythonExpression( - [ - "'--headless-rendering -s -v 4 -r' if ", - headless, - " else '-r'", - ] - ) - gz_args = [headless_cfg, " ", world_cfg] +def launch_setup(context, *args, **kwargs): + namespace = LaunchConfiguration("namespace").perform(context) + mecanum = LaunchConfiguration("mecanum").perform(context) + world = LaunchConfiguration("world").perform(context) + headless = LaunchConfiguration("headless").perform(context) + use_gpu = LaunchConfiguration("use_gpu").perform(context) + x = LaunchConfiguration("x", default="0.0").perform(context) + y = LaunchConfiguration("y", default="2.0").perform(context) + z = LaunchConfiguration("z", default="0.0").perform(context) + roll = LaunchConfiguration("roll", default="0.0").perform(context) + pitch = LaunchConfiguration("pitch", default="0.0").perform(context) + yaw = LaunchConfiguration("yaw", default="0.0").perform(context) - use_gpu = LaunchConfiguration("use_gpu") - declare_use_gpu_arg = DeclareLaunchArgument( - "use_gpu", - default_value="True", - description="Whether GPU acceleration is used", - ) - - declare_robots_arg = DeclareLaunchArgument( - "robots", - default_value="", - description=( - "Spawning multiple robots at positions with yaw orientations e. g. robots:='robot1={x:" - " 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0}; robot4={x: 3.0," - " y: -1.0}'" - ), - ) + gz_args = f"--headless-rendering -s -v 4 -r {world}" if eval(headless) else f"-r {world}" gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -109,34 +63,19 @@ def generate_launch_description(): ) robots_list = ParseMultiRobotPose("robots").value() - spawn_group = [] if len(robots_list) == 0: - spawn_single_robot = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - get_package_share_directory("rosbot_gazebo"), - "launch", - "spawn.launch.py", - ] - ) - ), - launch_arguments={ - "mecanum": mecanum, - "use_sim": "True", - "use_gpu": use_gpu, - "simulation_engine": "ignition-gazebo", - "namespace": namespace, - "x": LaunchConfiguration("x", default="0.00"), - "y": LaunchConfiguration("y", default="2.00"), - "z": LaunchConfiguration("z", default="0.20"), - "roll": LaunchConfiguration("roll", default="0.00"), - "pitch": LaunchConfiguration("pitch", default="0.00"), - "yaw": LaunchConfiguration("yaw", default="0.00"), - }.items(), - ) - spawn_group.append(spawn_single_robot) + robots_list = { + namespace: { + "x": x, + "y": y, + "z": z, + "roll": roll, + "pitch": pitch, + "yaw": yaw, + } + } + spawn_group = [] for robot_name in robots_list: init_pose = robots_list[robot_name] group = GroupAction( @@ -149,13 +88,6 @@ def generate_launch_description(): str(init_pose), ] ), - LogWarn( - msg=[ - "Parameter 'namespace' should not be used with robots parameter. Skipping" - " namespace..." - ], - condition=namespace, - ), IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -184,6 +116,53 @@ def generate_launch_description(): ) spawn_group.append(group) + return [gz_sim, *spawn_group] + + +def generate_launch_description(): + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + + declare_mecanum_arg = DeclareLaunchArgument( + "mecanum", + default_value="False", + description=( + "Whether to use mecanum drive controller (otherwise diff drive controller is used)" + ), + ) + + world_package = get_package_share_directory("husarion_office_gz") + world_file = PathJoinSubstitution([world_package, "worlds", "husarion_world.sdf"]) + declare_world_arg = DeclareLaunchArgument( + "world", default_value=world_file, description="SDF world file" + ) + + declare_headless_arg = DeclareLaunchArgument( + "headless", + default_value="False", + description="Run Gazebo Ignition in the headless mode", + choices=["True", "False"], + ) + + declare_use_gpu_arg = DeclareLaunchArgument( + "use_gpu", + default_value="True", + description="Whether GPU acceleration is used", + ) + + declare_robots_arg = DeclareLaunchArgument( + "robots", + default_value="", + description=( + "Spawning multiple robots at positions with yaw orientations e. g. robots:='robot1={x:" + " 0.0, y: -1.0}; robot2={x: 1.0, y: -1.0}; robot3={x: 2.0, y: -1.0}; robot4={x: 3.0," + " y: -1.0}'" + ), + ) + return LaunchDescription( [ declare_namespace_arg, @@ -195,7 +174,6 @@ def generate_launch_description(): # Sets use_sim_time for all nodes started below # (doesn't work for nodes started from ignition gazebo) SetParameter(name="use_sim_time", value=True), - gz_sim, + OpaqueFunction(function=launch_setup), ] - + spawn_group, ) From 2b00c9c9b9dde9da6a60fc3762b14da1905c4f4d Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 11 Jan 2024 18:11:58 +0100 Subject: [PATCH 77/79] test max 3 robots + env(NS) + readme --- README.md | 4 ++-- rosbot_bringup/config/ekf.yaml | 2 +- rosbot_bringup/launch/bringup.launch.py | 4 ++-- rosbot_bringup/test/test_multirobot_ekf.py | 2 +- rosbot_controller/test/test_multirobot_controllers.py | 2 +- rosbot_gazebo/launch/simulation.launch.py | 3 ++- rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py | 2 +- rosbot_gazebo/test/test_multirobot_mecanum_simulation.py | 4 +--- 8 files changed, 11 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index daf3ab27..5ab328d1 100644 --- a/README.md +++ b/README.md @@ -62,7 +62,7 @@ rm -r src/rosbot_gazebo sudo rosdep init rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y -colcon build +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release ``` > **Prerequisites** @@ -106,7 +106,7 @@ rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos src/gazebosim/gz_ros sudo rosdep init rosdep update --rosdistro $ROS_DISTRO rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y -colcon build +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release ``` Running: diff --git a/rosbot_bringup/config/ekf.yaml b/rosbot_bringup/config/ekf.yaml index c2d5a8f9..b8bb1672 100644 --- a/rosbot_bringup/config/ekf.yaml +++ b/rosbot_bringup/config/ekf.yaml @@ -3,7 +3,7 @@ ## ekf config file ### /**/ekf_filter_node: ros__parameters: - frequency: 25.0 + frequency: 20.0 sensor_timeout: 0.05 two_d_mode: true diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index dcf8eade..b77d3137 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -17,14 +17,14 @@ from ament_index_python.packages import get_package_share_directory from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution def generate_launch_description(): namespace = LaunchConfiguration("namespace") declare_namespace_arg = DeclareLaunchArgument( "namespace", - default_value="", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), description="Namespace for all topics and tfs", ) diff --git a/rosbot_bringup/test/test_multirobot_ekf.py b/rosbot_bringup/test/test_multirobot_ekf.py index 64a38731..57ae7a6c 100644 --- a/rosbot_bringup/test/test_multirobot_ekf.py +++ b/rosbot_bringup/test/test_multirobot_ekf.py @@ -25,7 +25,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from test_utils import BringupTestNode -robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"] +robot_names = ["robot1", "robot2", "robot3"] @launch_pytest.fixture diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py index 8f067081..130956cf 100644 --- a/rosbot_controller/test/test_multirobot_controllers.py +++ b/rosbot_controller/test/test_multirobot_controllers.py @@ -25,7 +25,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from test_utils import ControllersTestNode -robot_names = ["rosbot1", "rosbot2", "rosbot3", "rosbot4"] +robot_names = ["robot1", "robot2", "robot3"] @launch_pytest.fixture diff --git a/rosbot_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py index c8706de6..962d9517 100644 --- a/rosbot_gazebo/launch/simulation.launch.py +++ b/rosbot_gazebo/launch/simulation.launch.py @@ -21,6 +21,7 @@ OpaqueFunction, ) from launch.substitutions import ( + EnvironmentVariable, PathJoinSubstitution, LaunchConfiguration, TextSubstitution, @@ -122,7 +123,7 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): declare_namespace_arg = DeclareLaunchArgument( "namespace", - default_value="", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), description="Namespace for all topics and tfs", ) diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py index 02607d8d..93188bfd 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -66,7 +66,7 @@ def generate_test_description(): @pytest.mark.launch(fixture=generate_test_description) def test_multirobot_simulation(): - robot_names = ["robot1", "robot2", "robot3", "robot4"] + robot_names = ["robot1", "robot2", "robot3"] rclpy.init() try: nodes = [] diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py index c99bbc9b..f7467c7e 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py @@ -30,8 +30,6 @@ from test_utils import SimulationTestNode from test_ign_kill_utils import kill_ign_linux_processes -robot_names = ["robot1", "robot2", "robot3", "robot4"] - @launch_pytest.fixture def generate_test_description(): @@ -69,7 +67,7 @@ def generate_test_description(): @pytest.mark.launch(fixture=generate_test_description) def test_multirobot_mecanum_simulation(): - robot_names = ["robot1", "robot2", "robot3", "robot4"] + robot_names = ["robot1", "robot2", "robot3"] rclpy.init() try: nodes = [] From 1a638206a8223bb2b608ec5a9b5fa5db0f2f44ec Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 11 Jan 2024 18:48:14 +0100 Subject: [PATCH 78/79] Better diagnostic in multirobot test --- .../test/test_multirobot_controllers.py | 6 +- .../test_multirobot_diff_drive_simulation.py | 72 ++++++++------- .../test_multirobot_mecanum_simulation.py | 87 +++++++++++-------- rosbot_gazebo/test/test_utils.py | 2 + 4 files changed, 97 insertions(+), 70 deletions(-) diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py index 130956cf..6e7963a7 100644 --- a/rosbot_controller/test/test_multirobot_controllers.py +++ b/rosbot_controller/test/test_multirobot_controllers.py @@ -66,17 +66,17 @@ def test_multirobot_controllers_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + msgs_received_flag = node.joint_state_msg_event.wait(timeout=20.0) assert msgs_received_flag, ( f"Expected JointStates message but it was not received. Check {robot_name}/" "joint_state_broadcaster!" ) - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + msgs_received_flag = node.odom_msg_event.wait(timeout=20.0) assert msgs_received_flag, ( f"Expected Odom message but it was not received. Check {robot_name}/" "rosbot_base_controller!" ) - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + msgs_received_flag = node.imu_msg_event.wait(timeout=20.0) assert ( msgs_received_flag ), f"Expected Imu message but it was not received. Check {robot_name}/imu_broadcaster!" diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py index 93188bfd..ecfa7b29 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -22,10 +22,10 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ExecuteProcess -from rclpy.executors import MultiThreadedExecutor -from threading import Thread from launch_testing.actions import ReadyToTest from launch_testing.util import KeepAliveProc +from rclpy.executors import MultiThreadedExecutor +from threading import Thread from test_utils import SimulationTestNode from test_ign_kill_utils import kill_ign_linux_processes @@ -48,7 +48,7 @@ def generate_test_description(): f'world:={get_package_share_directory("husarion_office_gz")}' "/worlds/empty_with_plugins.sdf" ), - "robots:=robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}", + "robots:=robot1={y: -4.0}; robot2={y: 0.0}; robot3={y: 4.0};", "headless:=True", ], output="screen", @@ -69,64 +69,74 @@ def test_multirobot_simulation(): robot_names = ["robot1", "robot2", "robot3"] rclpy.init() try: - nodes = [] + nodes = {} executor = MultiThreadedExecutor(num_threads=len(robot_names)) for robot_name in robot_names: node = SimulationTestNode("test_simulation", namespace=robot_name) node.create_test_subscribers_and_publishers() - nodes.append(node) + nodes[robot_name] = node executor.add_node(node) ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) ros_spin_thread.start() - for node in nodes: + for robot_name in robot_names: + node = nodes[robot_name] # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/mecanum_drive_controller.yaml + # rosbot_controller/config/diff_drive_controller.yaml node.set_destination_speed(0.9, 0.0, 0.0) - for node in nodes: - assert node.vel_stabilization_time_event.wait(timeout=60.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting" - f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." + for robot_name in robot_names: + node = nodes[robot_name] + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name}: does not move properly in x direction. Check" + f" rosbot_base_controller! Twist: {node.twist}" + ) + assert node.ekf_odom_flag, ( + f"{robot_name}: does not move properly in x direction. Check ekf_filter_node!" + f" Twist: {node.twist}" ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" - for node in nodes: + for robot_name in robot_names: + node = nodes[robot_name] node.set_destination_speed(0.0, 0.0, 3.0) - for node in nodes: - assert node.vel_stabilization_time_event.wait(timeout=60.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting" - f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." + for robot_name in robot_names: + node = nodes[robot_name] + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name} does not rotate properly. Check rosbot_base_controller! Twist:" + f" {node.twist}" ) - assert ( - node.controller_odom_flag - ), f"{robot_name} does not rotate properly. Check rosbot_base_controller!" assert ( node.ekf_odom_flag - ), f"{robot_name} does not rotate properly. Check ekf_filter_node!" + ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.twist}" 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!" + assert flag, ( + f"{robot_name}:'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!" + assert flag, f"{robot_name}:'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!" + assert flag, f"{robot_name}:'s camera point cloud does not work properly!" node.destroy_node() diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py index f7467c7e..f760acdd 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py @@ -48,7 +48,7 @@ def generate_test_description(): f'world:={get_package_share_directory("husarion_office_gz")}' "/worlds/empty_with_plugins.sdf" ), - "robots:=robot1={y: 0.0}; robot2={y: 1.0}; robot3={y: 2.0}; robot4={y: 3.0}", + "robots:=robot1={y: -4.0}; robot2={y: 0.0}; robot3={y: 4.0};", "headless:=True", "mecanum:=True", ], @@ -70,81 +70,96 @@ def test_multirobot_mecanum_simulation(): robot_names = ["robot1", "robot2", "robot3"] rclpy.init() try: - nodes = [] + nodes = {} executor = MultiThreadedExecutor(num_threads=len(robot_names)) for robot_name in robot_names: node = SimulationTestNode("test_simulation", namespace=robot_name) node.create_test_subscribers_and_publishers() - nodes.append(node) + nodes[robot_name] = node executor.add_node(node) ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) ros_spin_thread.start() - for node in nodes: + for robot_name in robot_names: + node = nodes[robot_name] # 0.9 m/s and 3.0 rad/s are controller's limits defined in # rosbot_controller/config/mecanum_drive_controller.yaml node.set_destination_speed(0.9, 0.0, 0.0) - for node in nodes: + for robot_name in robot_names: + node = nodes[robot_name] assert node.vel_stabilization_time_event.wait(timeout=120.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting" - f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name}: does not move properly in x direction. Check" + f" rosbot_base_controller! Twist: {node.twist}" + ) + assert node.ekf_odom_flag, ( + f"{robot_name}: does not move properly in x direction. Check ekf_filter_node!" + f" Twist: {node.twist}" ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" - for node in nodes: + for robot_name in robot_names: + node = nodes[robot_name] # 0.9 m/s and 3.0 rad/s are controller's limits defined in # rosbot_controller/config/mecanum_drive_controller.yaml node.set_destination_speed(0.0, 0.9, 0.0) - for node in nodes: + for robot_name in robot_names: + node = nodes[robot_name] assert node.vel_stabilization_time_event.wait(timeout=120.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting" - f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name} does not move properly in y direction. Check" + f" rosbot_base_controller! Twist: {node.twist}" + ) + assert node.ekf_odom_flag, ( + f"{robot_name} does not move properly in y direction. Check ekf_filter_node!" + f" Twist: {node.twist}" ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in y direction. Check rosbot_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in y direction. Check ekf_filter_node!" - for node in nodes: + for robot_name in robot_names: + node = nodes[robot_name] node.set_destination_speed(0.0, 0.0, 3.0) - for node in nodes: + for robot_name in robot_names: + node = nodes[robot_name] assert node.vel_stabilization_time_event.wait(timeout=120.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting" - f" the target speed is: {(node.current_time - node.goal_received_time):.1f}." + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name} does not rotate properly. Check rosbot_base_controller! Twist:" + f" {node.twist}" ) - assert ( - node.controller_odom_flag - ), f"{robot_name} does not rotate properly. Check rosbot_base_controller!" assert ( node.ekf_odom_flag - ), f"{robot_name} does not rotate properly. Check ekf_filter_node!" + ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.twist}" 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!" + assert flag, ( + f"{robot_name}:'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!" + assert flag, f"{robot_name}:'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!" + assert flag, f"{robot_name}:'s camera point cloud does not work properly!" node.destroy_node() diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index a4a3a071..b50db1b6 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -49,6 +49,7 @@ def __init__(self, name="test_node", namespace=None): self.v_x = 0.0 self.v_y = 0.0 self.v_yaw = 0.0 + self.twist = None # Debug info self.controller_odom_flag = False self.ekf_odom_flag = False @@ -116,6 +117,7 @@ def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, def controller_callback(self, data: Odometry): self.get_logger().debug(f"Received twist from controller: {data.twist.twist}") self.controller_odom_flag = self.is_twist_ok(data.twist.twist) + self.twist = data.twist.twist def ekf_callback(self, data: Odometry): self.get_logger().debug(f"Received twist filtered: {data.twist.twist}") From e86947fef0566cc69f67affe24df35739ee463b4 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 12 Jan 2024 13:55:43 +0100 Subject: [PATCH 79/79] simplify test --- rosbot_bringup/launch/bringup.launch.py | 1 + .../test/test_diff_drive_controllers.py | 15 +-- .../test/test_mecanum_controllers.py | 15 +-- .../test/test_multirobot_controllers.py | 18 +--- .../test_namespaced_diff_drive_controllers.py | 15 +-- .../test_namespaced_mecanum_controllers.py | 15 +-- rosbot_controller/test/test_utils.py | 17 ++++ .../test/test_diff_drive_simulation.py | 47 +--------- rosbot_gazebo/test/test_mecanum_simulation.py | 59 +----------- .../test_multirobot_diff_drive_simulation.py | 58 +----------- .../test_multirobot_mecanum_simulation.py | 80 +--------------- .../test_namespaced_diff_drive_simulation.py | 42 +-------- .../test_namespaced_mecanum_simulation.py | 54 +---------- rosbot_gazebo/test/test_utils.py | 94 +++++++++++++++++++ 14 files changed, 141 insertions(+), 389 deletions(-) diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py index b77d3137..4785a804 100644 --- a/rosbot_bringup/launch/bringup.launch.py +++ b/rosbot_bringup/launch/bringup.launch.py @@ -89,6 +89,7 @@ def generate_launch_description(): output="screen", parameters=[ekf_config], remappings=[ + ("/diagnostics", "diagnostics"), ("/tf", "tf"), ("/tf_static", "tf_static"), ], diff --git a/rosbot_controller/test/test_diff_drive_controllers.py b/rosbot_controller/test/test_diff_drive_controllers.py index 3b69459c..9a9aa6fb 100644 --- a/rosbot_controller/test/test_diff_drive_controllers.py +++ b/rosbot_controller/test/test_diff_drive_controllers.py @@ -23,7 +23,7 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode +from test_utils import ControllersTestNode, controller_readings_test @launch_pytest.fixture @@ -85,17 +85,6 @@ def test_controllers_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" + controller_readings_test(node) finally: rclpy.shutdown() diff --git a/rosbot_controller/test/test_mecanum_controllers.py b/rosbot_controller/test/test_mecanum_controllers.py index 5f2607e3..9b8aa53c 100644 --- a/rosbot_controller/test/test_mecanum_controllers.py +++ b/rosbot_controller/test/test_mecanum_controllers.py @@ -23,7 +23,7 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode +from test_utils import ControllersTestNode, controller_readings_test @launch_pytest.fixture @@ -85,17 +85,6 @@ def test_controllers_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" + controller_readings_test(node) finally: rclpy.shutdown() diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py index 6e7963a7..0b2e9084 100644 --- a/rosbot_controller/test/test_multirobot_controllers.py +++ b/rosbot_controller/test/test_multirobot_controllers.py @@ -23,7 +23,7 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode +from test_utils import ControllersTestNode, controller_readings_test robot_names = ["robot1", "robot2", "robot3"] @@ -66,19 +66,7 @@ def test_multirobot_controllers_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=20.0) - assert msgs_received_flag, ( - f"Expected JointStates message but it was not received. Check {robot_name}/" - "joint_state_broadcaster!" - ) - msgs_received_flag = node.odom_msg_event.wait(timeout=20.0) - assert msgs_received_flag, ( - f"Expected Odom message but it was not received. Check {robot_name}/" - "rosbot_base_controller!" - ) - msgs_received_flag = node.imu_msg_event.wait(timeout=20.0) - assert ( - msgs_received_flag - ), f"Expected Imu message but it was not received. Check {robot_name}/imu_broadcaster!" + + controller_readings_test(node, robot_name) finally: rclpy.shutdown() diff --git a/rosbot_controller/test/test_namespaced_diff_drive_controllers.py b/rosbot_controller/test/test_namespaced_diff_drive_controllers.py index fdab2de6..7371ab7d 100644 --- a/rosbot_controller/test/test_namespaced_diff_drive_controllers.py +++ b/rosbot_controller/test/test_namespaced_diff_drive_controllers.py @@ -23,7 +23,7 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode +from test_utils import ControllersTestNode, controller_readings_test @launch_pytest.fixture @@ -86,17 +86,6 @@ def test_namespaced_controllers_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" + controller_readings_test(node) finally: rclpy.shutdown() diff --git a/rosbot_controller/test/test_namespaced_mecanum_controllers.py b/rosbot_controller/test/test_namespaced_mecanum_controllers.py index e2321fd0..d1ccd623 100644 --- a/rosbot_controller/test/test_namespaced_mecanum_controllers.py +++ b/rosbot_controller/test/test_namespaced_mecanum_controllers.py @@ -23,7 +23,7 @@ from launch.actions import IncludeLaunchDescription from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from test_utils import ControllersTestNode +from test_utils import ControllersTestNode, controller_readings_test @launch_pytest.fixture @@ -86,17 +86,6 @@ def test_namespaced_controllers_startup_success(): node.start_publishing_fake_hardware() node.start_node_thread() - msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected JointStates message but it was not received. Check joint_state_broadcaster!" - msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Odom message but it was not received. Check rosbot_base_controller!" - msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) - assert ( - msgs_received_flag - ), "Expected Imu message but it was not received. Check imu_broadcaster!" + controller_readings_test(node) finally: rclpy.shutdown() diff --git a/rosbot_controller/test/test_utils.py b/rosbot_controller/test/test_utils.py index 831eadad..f5fdd34f 100644 --- a/rosbot_controller/test/test_utils.py +++ b/rosbot_controller/test/test_utils.py @@ -91,3 +91,20 @@ def publish_fake_hardware_messages(self): self.imu_publisher.publish(imu_msg) self.joint_states_publisher.publish(joint_state_msg) + + +def controller_readings_test(node, robot_name="ROSbot"): + msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0) + assert msgs_received_flag, ( + f"{robot_name}: expected JointStates message but it was not received. Check" + " joint_state_broadcaster!" + ) + msgs_received_flag = node.odom_msg_event.wait(timeout=10.0) + assert msgs_received_flag, ( + f"{robot_name}: expected Odom message but it was not received. Check" + " rosbot_base_controller!" + ) + msgs_received_flag = node.imu_msg_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), f"{robot_name}: expected Imu message but it was not received. Check imu_broadcaster!" diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py index bcbc83ff..130b1fec 100644 --- a/rosbot_gazebo/test/test_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_diff_drive_simulation.py @@ -27,7 +27,7 @@ from launch_testing.actions import ReadyToTest from launch_testing.util import KeepAliveProc -from test_utils import SimulationTestNode +from test_utils import SimulationTestNode, tf_test, diff_test from test_ign_kill_utils import kill_ign_linux_processes @@ -78,49 +78,8 @@ def test_diff_drive_simulation(): node.create_test_subscribers_and_publishers() node.start_node_thread() - flag = node.odom_tf_event.wait(timeout=60.0) - assert ( - flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" - - # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/diff_drive_controller.yaml - node.set_destination_speed(0.9, 0.0, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" - - node.set_destination_speed(0.0, 0.0, 3.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - 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!" - - 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!" + tf_test(node) + diff_test(node) finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py index 9a20adb6..eed30286 100644 --- a/rosbot_gazebo/test/test_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_mecanum_simulation.py @@ -27,7 +27,7 @@ from launch_testing.actions import ReadyToTest from launch_testing.util import KeepAliveProc -from test_utils import SimulationTestNode +from test_utils import SimulationTestNode, tf_test, mecanum_test from test_ign_kill_utils import kill_ign_linux_processes @@ -79,61 +79,8 @@ def test_mecanum_simulation(): node.create_test_subscribers_and_publishers() node.start_node_thread() - flag = node.odom_tf_event.wait(timeout=60.0) - assert ( - flag - ), "Expected odom to base_link tf but it was not received. Check robot_localization!" - - # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/mecanum_drive_controller.yaml - node.set_destination_speed(0.9, 0.0, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" - - node.set_destination_speed(0.0, 0.9, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in y direction. Check rosbot_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in y direction. Check ekf_filter_node!" - - node.set_destination_speed(0.0, 0.0, 3.0) - assert node.vel_stabilization_time_event.wait(timeout=20.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - 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!" - - 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!" + tf_test(node) + mecanum_test(node) finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py index ecfa7b29..6894e8ab 100644 --- a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py @@ -27,7 +27,8 @@ from rclpy.executors import MultiThreadedExecutor from threading import Thread -from test_utils import SimulationTestNode +from test_utils import SimulationTestNode, diff_test + from test_ign_kill_utils import kill_ign_linux_processes @@ -83,60 +84,7 @@ def test_multirobot_simulation(): for robot_name in robot_names: node = nodes[robot_name] - # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/diff_drive_controller.yaml - node.set_destination_speed(0.9, 0.0, 0.0) - - for robot_name in robot_names: - node = nodes[robot_name] - assert node.vel_stabilization_time_event.wait(timeout=120.0), ( - f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" - " since setting the target speed is:" - f" {(node.current_time - node.goal_received_time):.1f}." - ) - assert node.controller_odom_flag, ( - f"{robot_name}: does not move properly in x direction. Check" - f" rosbot_base_controller! Twist: {node.twist}" - ) - assert node.ekf_odom_flag, ( - f"{robot_name}: does not move properly in x direction. Check ekf_filter_node!" - f" Twist: {node.twist}" - ) - - for robot_name in robot_names: - node = nodes[robot_name] - node.set_destination_speed(0.0, 0.0, 3.0) - - for robot_name in robot_names: - node = nodes[robot_name] - assert node.vel_stabilization_time_event.wait(timeout=120.0), ( - f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" - " since setting the target speed is:" - f" {(node.current_time - node.goal_received_time):.1f}." - ) - assert node.controller_odom_flag, ( - f"{robot_name} does not rotate properly. Check rosbot_base_controller! Twist:" - f" {node.twist}" - ) - assert ( - node.ekf_odom_flag - ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.twist}" - - 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"{robot_name}:'s range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work" - " properly!" - ) - - flag = node.camera_color_event.wait(timeout=20.0) - assert flag, f"{robot_name}:'s camera color image does not work properly!" - - flag = node.camera_points_event.wait(timeout=20.0) - assert flag, f"{robot_name}:'s camera point cloud does not work properly!" + diff_test(node, robot_name) node.destroy_node() diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py index f760acdd..65d76431 100644 --- a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py @@ -27,7 +27,7 @@ from rclpy.executors import MultiThreadedExecutor from threading import Thread -from test_utils import SimulationTestNode +from test_utils import SimulationTestNode, mecanum_test from test_ign_kill_utils import kill_ign_linux_processes @@ -82,84 +82,10 @@ def test_multirobot_mecanum_simulation(): ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,)) ros_spin_thread.start() + # Speed test for robot_name in robot_names: node = nodes[robot_name] - # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/mecanum_drive_controller.yaml - node.set_destination_speed(0.9, 0.0, 0.0) - - for robot_name in robot_names: - node = nodes[robot_name] - assert node.vel_stabilization_time_event.wait(timeout=120.0), ( - f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" - " since setting the target speed is:" - f" {(node.current_time - node.goal_received_time):.1f}." - ) - assert node.controller_odom_flag, ( - f"{robot_name}: does not move properly in x direction. Check" - f" rosbot_base_controller! Twist: {node.twist}" - ) - assert node.ekf_odom_flag, ( - f"{robot_name}: does not move properly in x direction. Check ekf_filter_node!" - f" Twist: {node.twist}" - ) - - for robot_name in robot_names: - node = nodes[robot_name] - # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/mecanum_drive_controller.yaml - node.set_destination_speed(0.0, 0.9, 0.0) - - for robot_name in robot_names: - node = nodes[robot_name] - assert node.vel_stabilization_time_event.wait(timeout=120.0), ( - f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" - " since setting the target speed is:" - f" {(node.current_time - node.goal_received_time):.1f}." - ) - assert node.controller_odom_flag, ( - f"{robot_name} does not move properly in y direction. Check" - f" rosbot_base_controller! Twist: {node.twist}" - ) - assert node.ekf_odom_flag, ( - f"{robot_name} does not move properly in y direction. Check ekf_filter_node!" - f" Twist: {node.twist}" - ) - - for robot_name in robot_names: - node = nodes[robot_name] - node.set_destination_speed(0.0, 0.0, 3.0) - - for robot_name in robot_names: - node = nodes[robot_name] - assert node.vel_stabilization_time_event.wait(timeout=120.0), ( - f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" - " since setting the target speed is:" - f" {(node.current_time - node.goal_received_time):.1f}." - ) - assert node.controller_odom_flag, ( - f"{robot_name} does not rotate properly. Check rosbot_base_controller! Twist:" - f" {node.twist}" - ) - assert ( - node.ekf_odom_flag - ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.twist}" - - 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"{robot_name}:'s range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work" - " properly!" - ) - - flag = node.camera_color_event.wait(timeout=20.0) - assert flag, f"{robot_name}:'s camera color image does not work properly!" - - flag = node.camera_points_event.wait(timeout=20.0) - assert flag, f"{robot_name}:'s camera point cloud does not work properly!" + mecanum_test(node, robot_name) node.destroy_node() diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py index f68440cc..e4a15ff0 100644 --- a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py @@ -27,7 +27,7 @@ from launch_testing.actions import ReadyToTest from launch_testing.util import KeepAliveProc -from test_utils import SimulationTestNode +from test_utils import SimulationTestNode, tf_test, diff_test from test_ign_kill_utils import kill_ign_linux_processes @@ -79,44 +79,8 @@ def test_namespaced_diff_drive_simulation(): node.create_test_subscribers_and_publishers() node.start_node_thread() - # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/diff_drive_controller.yaml - node.set_destination_speed(0.9, 0.0, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=40.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" - - node.set_destination_speed(0.0, 0.0, 3.0) - assert node.vel_stabilization_time_event.wait(timeout=40.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - 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!" - - 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!" + tf_test(node) + diff_test(node) finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py index a8ea08eb..138fb47d 100644 --- a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py +++ b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py @@ -27,7 +27,7 @@ from launch_testing.actions import ReadyToTest from launch_testing.util import KeepAliveProc -from test_utils import SimulationTestNode +from test_utils import SimulationTestNode, tf_test, mecanum_test from test_ign_kill_utils import kill_ign_linux_processes @@ -80,56 +80,8 @@ def test_namespaced_mecanum_simulation(): node.create_test_subscribers_and_publishers() node.start_node_thread() - # 0.9 m/s and 3.0 rad/s are controller's limits defined in - # rosbot_controller/config/mecanum_drive_controller.yaml - node.set_destination_speed(0.9, 0.0, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=40.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in x direction. Check rosbot_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in x direction. Check ekf_filter_node!" - - node.set_destination_speed(0.0, 0.9, 0.0) - assert node.vel_stabilization_time_event.wait(timeout=40.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - assert ( - node.controller_odom_flag - ), "ROSbot does not move properly in y direction. Check rosbot_base_controller!" - assert ( - node.ekf_odom_flag - ), "ROSbot does not move properly in y direction. Check ekf_filter_node!" - - node.set_destination_speed(0.0, 0.0, 3.0) - assert node.vel_stabilization_time_event.wait(timeout=40.0), ( - "The simulation is running slowly or has crashed! The time elapsed since setting the" - f" target speed is: {(node.current_time - node.goal_received_time):.1f}." - ) - 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!" - - 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!" + tf_test(node) + mecanum_test(node) finally: # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py index b50db1b6..8457dfc5 100644 --- a/rosbot_gazebo/test/test_utils.py +++ b/rosbot_gazebo/test/test_utils.py @@ -159,3 +159,97 @@ def publish_cmd_vel_messages(self): self.get_logger().debug(f"Publishing twist: {twist_msg}") self.cmd_vel_publisher.publish(twist_msg) + + +def x_speed_test(node, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"): + node.set_destination_speed(v_x, v_y, v_yaw) + + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name}: does not move properly in x direction. Check" + f" rosbot_base_controller! Twist: {node.twist}" + ) + assert node.ekf_odom_flag, ( + f"{robot_name}: does not move properly in x direction. Check ekf_filter_node!" + f" Twist: {node.twist}" + ) + + +def y_speed_test(node, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"): + node.set_destination_speed(v_x, v_y, v_yaw) + + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert node.controller_odom_flag, ( + f"{robot_name} does not move properly in y direction. Check" + f" rosbot_base_controller! Twist: {node.twist}" + ) + assert node.ekf_odom_flag, ( + f"{robot_name} does not move properly in y direction. Check ekf_filter_node!" + f" Twist: {node.twist}" + ) + + +def yaw_speed_test(node, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"): + node.set_destination_speed(v_x, v_y, v_yaw) + + assert node.vel_stabilization_time_event.wait(timeout=120.0), ( + f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed" + " since setting the target speed is:" + f" {(node.current_time - node.goal_received_time):.1f}." + ) + assert ( + node.controller_odom_flag + ), f"{robot_name} does not rotate properly. Check rosbot_base_controller! Twist: {node.twist}" + assert ( + node.ekf_odom_flag + ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.twist}" + + +def sensors_readings_test(node, robot_name="ROSbot"): + 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"{robot_name}'s range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!" + + flag = node.camera_color_event.wait(timeout=20.0) + assert flag, f"{robot_name}'s camera color image does not work properly!" + + flag = node.camera_points_event.wait(timeout=20.0) + assert flag, f"{robot_name}'s camera point cloud does not work properly!" + + +def tf_test(node, robot_name="ROSbot"): + flag = node.odom_tf_event.wait(timeout=20.0) + assert flag, ( + f"{robot_name}: expected odom to base_link tf but it was not received. Check" + " robot_localization!" + ) + + +def diff_test(node, robot_name="ROSbot"): + sensors_readings_test(node, robot_name) + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/mecanum_drive_controller.yaml + x_speed_test(node, v_x=0.9, robot_name=robot_name) + yaw_speed_test(node, v_yaw=3.0, robot_name=robot_name) + + +def mecanum_test(node, robot_name="ROSbot"): + sensors_readings_test(node, robot_name) + # 0.9 m/s and 3.0 rad/s are controller's limits defined in + # rosbot_controller/config/mecanum_drive_controller.yaml + x_speed_test(node, v_x=0.9, robot_name=robot_name) + y_speed_test(node, v_y=0.9, robot_name=robot_name) + yaw_speed_test(node, v_yaw=3.0, robot_name=robot_name)