diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 4f370844..5c23ae95 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -51,5 +51,5 @@ colcon build --packages-up-to buoy_tests --event-handlers console_direct+ source $COLCON_WS/install/setup.bash # Test all buoy packages -colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+ +colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+ --retest-until-pass 10 colcon test-result diff --git a/buoy_description/CMakeLists.txt b/buoy_description/CMakeLists.txt index be7cbb13..18c832ea 100644 --- a/buoy_description/CMakeLists.txt +++ b/buoy_description/CMakeLists.txt @@ -6,7 +6,7 @@ find_package(ament_cmake REQUIRED) set(BUOY_BASE_MODEL_PATH "/models/mbari_wec_base") file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}${BUOY_BASE_MODEL_PATH}) add_custom_command( - OUTPUT model_gen_cmd + OUTPUT base_model_gen_cmd COMMAND python3 -m em ${CMAKE_CURRENT_SOURCE_DIR}${BUOY_BASE_MODEL_PATH}/model.sdf.em > ${CMAKE_CURRENT_BINARY_DIR}${BUOY_BASE_MODEL_PATH}/model.sdf @@ -16,6 +16,17 @@ file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}${BUOY_MODEL_PATH}) set(BUOY_ROS_MODEL_PATH "/models/mbari_wec_ros") file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}${BUOY_ROS_MODEL_PATH}) +add_custom_target(base_model_gen_target ALL + DEPENDS base_model_gen_cmd +) + +add_custom_command( + OUTPUT model_gen_cmd + COMMAND python3 -m em + ${CMAKE_CURRENT_SOURCE_DIR}${BUOY_MODEL_PATH}/model.sdf.em > + ${CMAKE_CURRENT_BINARY_DIR}${BUOY_MODEL_PATH}/model.sdf +) + add_custom_target(model_gen_target ALL DEPENDS model_gen_cmd ) diff --git a/buoy_description/models/mbari_wec/model.sdf b/buoy_description/models/mbari_wec/model.sdf.em similarity index 94% rename from buoy_description/models/mbari_wec/model.sdf rename to buoy_description/models/mbari_wec/model.sdf.em index 4c9da60c..501ed4ee 100644 --- a/buoy_description/models/mbari_wec/model.sdf +++ b/buoy_description/models/mbari_wec/model.sdf.em @@ -1,4 +1,11 @@ +@{ +try: + scale_factor +except NameError: + scale_factor = 1.0 # not defined so default +}@ + @@ -12,6 +19,7 @@ 1.375 0.30 1 + @(scale_factor) diff --git a/buoy_description/models/mbari_wec_base/model.sdf.em b/buoy_description/models/mbari_wec_base/model.sdf.em index bdc73eef..57595454 100644 --- a/buoy_description/models/mbari_wec_base/model.sdf.em +++ b/buoy_description/models/mbari_wec_base/model.sdf.em @@ -27,13 +27,37 @@ tether_top_length = 2.5 num_tether_bottom_links = 5 -# Heave cone -heave_total_mass = 817 -trefoil_mass = 10 +# TrefoilDoors +trefoil_pose = {'open': 1.047, 'closed': 0.0} +# check if door_state was passed in by empy +try: + door_state +except NameError: + door_state = 'closed' # not defined so default closed + +if 'open' in door_state: + mu_zz = 3000.0 # kg, Heave Added Mass + heavecone_zWabsW = -3200.0 # kg/m, Heave Quadratic Drag # Heave cone -heave_total_mass = 817 -trefoil_mass = 10 +heave_total_mass = 817 # kg +trefoil_mass = 20 # kg + +######################## +# TODO(anyone) mu_zz below is currently unused. Will be used in #115 +######################## + +# check if mu_zz was set by door_state 'open' (or passed in by empy) +try: + mu_zz +except NameError: + mu_zz = 10000.0 # kg, not defined so default with doors closed + +# check if z_ww was set by door_state 'open' (or passed in by empy) +try: + heavecone_zWabsW +except NameError: + heavecone_zWabsW = -3900.0 # kg/m, not defined so default with doors closed ################### # Computed values # @@ -449,18 +473,11 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius - + HeaveCone Trefoil 1 - 0.0 0.0 0.0 0 0 0 - - - 0.0 - 1.047 - - 0.0 0.0 1.0 - + 0.0 0.0 0.0 0 0 @(trefoil_pose[door_state]) @@ -497,7 +514,8 @@ pto_scale = pto_inner_radius / pto_stl_inner_radius HeaveCone -1580 -1580 - -3900 + + @(heavecone_zWabsW) -4620 -4620 -50 diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt index bab3f0db..45ee439f 100644 --- a/buoy_gazebo/CMakeLists.txt +++ b/buoy_gazebo/CMakeLists.txt @@ -143,6 +143,10 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +# Install batch running script +install(PROGRAMS scripts/mbari_wec_batch DESTINATION lib/${PROJECT_NAME}/) +install(FILES scripts/example_sim_params.yaml DESTINATION share/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/buoy_gazebo/launch/mbari_wec.launch.py b/buoy_gazebo/launch/mbari_wec.launch.py index e29126dd..b80fa48a 100644 --- a/buoy_gazebo/launch/mbari_wec.launch.py +++ b/buoy_gazebo/launch/mbari_wec.launch.py @@ -61,12 +61,16 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'), ), - launch_arguments={'gz_args': PathJoinSubstitution([ - pkg_buoy_gazebo, - 'worlds', - LaunchConfiguration('world_file') - ]), - 'debugger': LaunchConfiguration('debugger')}.items(), + launch_arguments={'gz_args': [ + LaunchConfiguration('gz_args'), PathJoinSubstitution([' ']), + PathJoinSubstitution([ + pkg_buoy_gazebo, + 'worlds', + LaunchConfiguration('world_file') + ]) + ], + 'debugger': LaunchConfiguration('debugger'), + 'on_exit_shutdown': 'True'}.items(), ) # Bridge to forward tf and joint states to ros2 diff --git a/buoy_gazebo/launch/mbari_wec_batch.launch.py b/buoy_gazebo/launch/mbari_wec_batch.launch.py new file mode 100644 index 00000000..4fb76a2a --- /dev/null +++ b/buoy_gazebo/launch/mbari_wec_batch.launch.py @@ -0,0 +1,44 @@ +# Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch Gazebo world with a buoy.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, Shutdown +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + + +def generate_launch_description(): + + sim_params_yaml_launch_arg = DeclareLaunchArgument( + 'sim_params_yaml', + description='Input batch sim params yaml' + ) + + batch_sim = Node( + package='buoy_gazebo', + executable='mbari_wec_batch', + arguments=[ + LaunchConfiguration('sim_params_yaml'), + ], + output='screen', + on_exit=Shutdown() + ) + + return LaunchDescription([ + sim_params_yaml_launch_arg, + batch_sim, + ]) diff --git a/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py b/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py index 25bc1bdd..4d148dea 100644 --- a/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py +++ b/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py @@ -66,7 +66,8 @@ def generate_launch_description(): 'worlds', LaunchConfiguration('world_file') ]), - 'debugger': LaunchConfiguration('debugger')}.items(), + 'debugger': LaunchConfiguration('debugger'), + 'on_exit_shutdown': 'True'}.items(), ) # Bridge to forward tf and joint states to ros2 diff --git a/buoy_gazebo/scripts/example_sim_params.yaml b/buoy_gazebo/scripts/example_sim_params.yaml new file mode 100644 index 00000000..588bee43 --- /dev/null +++ b/buoy_gazebo/scripts/example_sim_params.yaml @@ -0,0 +1,4 @@ +seed: 42 +duration: 5 +door_state: ['closed', 'open'] +scale_factor: [0.5, 0.625, 0.75, 0.875, 1.0, 1.125, 1.25, 1.375, 1.4] diff --git a/buoy_gazebo/scripts/mbari_wec_batch b/buoy_gazebo/scripts/mbari_wec_batch new file mode 100755 index 00000000..e6ad0907 --- /dev/null +++ b/buoy_gazebo/scripts/mbari_wec_batch @@ -0,0 +1,205 @@ +#!/usr/bin/python3 +# Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch batch of buoy simulations.""" + +from em import invoke as empy +import numpy as np +import yaml + +import os +import shutil +import sys +import time + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + +import rclpy +from rclpy.node import Node + + +PHYSICS_MAX_STEP_SIZE = 0.001 # seconds + +def generate_simulations(sim_params_yaml): + + rclpy.init() + node = Node('mbari_wec_batch') + + with open(sim_params_yaml, 'r') as fd: + sim_params = yaml.load(fd, Loader=yaml.CLoader) + + # Grab params from yaml + # seed + try: + if 'seed' in sim_params: + seed = int(sim_params['seed']) + # TODO(anyone) remove warning and `seed = None` when --seed + # supported by gz sim + node.get_logger().warn('sim_params_yaml: seed parameter not yet supported -- ignoring') + seed = None + else: + seed = None + node.get_logger().warn('sim_params_yaml: optional seed parameter not specified') + except TypeError as err: + node.get_logger().error('sim_params_yaml: seed parameter ' + + 'must be a single value') + sys.exit(-1) + + # duration + try: + if 'duration' not in sim_params: + node.get_logger().error('sim_params_yaml: required duration parameter not specified') + sys.exit(-1) + duration = float(sim_params['duration']) + except TypeError as err: + node.get_logger().error('sim_params_yaml: duration parameter ' + + 'must be a single value in seconds') + sys.exit(-1) + + # convert duration to iterations + iterations = int(round(duration / PHYSICS_MAX_STEP_SIZE)) + + # door state + if 'door_state' in sim_params: + door_state = sim_params['door_state'] + else: + node.get_logger().warn( + "sim_params_yaml: door_state parameter not specified -- defaulting to 'closed'" + ) + door_state = ['closed'] + + # scale factor + if 'scale_factor' in sim_params: + scale_factor = np.array(sim_params['scale_factor'], dtype=float) + else: + node.get_logger().warn( + 'sim_params_yaml: scale_factor parameter not specified -- defaulting to 1.0' + ) + scale_factor = [1.0] + + # create batch results directory + timestr = time.strftime("%Y%m%d%H%M%S") + batch_results_dir = f'batch_results_{timestr}' + node.get_logger().info(f'Creating batch results directory: {batch_results_dir}') + os.makedirs(batch_results_dir) + + sim_params_name, dot_yaml = os.path.splitext(os.path.basename(sim_params_yaml)) + sim_params_date_yaml = sim_params_name + f'_{timestr}' + dot_yaml + node.get_logger().info('Copying sim params yaml to batch results directory: ' + + os.path.join(batch_results_dir, + sim_params_date_yaml)) + shutil.copy(sim_params_yaml, + os.path.join(batch_results_dir, + sim_params_date_yaml)) + + # generate test matrix + batch_params = list(zip(*[param.ravel() for param in np.meshgrid(door_state, + scale_factor)])) + + node.get_logger().info(f'Generated {len(batch_params)} simulation runs:') + node.get_logger().info('Seed, Duration, Door State, Scale Factor') + [node.get_logger().info(f'{seed if seed is not None else ""}, {duration}, {ds}, {sf}') + for ds, sf in batch_params] + + node.get_logger().info('Creating log file: ' + + os.path.join(batch_results_dir, f'batch_runs_{timestr}.log')) + with open(os.path.join(batch_results_dir, f'batch_runs_{timestr}.log'), 'w') as fd: + fd.write(f'# Generated {len(batch_params)} simulation runs\n') + fd.write('Run Index, Start Time, rosbag2 File Name,' + + ' Seed, Duration, Door State, Scale Factor\n') + + # Find packages + pkg_buoy_gazebo = get_package_share_directory('buoy_gazebo') + pkg_buoy_description = get_package_share_directory('buoy_description') + + # Find model templates + model_dir = 'mbari_wec_base' + empy_base_sdf_file = os.path.join(pkg_buoy_description, 'models', model_dir, 'model.sdf.em') + base_sdf_file = os.path.join(pkg_buoy_description, 'models', model_dir, 'model.sdf') + + model_dir = 'mbari_wec' + empy_sdf_file = os.path.join(pkg_buoy_description, 'models', model_dir, 'model.sdf.em') + sdf_file = os.path.join(pkg_buoy_description, 'models', model_dir, 'model.sdf') + + # Start batch runs + for idx, (ds, sf) in enumerate(batch_params): + node.get_logger().info(f'\n\nSim run [{idx}] for {duration} seconds:' + + f" door state='{ds}', scale factor={sf}\n") + + # fill model templates with params + empy(['-D', f"door_state = '{ds}'", + '-o', base_sdf_file, + empy_base_sdf_file]) + + empy(['-D', f"scale_factor = {sf}", + '-o', sdf_file, + empy_sdf_file]) + + node.get_logger().info('running gz-sim with gz_args:=' + + '-rs' + + f' --iterations {iterations}' + + (f' --seed {seed}' if seed is not None else '')) + + # Dynamically create launch file + def generate_launch_description(): + mbari_wec = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_buoy_gazebo, 'launch', 'mbari_wec.launch.py'), + ), + launch_arguments={'gz_args': '-rs' + + f' --iterations {iterations}' + + (f' --seed {seed}' if seed is not None else '') + }.items(), + ) + + # record all topics with rosbag2 + start_time = time.strftime("%Y%m%d%H%M%S") + rosbag2_name = f'rosbag2_batch_sim_{idx}_{start_time}' + rosbag2 = ExecuteProcess( + cmd=['ros2', 'bag', 'record', + '-o', os.path.join(batch_results_dir, rosbag2_name), '-a'], + output='screen' + ) + + # Write to log + with open(os.path.join(batch_results_dir, f'batch_runs_{timestr}.log'), 'a') as fd: + fd.write(f'{idx}, {start_time}, {rosbag2_name}' + + f', {seed if seed is not None else ""}, {duration}, {ds}, {sf}\n') + + return LaunchDescription([ + rosbag2, + mbari_wec + ]) + + # Launch sim instance + ls = LaunchService() + ls.include_launch_description(generate_launch_description()) + result = ls.run() + if result: + node.get_logger().error(f'Simulation run [{idx}] was not successful: ' + + f'return code [{result}]') + +if __name__=='__main__': + import argparse + parser = argparse.ArgumentParser(description='Batch running for buoy_sim') + parser.add_argument('sim_params_yaml', + help='yaml file with batch parameters') + args, unknown = parser.parse_known_args() + generate_simulations(args.sim_params_yaml) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index a9bf6e9b..601b0480 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -54,12 +54,15 @@ class ElectroHydraulicPTOPrivate /// \brief Piston joint entity gz::sim::Entity PrismaticJointEntity{gz::sim::kNullEntity}; -/// \brief Piston area + /// \brief Piston area double PistonArea{1.0}; /// \brief Rotor Inertia double RotorInertia{1.0}; + /// \brief Default Scale Factor + double DefaultScaleFactor{DEFAULT_SCALE_FACTOR}; + /// \brief Model interface gz::sim::Model model{gz::sim::kNullEntity}; @@ -72,6 +75,7 @@ class ElectroHydraulicPTOPrivate static constexpr double I_BattChargeMax{7.0}; static constexpr double MaxTargetVoltage{325.0}; + // Dummy compensator pressure for ROS messages, not simulated static constexpr double CompensatorPressure{2.91}; @@ -133,6 +137,9 @@ void ElectroHydraulicPTO::Configure( this->dataPtr->PistonArea = SdfParamDouble(_sdf, "PistonArea", this->dataPtr->PistonArea); } + this->dataPtr->DefaultScaleFactor = + SdfParamDouble(_sdf, "ScaleFactor", this->dataPtr->DefaultScaleFactor); + if (_sdf->HasElement("VelMode")) { this->dataPtr->VelMode = true; } @@ -247,7 +254,7 @@ void ElectroHydraulicPTO::PreUpdate( if (pto_state.scale_command) { this->dataPtr->functor.I_Wind.ScaleFactor = pto_state.scale_command.value(); } else { - this->dataPtr->functor.I_Wind.ScaleFactor = DEFAULT_SCALE_FACTOR; + this->dataPtr->functor.I_Wind.ScaleFactor = this->dataPtr->DefaultScaleFactor; } if (pto_state.retract_command) { diff --git a/buoy_tests/launch/no_inputs_py.launch.py b/buoy_tests/launch/no_inputs_py.launch.py index 85f7f0fa..2fce7c23 100644 --- a/buoy_tests/launch/no_inputs_py.launch.py +++ b/buoy_tests/launch/no_inputs_py.launch.py @@ -35,7 +35,9 @@ def generate_test_description(): gazebo_test_fixture = Node( package='buoy_tests', executable='no_inputs', - output='screen' + output='screen' # , + # TODO(anyone) for some reason this makes the test fail + # on_exit=launch.actions.Shutdown() ) bridge = Node(package='ros_gz_bridge', diff --git a/buoy_tests/launch/no_inputs_ros_feedback.launch.py b/buoy_tests/launch/no_inputs_ros_feedback.launch.py index cff3fadc..81a67f2e 100644 --- a/buoy_tests/launch/no_inputs_ros_feedback.launch.py +++ b/buoy_tests/launch/no_inputs_ros_feedback.launch.py @@ -29,7 +29,8 @@ def generate_test_description(): gazebo_test_fixture = Node( package='buoy_tests', executable='no_inputs_ros_feedback', - output='screen' + output='screen', + on_exit=launch.actions.Shutdown() ) bridge = Node(package='ros_gz_bridge', diff --git a/buoy_tests/launch/pc_commands_ros_feedback.launch.py b/buoy_tests/launch/pc_commands_ros_feedback.launch.py index 8ebc275c..b1df2ee5 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback.launch.py @@ -39,7 +39,8 @@ def generate_test_description(): package='buoy_tests', executable='pc_commands_ros_feedback', output='screen', - parameters=[config] + parameters=[config], + on_exit=launch.actions.Shutdown() ) bridge = Node(package='ros_gz_bridge', diff --git a/buoy_tests/launch/pto_experiment_comparison.launch.py b/buoy_tests/launch/pto_experiment_comparison.launch.py index a65a2c30..3c6b4b7b 100644 --- a/buoy_tests/launch/pto_experiment_comparison.launch.py +++ b/buoy_tests/launch/pto_experiment_comparison.launch.py @@ -23,7 +23,8 @@ def generate_launch_description(): gazebo_test_fixture = launchNode( package='buoy_tests', executable='experiment_comparison', - output='screen' + output='screen', + on_exit=launch.actions.Shutdown() ) return launch.LaunchDescription([ diff --git a/buoy_tests/launch/run_server.launch.py b/buoy_tests/launch/run_server.launch.py index 03229aa6..09f8ff08 100644 --- a/buoy_tests/launch/run_server.launch.py +++ b/buoy_tests/launch/run_server.launch.py @@ -23,7 +23,8 @@ def generate_launch_description(): gazebo_test_fixture = launchNode( package='buoy_tests', executable='fixture_server', - output='screen' + output='screen', + on_exit=launch.actions.Shutdown() ) bridge = launchNode(package='ros_gz_bridge', diff --git a/buoy_tests/launch/sc_commands_ros_feedback.launch.py b/buoy_tests/launch/sc_commands_ros_feedback.launch.py index c6306960..f061f3d5 100644 --- a/buoy_tests/launch/sc_commands_ros_feedback.launch.py +++ b/buoy_tests/launch/sc_commands_ros_feedback.launch.py @@ -29,7 +29,8 @@ def generate_test_description(): gazebo_test_fixture = Node( package='buoy_tests', executable='sc_commands_ros_feedback', - output='screen' + output='screen', + on_exit=launch.actions.Shutdown() ) bridge = Node(package='ros_gz_bridge', diff --git a/buoy_tests/testing_utils/utils.py b/buoy_tests/testing_utils/utils.py index 3d7e439b..b999c602 100644 --- a/buoy_tests/testing_utils/utils.py +++ b/buoy_tests/testing_utils/utils.py @@ -48,7 +48,8 @@ def default_generate_test_description(server='fixture_server', gazebo_test_fixture = launchNode( package='buoy_tests', executable=server, - output='screen' + output='screen', + on_exit=launch.actions.Shutdown() ) bridge = launchNode(package='ros_gz_bridge', diff --git a/buoy_tests/tests/fixture_server_sinusoidal_piston.cpp b/buoy_tests/tests/fixture_server_sinusoidal_piston.cpp index 9a0636fb..7bfbb956 100644 --- a/buoy_tests/tests/fixture_server_sinusoidal_piston.cpp +++ b/buoy_tests/tests/fixture_server_sinusoidal_piston.cpp @@ -104,7 +104,7 @@ TEST(BuoyTests, RunServer) response->success = true; return; } else { - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( rclcpp::get_logger("run_server"), "Incoming request\niterations: " << request->iterations); } @@ -119,7 +119,7 @@ TEST(BuoyTests, RunServer) EXPECT_EQ(response->iterations, request->iterations); - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( rclcpp::get_logger("run_server"), "Response: " << std::boolalpha << response->success << std::noboolalpha); }