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

* Fixed instances where old m2m api was used * precommit
Robotic-Decision-Making-Lab · Sep 25, 2024 · 2d17d70 · 2d17d70
1 parent 608e987
commit 2d17d70
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,