Jakub suggestions

husarion · Jan 30, 2024 · 066ae9b · 066ae9b
1 parent 1fb0a56
commit 066ae9b
Show file tree

Hide file tree

Showing 13 changed files with 108 additions and 110 deletions.
diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml
@@ -49,10 +49,6 @@ jobs:
           cp -r src/ros2_controllers/diff_drive_controller src/
           cp -r src/ros2_controllers/imu_sensor_broadcaster src/
           rm -rf src/ros2_controllers
-          cp -r src/geometry2/tf2_ros_py src/
-          rm -rf src/geometry2/
-          rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos
-          rm -rf src/gazebosim/gz_ros2_control/gz_ros2_control_tests
 
       - uses: ros-industrial/industrial_ci@master
         env:

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -29,7 +29,7 @@ repos:
     rev: 0.2.3
     hooks:
       - id: yamlfmt
-        files: ^(?!.*compose).*$
+        files: ^(?!.*compose)(?!.*ekf\.yaml$).*$
         args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '100']
 
   - repo: https://github.com/psf/black

diff --git a/rosbot_xl/rosbot_xl_hardware.repos b/rosbot_xl/rosbot_xl_hardware.repos
@@ -6,7 +6,7 @@ repositories:
   ros_components_description:
     type: git
     url: https://github.com/husarion/ros_components_description.git
-    version: gazebo-ignition-rosbot-xl-namespaces
+    version: ros2
   rosbot_controllers:
     type: git
     url: https://github.com/husarion/rosbot_controllers
@@ -15,7 +15,3 @@ repositories:
     type: git
     url: https://github.com/delihus/ros2_controllers
     version: humble
-  geometry2:
-    type: git
-    url: https://github.com/delihus/geometry2
-    version: rolling
diff --git a/rosbot_xl/rosbot_xl_simulation.repos b/rosbot_xl/rosbot_xl_simulation.repos
@@ -1,9 +1,4 @@
 repositories:
-  gazebosim/gz_ros2_control:
-    type: git
-    url: https://github.com/ros-controls/gz_ros2_control
-    version: humble
-
   husarion/husarion_office_gz:
     type: git
     url: https://github.com/husarion/husarion_office_gz

diff --git a/rosbot_xl_bringup/config/ekf.yaml b/rosbot_xl_bringup/config/ekf.yaml
@@ -1,57 +1,64 @@
----
 # Ref: http://docs.ros.org/en/noetic/api/robot_localization/html/state_estimation_nodes.html
 
 ## ekf config file ###
 /**/ekf_filter_node:
-  ros__parameters:
-    frequency: 25.0
-    sensor_timeout: 0.05
-    two_d_mode: true
-
-    transform_time_offset: 0.0
-    transform_timeout: 0.05
-
-    map_frame: map
-    odom_frame: odom
-    base_link_frame: base_link
-    world_frame: odom
-    publish_tf: true
-    publish_acceleration: false
-
-    odom0: rosbot_xl_base_controller/odom
-    odom0_config: [false, false, false, false, false, false, true, true, false, false, false, true, false,
-      false, false]                        # ddX , ddY , ddZ
-
-    odom0_queue_size: 2
-    odom0_nodelay: true
-    odom0_differential: false
-    odom0_relative: true
-
-    imu0: imu_broadcaster/imu
-    imu0_config: [false, false, false, false, false, true, false, false, false, false, false, true, false,
-      false, false]                        # ddX , ddY , ddZ
-    imu0_queue_size: 5
-    imu0_nodelay: true
-    imu0_differential: false
-    imu0_relative: true
-    imu0_remove_gravitational_acceleration: true
-
-    reset_on_time_jump: false
-    predict_to_current_time: false
-    print_diagnostics: false
+    ros__parameters:
+        frequency: 20.0
+        sensor_timeout: 0.05
+        two_d_mode: true
+
+        transform_time_offset: 0.0
+        transform_timeout: 0.05
+
+        map_frame: map
+        odom_frame: odom
+        base_link_frame: base_link
+        world_frame: odom
+        publish_tf: true
+        publish_acceleration: false
+
+        odom0: rosbot_xl_base_controller/odom
+        odom0_config: [false, false, false, # X , Y , Z
+                      false, false, false, # roll , pitch ,yaw
+                      true, true, false, # dX , dY , dZ
+                      false, false, true, # droll , dpitch ,dyaw
+                      false, false, false] # ddX , ddY , ddZ
+
+        odom0_queue_size: 2
+        odom0_nodelay: true
+        odom0_differential: false
+        odom0_relative: true
+
+        imu0: imu_broadcaster/imu
+        imu0_config: [false, false, false, # X , Y , Z
+                      false, false, true, # roll , pitch ,yaw
+                      false, false, false, # dX , dY , dZ
+                      false, false, true, # droll , dpitch ,dyaw
+                      false, false, false] # ddX , ddY , ddZ
+        imu0_queue_size: 5
+        imu0_nodelay: true
+        imu0_differential: false
+        imu0_relative: true
+        imu0_remove_gravitational_acceleration: true
+
+        reset_on_time_jump: false
+        predict_to_current_time: false
+        print_diagnostics: false
 
         # Selected values experimentally so as to ensure relatively fast convergence (values should be about 10x higher than the sensor variance values)
