diff --git a/blue_bringup/launch/bluerov2/bluerov2.launch.yaml b/blue_bringup/launch/bluerov2/bluerov2.launch.yaml index f8aa9b50..7781fd27 100644 --- 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 index 368be9ee..8d8f8de7 100644 --- 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 index 6af54b05..1464a0a8 100644 --- 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 new file mode 100644 index 00000000..6f562a34 --- /dev/null +++ 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 index b4e6d059..e37dc412 100644 --- 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 index 5e367672..3f6ab9a9 100644 --- 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,