27 control system tester

.gitignore

@@ -18,6 +18,9 @@ msg/_*.py
 build_isolated/
 devel_isolated/
 
+src/control_system/rov_control_system_tester/output
+src/trajectory/rov_position_snapshot/output
+
 # Generated by dynamic reconfigure
 *.cfgc
 /cfg/cpp/

src/control_system/rov_control_system_tester/launch/mfsmc.launch.py

@@ -0,0 +1,135 @@
+from launch_ros.actions import Node
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, TimerAction
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+
+from ament_index_python import get_package_share_directory
+
+
+def generate_launch_description():
+
+    control_system_share = FindPackageShare('rov_control_system_tester')
+    passthrough_share = FindPackageShare('rov_passthrough_control')
+
+    default_config_path = PathJoinSubstitution([control_system_share, 'config', 'params.yaml'])
+    default_rviz_config_path = PathJoinSubstitution([control_system_share, 'rviz', 'tf_basic.rviz'])
+
+    # Define default configuration paths
+    default_config_path = PathJoinSubstitution([control_system_share, 'config', 'params.yaml'])
+
+    # Launch arguments
+    config_arg = DeclareLaunchArgument(
+        name="config",
+        default_value=default_config_path,
+        description="Path to the configuration file for the stonefish simulator"
+    )
+    rviz_config_arg = DeclareLaunchArgument(
+        name="rviz_config",
+        default_value=default_rviz_config_path,
+        description="Path to the RViz2 configuration file"
+    )
+
+    thruster_manager_node = Node(
+        package='rov_thruster_manager',
+        executable="thruster_manager",
+        parameters=[LaunchConfiguration('config')],
+        output="screen"
+    )
+
+    rov_state_publisher_node = Node(
+        package='rov_description',
+        executable='rov_state_publisher',
+        parameters=[LaunchConfiguration('config')]
+    )
+
+    traj_gen_node = Node(
+        package='traj_gen',
+        executable='min_snap_traj_generator',
+        parameters=[LaunchConfiguration('config')],
+        output='screen'
+    )
+
+    tf_traj_gen = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=["0", "0", "0", "0", "0", "0", "world_ned", "traj_gen"]
+    )
+
+    robot_localization_node = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node',
+        parameters=[LaunchConfiguration('config')],
+        output='screen'
+    )
+
+    robot_localization_node_ref = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_ref',
+        parameters=[LaunchConfiguration('config')],
+        output='screen'
+    )
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        arguments=['-d', LaunchConfiguration('rviz_config')],
+        output='screen'
+    )
+
+    mfsm_node = Node(
+        package='rov_mfsmc',
+        executable='rov_mfsmc_node',
+        parameters=[LaunchConfiguration('config')],
+        output='screen'
+    )
+
+    control_system_tester_node = Node(
+        package='rov_control_system_tester',
+        executable='control_system_tester',
+        parameters=[LaunchConfiguration('config')],
+        output='screen'
+    )
+
+    currents_node = Node(
+        package='rov_stonefish',
+        executable='enable_stonefish_currents'
+    )
+
+    launch_include = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(get_package_share_directory('stonefish_ros2') + \
+                                      '/launch/stonefish_simulator.launch.py'),
+        launch_arguments={
+            'simulation_data': get_package_share_directory('rov_stonefish') + '/data/',
+            'scenario_desc': get_package_share_directory('rov_stonefish') + '/scenarios/windturbine_bluerov2.scn',
+            'simulation_rate': '30.0',
+            'window_res_x': '1820',
+            'window_res_y': '980',
+            'rendering_quality': 'low',
+        }.items()
+    )
+
+    description_timer = TimerAction(period=1.0, actions=[rov_state_publisher_node])
+    rviz_timer = TimerAction(period=1.0, actions=[rviz_node])
+    stonefish_timer = TimerAction(period=0.0, actions=[launch_include])
+    currents_timer = TimerAction(period=6.0, actions=[currents_node])
+    traj_gen_timer = TimerAction(period=10.0, actions=[traj_gen_node, tf_traj_gen])
+
+    return LaunchDescription([
+        config_arg,
+        rviz_config_arg,
+        robot_localization_node,
+        robot_localization_node_ref,
+        description_timer,
+        rviz_timer,
+        stonefish_timer,
+        traj_gen_timer,
+        thruster_manager_node,
+        mfsm_node,
+        control_system_tester_node,
+        currents_timer
+    ])