-    dynamic_process_noise_covariance: true
-    process_noise_covariance: [5e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-      0.0, 0.0, 5e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5e-3,
-      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 3e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-      3e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4e-2, 0.0, 0.0,
-      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4e-2, 0.0, 0.0, 0.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4e-2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-      0.0, 0.0, 0.0, 0.0, 1e-5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-      0.0, 1e-5]
+        dynamic_process_noise_covariance: true
+        process_noise_covariance: [5e-3,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                    0.0,   5e-3,  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                    0.0,   0.0,   5e-3,  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                    0.0,   0.0,   0.0,   1e-3,  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                    0.0,   0.0,   0.0,   0.0,   1e-3,  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                    0.0,   0.0,   0.0,   0.0,   0.0,   1e-3,  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   3e-3,  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   3e-3,  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   3e-3,  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+                                    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   4e-2,  0.0,   0.0,   0.0,   0.0,   0.0,
+                                    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   4e-2,  0.0,   0.0,   0.0,   0.0,
+                                    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   4e-2,  0.0,   0.0,   0.0,
+                                    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   1e-5,  0.0,   0.0,
+                                    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   1e-5,  0.0,
+                                    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   1e-5]
diff --git a/rosbot_xl_bringup/test/test_multirobot_ekf_and_scan.py b/rosbot_xl_bringup/test/test_multirobot_ekf_and_scan.py
@@ -65,17 +65,17 @@ def test_namespaced_bringup_startup_success():
         nodes = {}
         executor = SingleThreadedExecutor()
 
-        for node_name in robot_names:
-            node = BringupTestNode("test_bringup", namespace=node_name)
+        for node_namespace in robot_names:
+            node = BringupTestNode("test_bringup", namespace=node_namespace)
             node.start_publishing_fake_hardware()
-            nodes[node_name] = node
+            nodes[node_namespace] = node
             executor.add_node(node)
 
         Thread(target=lambda executor: executor.spin(), args=(executor,)).start()
 
-        for node_name in robot_names:
-            node = nodes[node_name]
-            ekf_and_scan_test(node, node_name)
+        for node_namespace in robot_names:
+            node = nodes[node_namespace]
+            ekf_and_scan_test(node, node_namespace)
             executor.remove_node(node)
             node.destroy_node()
 

diff --git a/rosbot_xl_gazebo/test/test_diff_drive_simulation.py b/rosbot_xl_gazebo/test/test_diff_drive_simulation.py
@@ -26,7 +26,7 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_testing.actions import ReadyToTest
 from launch_testing.util import KeepAliveProc
-from test_utils import SimulationTest, diff_test
+from test_utils import SimulationTestNode, diff_test
 from test_ign_kill_utils import kill_ign_linux_processes
 from threading import Thread
 
@@ -71,7 +71,7 @@ def generate_test_description():
 def test_diff_drive_simulation():
     rclpy.init()
     try:
-        node = SimulationTest("test_diff_drive_simulation")
+        node = SimulationTestNode("test_diff_drive_simulation")
         Thread(target=lambda node: rclpy.spin(node), args=(node,)).start()
 
         diff_test(node)

diff --git a/rosbot_xl_gazebo/test/test_mecanum_simulation.py b/rosbot_xl_gazebo/test/test_mecanum_simulation.py
@@ -26,7 +26,7 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_testing.actions import ReadyToTest
 from launch_testing.util import KeepAliveProc
-from test_utils import SimulationTest, mecanum_test
+from test_utils import SimulationTestNode, mecanum_test
 from test_ign_kill_utils import kill_ign_linux_processes
 from threading import Thread
 
@@ -72,7 +72,7 @@ def generate_test_description():
 def test_mecanum_simulation():
     rclpy.init()
     try:
-        node = SimulationTest("test_mecanum_simulation")
+        node = SimulationTestNode("test_mecanum_simulation")
         Thread(target=lambda node: rclpy.spin(node), args=(node,)).start()
 
         mecanum_test(node)

