Fixed instances where mobile_to_maritime API wasn't updated (#320) (#323

) * Fixed instances where old m2m api was used * precommit (cherry picked from commit 2d17d70) Co-authored-by: Evan Palmer <[email protected]>
Robotic-Decision-Making-Lab · Sep 25, 2024 · d7f5d87 · d7f5d87
1 parent 37c9a6d
commit d7f5d87
Show file tree

Hide file tree

Showing 6 changed files with 72 additions and 33 deletions.
diff --git a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml
@@ -122,4 +122,4 @@ launch:
       file: $(find-pkg-share blue_bringup)/launch/$(var model_name)/thrusters.launch.yaml
 
   - include:
-      file: $(find-pkg-share mobile_to_maritime)/launch/tf.launch.yaml
+      file: $(find-pkg-share message_transforms)/launch/tf.launch.yaml
diff --git a/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml b/blue_bringup/launch/bluerov2_heavy/bluerov2_heavy.launch.yaml
@@ -122,4 +122,4 @@ launch:
       file: $(find-pkg-share blue_bringup)/launch/$(var model_name)/thrusters.launch.yaml
 
   - include:
-      file: $(find-pkg-share mobile_to_maritime)/launch/tf.launch.yaml
+      file: $(find-pkg-share message_transforms)/launch/tf.launch.yaml
diff --git a/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach.launch.yaml b/blue_bringup/launch/bluerov2_heavy_reach/bluerov2_heavy_reach.launch.yaml
@@ -122,4 +122,4 @@ launch:
       file: $(find-pkg-share blue_bringup)/launch/$(var model_name)/thrusters.launch.yaml
 
   - include:
-      file: $(find-pkg-share mobile_to_maritime)/launch/tf.launch.yaml
+      file: $(find-pkg-share message_transforms)/launch/tf.launch.yaml
diff --git a/blue_demos/control_integration/config/transforms.yaml b/blue_demos/control_integration/config/transforms.yaml
@@ -0,0 +1,11 @@
+/control_integration/message_transforms:
+  ros__parameters:
+
+    incoming_topics:
+      - /mavros/local_position/velocity_body
+
+    transforms:
+      /mavros/local_position/velocity_body:
+        outgoing_topic: /integral_sliding_mode_controller/system_state
+        message_type: geometry_msgs/msg/TwistStamped
+        frame_id: base_link_fsd
diff --git a/blue_demos/control_integration/launch/bluerov2_controllers.launch.py b/blue_demos/control_integration/launch/bluerov2_controllers.launch.py
@@ -19,9 +19,14 @@
 # THE SOFTWARE.
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+)
 from launch.event_handlers import OnProcessExit
-from launch.substitutions import PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution, TextSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
@@ -48,18 +53,27 @@ def generate_launch_description() -> LaunchDescription:
     ]
 
     # The ISMC expects state information to be provided in the FSD frame
-    mobile_to_maritime_velocity_state = Node(
-        package="mobile_to_maritime",
-        executable="mobile_twist_stamped_to_maritime_twist",
-        name="velocity_state_transform",
-        parameters=[
-            {
-                "in_topic": "/mavros/local_position/velocity_body",
-                "out_topic": "/integral_sliding_mode_controller/system_state",
-                "qos_reliability": "best_effort",
-                "qos_durability": "volatile",
-            }
-        ],
+    message_transformer = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("message_transforms"),
+                    "launch",
+                    "message_transforms.launch.py",
+                ]
+            )
+        ),
+        launch_arguments={
+            "parameters_file": PathJoinSubstitution(
+                [
+                    FindPackageShare("blue_demos"),
+                    "control_integration",
+                    "config",
+                    "transforms.yaml",
+                ]
+            ),
+            "ns": TextSubstitution(text="control_integration"),
+        }.items(),
     )
 
     controller_manager = Node(
@@ -151,7 +165,7 @@ def generate_launch_description() -> LaunchDescription:
     return LaunchDescription(
         [
             *args,
-            mobile_to_maritime_velocity_state,
+            message_transformer,
             controller_manager,
             *delay_thruster_spawners,
             delay_tam_controller_spawner_after_thruster_controller_spawners,

diff --git a/blue_demos/control_integration/launch/bluerov2_heavy_controllers.launch.py b/blue_demos/control_integration/launch/bluerov2_heavy_controllers.launch.py
@@ -19,9 +19,14 @@
 # THE SOFTWARE.
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+)
 from launch.event_handlers import OnProcessExit
-from launch.substitutions import PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution, TextSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
@@ -47,18 +52,27 @@ def generate_launch_description() -> LaunchDescription:
     ]
 
     # The ISMC expects state information to be provided in the FSD frame
-    mobile_to_maritime_velocity_state = Node(
-        package="mobile_to_maritime",
-        executable="mobile_twist_stamped_to_maritime_twist",
-        name="velocity_state_transform",
-        parameters=[
-            {
-                "in_topic": "/mavros/local_position/velocity_body",
-                "out_topic": "/integral_sliding_mode_controller/system_state",
-                "qos_reliability": "best_effort",
-                "qos_durability": "volatile",
-            }
-        ],
+    message_transformer = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("message_transforms"),
+                    "launch",
+                    "message_transforms.launch.py",
+                ]
+            )
+        ),
+        launch_arguments={
+            "parameters_file": PathJoinSubstitution(
+                [
+                    FindPackageShare("blue_demos"),
+                    "control_integration",
+                    "config",
+                    "transforms.yaml",
+                ]
+            ),
+            "ns": TextSubstitution(text="control_integration"),
+        }.items(),
     )
 
     controller_manager = Node(
@@ -150,7 +164,7 @@ def generate_launch_description() -> LaunchDescription:
     return LaunchDescription(
         [
             *args,
-            mobile_to_maritime_velocity_state,
+            message_transformer,
             controller_manager,
             *delay_thruster_spawners,
             delay_tam_controller_spawner_after_thruster_controller_spawners,