src/control_system/rov_control_system_tester/launch/pid.launch.py

@@ -0,0 +1,135 @@
+from launch_ros.actions import Node
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, TimerAction
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+
+from ament_index_python import get_package_share_directory
+
+
+def generate_launch_description():
+
+    control_system_tester_share = FindPackageShare('rov_control_system_tester')
+    passthrough_share = FindPackageShare('rov_passthrough_control')
+
+    default_config_path = PathJoinSubstitution([control_system_tester_share, 'config', 'params.yaml'])
+    default_rviz_config_path = PathJoinSubstitution([control_system_tester_share, 'rviz', 'tf_basic.rviz'])
+
+    # Define default configuration paths
+    default_config_path = PathJoinSubstitution([control_system_tester_share, 'config', 'params.yaml'])
+
+    # Launch arguments
+    config_arg = DeclareLaunchArgument(
+        name="config",
+        default_value=default_config_path,
+        description="Path to the configuration file for the stonefish simulator"
+    )
+    rviz_config_arg = DeclareLaunchArgument(
+        name="rviz_config",
+        default_value=default_rviz_config_path,
+        description="Path to the RViz2 configuration file"
+    )
+
+    thruster_manager_node = Node(
+        package='rov_thruster_manager',
+        executable="thruster_manager",
+        parameters=[LaunchConfiguration('config')],
+        output="screen"
+    )
+
+    rov_state_publisher_node = Node(
+        package='rov_description',
+        executable='rov_state_publisher',
+        parameters=[LaunchConfiguration('config')]
+    )
+
+    traj_gen_node = Node(
+        package='traj_gen',
+        executable='min_snap_traj_generator',
+        parameters=[LaunchConfiguration('config')],
+        output='screen'
+    )
+
+    tf_traj_gen = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=["0", "0", "0", "0", "0", "0", "world_ned", "traj_gen"]
+    )
+
+    robot_localization_node = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_filter_node',
+        parameters=[LaunchConfiguration('config')],
+        output='screen'
+    )
+
+    robot_localization_node_ref = Node(
+        package='robot_localization',
+        executable='ekf_node',
+        name='ekf_ref',
+        parameters=[LaunchConfiguration('config')],
+        output='screen'
+    )
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        arguments=['-d', LaunchConfiguration('rviz_config')],
+        output='screen'
+    )
+
+    mfsm_node = Node(
+        package='rov_pid',
+        executable='rov_pid_node',
+        parameters=[LaunchConfiguration('config')],
+        output='screen'
+    )
+
+    control_system_tester_node = Node(
+        package='rov_control_system_tester',
+        executable='control_system_tester',
+        parameters=[LaunchConfiguration('config')],
+        output='screen'
+    )
+
+    currents_node = Node(
+        package='rov_stonefish',
+        executable='enable_stonefish_currents'
+    )
+
+    launch_include = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(get_package_share_directory('stonefish_ros2') + \
+                                      '/launch/stonefish_simulator.launch.py'),
+        launch_arguments={
+            'simulation_data': get_package_share_directory('rov_stonefish') + '/data/',
+            'scenario_desc': get_package_share_directory('rov_stonefish') + '/scenarios/windturbine_bluerov2.scn',
+            'simulation_rate': '30.0',
+            'window_res_x': '1820',
+            'window_res_y': '980',
+            'rendering_quality': 'low',
+        }.items()
+    )
+
+    description_timer = TimerAction(period=1.0, actions=[rov_state_publisher_node])
+    rviz_timer = TimerAction(period=1.0, actions=[rviz_node])
+    stonefish_timer = TimerAction(period=0.0, actions=[launch_include])
+    currents_timer = TimerAction(period=6.0, actions=[currents_node])
+    traj_gen_timer = TimerAction(period=10.0, actions=[traj_gen_node, tf_traj_gen])
+
+    return LaunchDescription([
+        config_arg,
+        rviz_config_arg,
+        robot_localization_node,
+        robot_localization_node_ref,
+        description_timer,
+        rviz_timer,
+        stonefish_timer,
+        traj_gen_timer,
+        thruster_manager_node,
+        mfsm_node,
+        control_system_tester_node,
+        currents_timer
+    ])