diff --git a/rosbot_xl_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_xl_gazebo/test/test_multirobot_diff_drive_simulation.py
@@ -24,7 +24,7 @@
 from launch_testing.actions import ReadyToTest
 from launch_testing.util import KeepAliveProc
 from rclpy.executors import SingleThreadedExecutor
-from test_utils import SimulationTest, diff_test
+from test_utils import SimulationTestNode, diff_test
 from test_ign_kill_utils import kill_ign_linux_processes
 from threading import Thread
 
@@ -67,16 +67,18 @@ def test_multirobot_diff_drive_simulation():
         nodes = {}
         executor = SingleThreadedExecutor()
 
-        for node_name in robots:
-            node = SimulationTest("test_multirobot_diff_drive_simulation", namespace=node_name)
-            nodes[node_name] = node
+        for node_namespace in robots:
+            node = SimulationTestNode(
+                "test_multirobot_diff_drive_simulation", namespace=node_namespace
+            )
+            nodes[node_namespace] = node
             executor.add_node(node)
 
         Thread(target=lambda executor: executor.spin(), args=(executor,)).start()
 
-        for node_name in robots:
-            node = nodes[node_name]
-            diff_test(node, node_name)
+        for node_namespace in robots:
+            node = nodes[node_namespace]
+            diff_test(node, node_namespace)
             executor.remove_node(node)
             node.destroy_node()
 

diff --git a/rosbot_xl_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_xl_gazebo/test/test_multirobot_mecanum_simulation.py
@@ -24,7 +24,7 @@
 from launch_testing.actions import ReadyToTest
 from launch_testing.util import KeepAliveProc
 from rclpy.executors import SingleThreadedExecutor
-from test_utils import SimulationTest, mecanum_test
+from test_utils import SimulationTestNode, mecanum_test
 from test_ign_kill_utils import kill_ign_linux_processes
 from threading import Thread
 
@@ -68,16 +68,18 @@ def test_multirobot_mecanum_simulation():
         nodes = {}
         executor = SingleThreadedExecutor()
 
-        for node_name in robots:
-            node = SimulationTest("test_multirobot_mecanum_simulation", namespace=node_name)
-            nodes[node_name] = node
+        for node_namespace in robots:
+            node = SimulationTestNode(
+                "test_multirobot_mecanum_simulation", namespace=node_namespace
+            )
+            nodes[node_namespace] = node
             executor.add_node(node)
 
         Thread(target=lambda executor: executor.spin(), args=(executor,)).start()
 
-        for node_name in robots:
-            node = nodes[node_name]
-            mecanum_test(node, node_name)
+        for node_namespace in robots:
+            node = nodes[node_namespace]
+            mecanum_test(node, node_namespace)
             executor.remove_node(node)
             node.destroy_node()
 

diff --git a/rosbot_xl_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_xl_gazebo/test/test_namespaced_diff_drive_simulation.py
@@ -26,7 +26,7 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_testing.actions import ReadyToTest
 from launch_testing.util import KeepAliveProc
-from test_utils import SimulationTest, diff_test
+from test_utils import SimulationTestNode, diff_test
 from test_ign_kill_utils import kill_ign_linux_processes
 from threading import Thread
 
@@ -46,7 +46,6 @@ def generate_test_description():
             )
         ),
         launch_arguments={
-            "mecanum": "True",
             "headless": "True",
             "world": PathJoinSubstitution(
                 [
@@ -73,7 +72,7 @@ def generate_test_description():
 def test_namespaced_diff_drive_simulation():
     rclpy.init()
     try:
-        node = SimulationTest("test_namespaced_diff_drive_simulation", namespace="rosbot_xl")
+        node = SimulationTestNode("test_namespaced_diff_drive_simulation", namespace="rosbot_xl")
         Thread(target=lambda node: rclpy.spin(node), args=(node,)).start()
 
         diff_test(node)

diff --git a/rosbot_xl_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_xl_gazebo/test/test_namespaced_mecanum_simulation.py
@@ -25,7 +25,7 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_testing.actions import ReadyToTest
 from launch_testing.util import KeepAliveProc
-from test_utils import SimulationTest, mecanum_test
+from test_utils import SimulationTestNode, mecanum_test
 from test_ign_kill_utils import kill_ign_linux_processes
 from threading import Thread
 
@@ -72,7 +72,7 @@ def generate_test_description():
 def test_namespaced_mecanum_simulation():
     rclpy.init()
     try:
-        node = SimulationTest("test_namespaced_mecanum_simulation", namespace="rosbot_xl")
+        node = SimulationTestNode("test_namespaced_mecanum_simulation", namespace="rosbot_xl")
         Thread(target=lambda node: rclpy.spin(node), args=(node,)).start()
 
         mecanum_test(node)