From f3270ac6974482d8847d53c90bf48bc6fe5c812c Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Mon, 23 Jan 2023 20:11:18 -0800 Subject: [PATCH 01/18] proof of concept for looping over simulation parameters Signed-off-by: Michael Anderson --- buoy_description/CMakeLists.txt | 13 ++- buoy_description/models/mbari_wec/model.sdf | 1 + .../models/mbari_wec/model.sdf.em | 83 ++++++++++++++++++ .../models/mbari_wec_base/model.sdf.em | 29 ++++--- buoy_gazebo/CMakeLists.txt | 3 + buoy_gazebo/launch/mbari_wec.launch.py | 16 ++-- buoy_gazebo/scripts/mbari_wec_batch | 87 +++++++++++++++++++ buoy_gazebo/scripts/sim_params.yaml | 3 + .../ElectroHydraulicPTO.cpp | 11 ++- 9 files changed, 225 insertions(+), 21 deletions(-) create mode 100644 buoy_description/models/mbari_wec/model.sdf.em create mode 100755 buoy_gazebo/scripts/mbari_wec_batch create mode 100644 buoy_gazebo/scripts/sim_params.yaml 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 index 77bf5b2a..cdaaa1d2 100644 --- a/buoy_description/models/mbari_wec/model.sdf +++ b/buoy_description/models/mbari_wec/model.sdf @@ -11,6 +11,7 @@ 1.375 0.30 1 + 1.0 diff --git a/buoy_description/models/mbari_wec/model.sdf.em b/buoy_description/models/mbari_wec/model.sdf.em new file mode 100644 index 00000000..cdb2bea2 --- /dev/null +++ b/buoy_description/models/mbari_wec/model.sdf.em @@ -0,0 +1,83 @@ + +@{ +try: + scale_factor +except NameError: + scale_factor = 1.0 # not defined so default +}@ + + + + + + package://buoy_description/models/mbari_wec_base + + + + HydraulicRam + 1.375 + 0.30 + 1 + @(scale_factor) + + + + + HydraulicRam + upper_polytropic + true + + 7.75e-6 + 5.1e-8 + 1.7e+6 + 2.03 + 0.0127 + 0.0226 + 263.15 + 0.1 + 0.2968 + 1.04 + true + -0.1 + 0.05 + 1.1725 + 1.2139 + 1.6159 + 1.8578 + 410152 + 410190 + + + + + HydraulicRam + lower_polytropic + false + + 7.75e-6 + 5.1e-8 + 1.7e+6 + 2.03 + 0.0115 + 0.0463 + 283.15 + 0.1 + 0.2968 + 1.04 + true + -0.01 + 0.1 + 1.1967 + 1.1944 + 0.9695 + 1.1850 + 1212060 + 1212740 + + + + HydraulicRam + + + + diff --git a/buoy_description/models/mbari_wec_base/model.sdf.em b/buoy_description/models/mbari_wec_base/model.sdf.em index 53ccd5db..156f9f8f 100644 --- a/buoy_description/models/mbari_wec_base/model.sdf.em +++ b/buoy_description/models/mbari_wec_base/model.sdf.em @@ -27,13 +27,23 @@ 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} +try: + door_state +except NameError: + door_state = 'closed' # not defined so default + +if 'open' in door_state: + mu_zz = 3000.0 # Heave cone heave_total_mass = 817 -trefoil_mass = 10 +trefoil_mass = 20 +try: + mu_zz +except NameError: + mu_zz = 10000.0 # not defined so default ################### # Computed values # @@ -449,18 +459,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]) diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt index bab3f0db..20998ae9 100644 --- a/buoy_gazebo/CMakeLists.txt +++ b/buoy_gazebo/CMakeLists.txt @@ -143,6 +143,9 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +# Install batch running script +install(PROGRAMS scripts/mbari_wec_batch DESTINATION bin) + 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..8c26de0b 100644 --- a/buoy_gazebo/launch/mbari_wec.launch.py +++ b/buoy_gazebo/launch/mbari_wec.launch.py @@ -25,6 +25,9 @@ from launch_ros.actions import Node +# empy -D "door_state = 'open'" /home/anderson/wec/igngzb/openrobotics/workspace/src/buoy_sim/buoy_description/models/mbari_wec_base/model.sdf.em + + def generate_launch_description(): pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') @@ -61,11 +64,14 @@ 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') - ]), + launch_arguments={'gz_args': [ + LaunchConfiguration('gz_args'), PathJoinSubstitution([' ']), + PathJoinSubstitution([ + pkg_buoy_gazebo, + 'worlds', + LaunchConfiguration('world_file') + ]) + ], 'debugger': LaunchConfiguration('debugger')}.items(), ) diff --git a/buoy_gazebo/scripts/mbari_wec_batch b/buoy_gazebo/scripts/mbari_wec_batch new file mode 100755 index 00000000..ef56a10c --- /dev/null +++ b/buoy_gazebo/scripts/mbari_wec_batch @@ -0,0 +1,87 @@ +#!/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 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_simulations(yaml_config): + with open(yaml_config) as fd: + config = yaml.load(fd, Loader=yaml.CLoader) + + iterations = config['iterations'] + door_state = config['door_state'] + scale_factor = config['scale_factor'] + simulation_params = list(zip(*[param.ravel() for param in np.meshgrid(iterations, + door_state, + scale_factor)])) + + print(f'Generated {len(simulation_params)} simulation runs:') + print(f'Iterations, Door State, Scale Factor\n{np.array(simulation_params)}') + + pkg_buoy_gazebo = get_package_share_directory('buoy_gazebo') + pkg_buoy_description = get_package_share_directory('buoy_description') + + 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') + + for idx, (itx, ds, sf) in enumerate(simulation_params): + print(f'\n\nSim run [{idx}] for {itx} iterations: door state={ds}, scale factor={sf}\n\n') + 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]) + + def generate_launch_description(): + mbari_wec = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_buoy_gazebo, 'launch', 'mbari_wec.launch.py'), + ), + launch_arguments={'gz_args': f'-rs --iterations {itx}', + 'on_exit_shutdown': 'True'}.items(), + ) + return LaunchDescription([ + mbari_wec + ]) + + ls = LaunchService() + ls.include_launch_description(generate_launch_description()) + result = ls.run() + if result: + print(f'Simulation run [{idx}] was not successful: return code [{result}]') + +if __name__=='__main__': + import sys + generate_simulations(sys.argv[1]) diff --git a/buoy_gazebo/scripts/sim_params.yaml b/buoy_gazebo/scripts/sim_params.yaml new file mode 100644 index 00000000..b27c3fef --- /dev/null +++ b/buoy_gazebo/scripts/sim_params.yaml @@ -0,0 +1,3 @@ +iterations: 5000 +door_state: ['closed', 'open'] +scale_factor: [0.5, 0.625, 0.75, 0.875, 1.0, 1.125, 1.25, 1.375, 1.5] 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) { From 7bd9088a3187269f9722528400b43ffd9f19bfc8 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Mon, 23 Jan 2023 20:32:12 -0800 Subject: [PATCH 02/18] try to fix Signed-off-by: Michael Anderson From d78e16ef9d2de9abe5dc2be05f08daa520d01ca0 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 09:33:38 -0800 Subject: [PATCH 03/18] set on_exit shutdown in launch files Signed-off-by: Michael Anderson --- buoy_gazebo/launch/mbari_wec.launch.py | 3 ++- buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py | 1 + buoy_gazebo/scripts/mbari_wec_batch | 3 +-- buoy_tests/launch/no_inputs_py.launch.py | 3 ++- buoy_tests/launch/no_inputs_ros_feedback.launch.py | 3 ++- buoy_tests/launch/pc_commands_ros_feedback.launch.py | 3 ++- buoy_tests/launch/pto_experiment_comparison.launch.py | 3 ++- buoy_tests/launch/run_server.launch.py | 3 ++- buoy_tests/launch/sc_commands_ros_feedback.launch.py | 3 ++- buoy_tests/testing_utils/utils.py | 3 ++- 10 files changed, 18 insertions(+), 10 deletions(-) diff --git a/buoy_gazebo/launch/mbari_wec.launch.py b/buoy_gazebo/launch/mbari_wec.launch.py index 8c26de0b..f465c90f 100644 --- a/buoy_gazebo/launch/mbari_wec.launch.py +++ b/buoy_gazebo/launch/mbari_wec.launch.py @@ -72,7 +72,8 @@ def generate_launch_description(): 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/launch/mbari_wec_sinusoidal.launch.py b/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py index 25bc1bdd..f100817b 100644 --- a/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py +++ b/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py @@ -67,6 +67,7 @@ def generate_launch_description(): LaunchConfiguration('world_file') ]), 'debugger': LaunchConfiguration('debugger')}.items(), + 'on_exit_shutdown': 'True'}.items(), ) # Bridge to forward tf and joint states to ros2 diff --git a/buoy_gazebo/scripts/mbari_wec_batch b/buoy_gazebo/scripts/mbari_wec_batch index ef56a10c..400eb581 100755 --- a/buoy_gazebo/scripts/mbari_wec_batch +++ b/buoy_gazebo/scripts/mbari_wec_batch @@ -69,8 +69,7 @@ def generate_simulations(yaml_config): PythonLaunchDescriptionSource( os.path.join(pkg_buoy_gazebo, 'launch', 'mbari_wec.launch.py'), ), - launch_arguments={'gz_args': f'-rs --iterations {itx}', - 'on_exit_shutdown': 'True'}.items(), + launch_arguments={'gz_args': f'-rs --iterations {itx}'}.items(), ) return LaunchDescription([ mbari_wec diff --git a/buoy_tests/launch/no_inputs_py.launch.py b/buoy_tests/launch/no_inputs_py.launch.py index 85f7f0fa..d93da944 100644 --- a/buoy_tests/launch/no_inputs_py.launch.py +++ b/buoy_tests/launch/no_inputs_py.launch.py @@ -35,7 +35,8 @@ def generate_test_description(): gazebo_test_fixture = Node( package='buoy_tests', executable='no_inputs', - output='screen' + output='screen', + 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', From 93866dc8feea11c67b19774e7a22c71ef080bfe7 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 09:59:08 -0800 Subject: [PATCH 04/18] install script and example batch sim params; add launch file; set default to use example sim params Signed-off-by: Michael Anderson --- buoy_gazebo/CMakeLists.txt | 3 +- buoy_gazebo/launch/mbari_wec.launch.py | 3 -- buoy_gazebo/launch/mbari_wec_batch.launch.py | 51 +++++++++++++++++++ ...im_params.yaml => example_sim_params.yaml} | 0 buoy_gazebo/scripts/mbari_wec_batch | 4 +- 5 files changed, 55 insertions(+), 6 deletions(-) create mode 100644 buoy_gazebo/launch/mbari_wec_batch.launch.py rename buoy_gazebo/scripts/{sim_params.yaml => example_sim_params.yaml} (100%) diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt index 20998ae9..45ee439f 100644 --- a/buoy_gazebo/CMakeLists.txt +++ b/buoy_gazebo/CMakeLists.txt @@ -144,7 +144,8 @@ install(DIRECTORY ) # Install batch running script -install(PROGRAMS scripts/mbari_wec_batch DESTINATION bin) +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) diff --git a/buoy_gazebo/launch/mbari_wec.launch.py b/buoy_gazebo/launch/mbari_wec.launch.py index f465c90f..b80fa48a 100644 --- a/buoy_gazebo/launch/mbari_wec.launch.py +++ b/buoy_gazebo/launch/mbari_wec.launch.py @@ -25,9 +25,6 @@ from launch_ros.actions import Node -# empy -D "door_state = 'open'" /home/anderson/wec/igngzb/openrobotics/workspace/src/buoy_sim/buoy_description/models/mbari_wec_base/model.sdf.em - - def generate_launch_description(): pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') 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..9758a6d4 --- /dev/null +++ b/buoy_gazebo/launch/mbari_wec_batch.launch.py @@ -0,0 +1,51 @@ +# 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.""" + +import os + +from ament_index_python.packages import get_package_share_directory + +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(): + + pkg_buoy_gazebo = get_package_share_directory('buoy_gazebo') + example_sim_params_yaml = os.path.join(pkg_buoy_gazebo, 'example_sim_params.yaml') + + sim_params_yaml_launch_arg = DeclareLaunchArgument( + 'sim_params_yaml', default_value=[example_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/scripts/sim_params.yaml b/buoy_gazebo/scripts/example_sim_params.yaml similarity index 100% rename from buoy_gazebo/scripts/sim_params.yaml rename to buoy_gazebo/scripts/example_sim_params.yaml diff --git a/buoy_gazebo/scripts/mbari_wec_batch b/buoy_gazebo/scripts/mbari_wec_batch index 400eb581..1b6528b2 100755 --- a/buoy_gazebo/scripts/mbari_wec_batch +++ b/buoy_gazebo/scripts/mbari_wec_batch @@ -37,8 +37,8 @@ def generate_simulations(yaml_config): door_state = config['door_state'] scale_factor = config['scale_factor'] simulation_params = list(zip(*[param.ravel() for param in np.meshgrid(iterations, - door_state, - scale_factor)])) + door_state, + scale_factor)])) print(f'Generated {len(simulation_params)} simulation runs:') print(f'Iterations, Door State, Scale Factor\n{np.array(simulation_params)}') From c2cf6ca68e06728bf0b92aaab34416bc2753805f Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 10:21:02 -0800 Subject: [PATCH 05/18] add rosbag and a log file; rename some things Signed-off-by: Michael Anderson --- buoy_gazebo/scripts/mbari_wec_batch | 43 ++++++++++++++++++++--------- 1 file changed, 30 insertions(+), 13 deletions(-) diff --git a/buoy_gazebo/scripts/mbari_wec_batch b/buoy_gazebo/scripts/mbari_wec_batch index 1b6528b2..63dbe942 100755 --- a/buoy_gazebo/scripts/mbari_wec_batch +++ b/buoy_gazebo/scripts/mbari_wec_batch @@ -20,28 +20,34 @@ import numpy as np import yaml import os +import time from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch import LaunchService -from launch.actions import IncludeLaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -def generate_simulations(yaml_config): - with open(yaml_config) as fd: - config = yaml.load(fd, Loader=yaml.CLoader) +def generate_simulations(sim_params_yaml): + with open(sim_params_yaml, 'r') as fd: + sim_params = yaml.load(fd, Loader=yaml.CLoader) - iterations = config['iterations'] - door_state = config['door_state'] - scale_factor = config['scale_factor'] - simulation_params = list(zip(*[param.ravel() for param in np.meshgrid(iterations, - door_state, - scale_factor)])) + iterations = sim_params['iterations'] + door_state = sim_params['door_state'] + scale_factor = sim_params['scale_factor'] + batch_params = list(zip(*[param.ravel() for param in np.meshgrid(iterations, + door_state, + scale_factor)])) - print(f'Generated {len(simulation_params)} simulation runs:') - print(f'Iterations, Door State, Scale Factor\n{np.array(simulation_params)}') + print(f'Generated {len(batch_params)} simulation runs:') + print(f'Iterations, Door State, Scale Factor\n{np.array(batch_params)}') + + timestr = time.strftime("%Y%m%d%H%M%S") + with open(f'batch_runs_{timestr}.log', 'w') as fd: + fd.write(f'# Generated {len(batch_params)} simulation runs\n') + fd.write(f'Run Index, Iterations, Door State, Scale Factor\n') pkg_buoy_gazebo = get_package_share_directory('buoy_gazebo') pkg_buoy_description = get_package_share_directory('buoy_description') @@ -54,7 +60,7 @@ def generate_simulations(yaml_config): 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') - for idx, (itx, ds, sf) in enumerate(simulation_params): + for idx, (itx, ds, sf) in enumerate(batch_params): print(f'\n\nSim run [{idx}] for {itx} iterations: door state={ds}, scale factor={sf}\n\n') empy(['-D', f"door_state = '{ds}'", '-o', base_sdf_file, @@ -71,7 +77,18 @@ def generate_simulations(yaml_config): ), launch_arguments={'gz_args': f'-rs --iterations {itx}'}.items(), ) + + rosbag2_name = f'rosbag2_batch_sim_{idx}' + rosbag2 = ExecuteProcess( + cmd=['ros2', 'bag', 'record', '-o', rosbag2_name, '-a'], + output='screen' + ) + + with open(f'batch_runs_{timestr}.log', 'a') as fd: + fd.write(f'{idx, itx, ds, sf}\n') + return LaunchDescription([ + rosbag2, mbari_wec ]) From a2203b66d592ad4d4546938280b27e41fec4933a Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 11:19:15 -0800 Subject: [PATCH 06/18] fix max scale in example yaml Signed-off-by: Michael Anderson --- buoy_gazebo/scripts/example_sim_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buoy_gazebo/scripts/example_sim_params.yaml b/buoy_gazebo/scripts/example_sim_params.yaml index b27c3fef..935746df 100644 --- a/buoy_gazebo/scripts/example_sim_params.yaml +++ b/buoy_gazebo/scripts/example_sim_params.yaml @@ -1,3 +1,3 @@ iterations: 5000 door_state: ['closed', 'open'] -scale_factor: [0.5, 0.625, 0.75, 0.875, 1.0, 1.125, 1.25, 1.375, 1.5] +scale_factor: [0.5, 0.625, 0.75, 0.875, 1.0, 1.125, 1.25, 1.375, 1.4] From e39345493358ff2389b65f5fca8ab10d0e64d299 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 11:24:30 -0800 Subject: [PATCH 07/18] typo Signed-off-by: Michael Anderson --- buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py b/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py index f100817b..4d148dea 100644 --- a/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py +++ b/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py @@ -66,7 +66,7 @@ def generate_launch_description(): 'worlds', LaunchConfiguration('world_file') ]), - 'debugger': LaunchConfiguration('debugger')}.items(), + 'debugger': LaunchConfiguration('debugger'), 'on_exit_shutdown': 'True'}.items(), ) From 94fce17e57012ec3a7cdc71c5257622984c7763f Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 11:38:48 -0800 Subject: [PATCH 08/18] remove noisy prints; get tests passing Signed-off-by: Michael Anderson --- buoy_tests/launch/no_inputs_py.launch.py | 5 +++-- buoy_tests/tests/fixture_server_sinusoidal_piston.cpp | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/buoy_tests/launch/no_inputs_py.launch.py b/buoy_tests/launch/no_inputs_py.launch.py index d93da944..2fce7c23 100644 --- a/buoy_tests/launch/no_inputs_py.launch.py +++ b/buoy_tests/launch/no_inputs_py.launch.py @@ -35,8 +35,9 @@ def generate_test_description(): gazebo_test_fixture = Node( package='buoy_tests', executable='no_inputs', - output='screen', - on_exit=launch.actions.Shutdown() + 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/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); } From 48667fdd77486e9096692d6e572ab93de26e931a Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 11:49:51 -0800 Subject: [PATCH 09/18] use argparse Signed-off-by: Michael Anderson --- buoy_gazebo/scripts/mbari_wec_batch | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/buoy_gazebo/scripts/mbari_wec_batch b/buoy_gazebo/scripts/mbari_wec_batch index 63dbe942..b91e0c4b 100755 --- a/buoy_gazebo/scripts/mbari_wec_batch +++ b/buoy_gazebo/scripts/mbari_wec_batch @@ -99,5 +99,9 @@ def generate_simulations(sim_params_yaml): print(f'Simulation run [{idx}] was not successful: return code [{result}]') if __name__=='__main__': - import sys - generate_simulations(sys.argv[1]) + import argparse + parser = argparse.ArgumentParser(description='Batch running for buoy_sim') + parser.add_argument('sim_params_yaml', + help='yaml file with batch parameters') + args = parser.parse_args() + generate_simulations(args.sim_params_yaml) From 70013558037c9160fbdef3774c0bfeeb05abcd48 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 11:53:50 -0800 Subject: [PATCH 10/18] mbari_wec/model.sdf is now generated by empy Signed-off-by: Michael Anderson --- buoy_description/models/mbari_wec/model.sdf | 76 --------------------- 1 file changed, 76 deletions(-) delete mode 100644 buoy_description/models/mbari_wec/model.sdf diff --git a/buoy_description/models/mbari_wec/model.sdf b/buoy_description/models/mbari_wec/model.sdf deleted file mode 100644 index cdaaa1d2..00000000 --- a/buoy_description/models/mbari_wec/model.sdf +++ /dev/null @@ -1,76 +0,0 @@ - - - - - - package://buoy_description/models/mbari_wec_base - - - - HydraulicRam - 1.375 - 0.30 - 1 - 1.0 - - - - - HydraulicRam - upper_polytropic - true - - 7.75e-6 - 5.1e-8 - 1.7e+6 - 2.03 - 0.0127 - 0.0226 - 263.15 - 0.1 - 0.2968 - 1.04 - true - -0.1 - 0.05 - 1.1725 - 1.2139 - 1.6159 - 1.8578 - 410152 - 410190 - - - - - HydraulicRam - lower_polytropic - false - - 7.75e-6 - 5.1e-8 - 1.7e+6 - 2.03 - 0.0115 - 0.0463 - 283.15 - 0.1 - 0.2968 - 1.04 - true - -0.01 - 0.1 - 1.1967 - 1.1944 - 0.9695 - 1.1850 - 1212060 - 1212740 - - - - HydraulicRam - - - - From 0ce6b715fe5e7e7e7091f967fe222e9f6adb5186 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 12:51:02 -0800 Subject: [PATCH 11/18] try to debug pc commands test Signed-off-by: Michael Anderson --- .github/workflows/build-and-test.sh | 3 ++- buoy_tests/tests/pc_commands_ros_feedback.cpp | 5 +++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 4f370844..52f43708 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -51,5 +51,6 @@ 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+ +launch_test install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback.launch.py colcon test-result diff --git a/buoy_tests/tests/pc_commands_ros_feedback.cpp b/buoy_tests/tests/pc_commands_ros_feedback.cpp index ffea2655..e198bc6e 100644 --- a/buoy_tests/tests/pc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/pc_commands_ros_feedback.cpp @@ -359,14 +359,19 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) // Now send retract command node->pc_retract_response_future_ = node->send_pc_retract_command(retract); + std::cout << "after retract command" << std::endl; EXPECT_TRUE(node->pc_retract_response_future_.valid()) << "Retract future invalid!"; + std::cout << "after expect valid future" << std::endl; node->pc_retract_response_future_.wait(); + std::cout << "after wait future" << std::endl; EXPECT_EQ( node->pc_retract_response_future_.get()->result.value, node->pc_retract_response_future_.get()->result.OK); + std::cout << "after expect retract command OK" << std::endl; // Run a bit for retract command to process fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); + std::cout << "after server run" << std::endl; EXPECT_EQ(preCmdIterations + 4 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); From e0c211b149ebf95629f2d18d6ffd304a9beeffd8 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 14:23:16 -0800 Subject: [PATCH 12/18] debug pc commands test Signed-off-by: Michael Anderson --- .../PowerController/PowerController.cpp | 6 ++ buoy_tests/tests/pc_commands_ros_feedback.cpp | 67 +++++++++++-------- 2 files changed, 44 insertions(+), 29 deletions(-) diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 6e027b09..0bfe6113 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -344,6 +344,9 @@ struct PowerControllerPrivate services_->valid_scale_range_.from_value << ", " << services_->valid_scale_range_.to_value << "]"); } + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] PCScaleCommand Done [" << (int)response->result.value << "]"); }; services_->scale_command_service_ = ros_->node_->create_service( @@ -374,6 +377,9 @@ struct PowerControllerPrivate services_->valid_retract_range_.from_value << ", " << services_->valid_retract_range_.to_value << "]"); } + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] PCRetractCommand Done [" << (int)response->result.value << "]"); }; services_->retract_command_service_ = ros_->node_->create_service( diff --git a/buoy_tests/tests/pc_commands_ros_feedback.cpp b/buoy_tests/tests/pc_commands_ros_feedback.cpp index e198bc6e..3019e556 100644 --- a/buoy_tests/tests/pc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/pc_commands_ros_feedback.cpp @@ -74,10 +74,10 @@ class PCROSNode final : public buoy_api::Interface PBTorqueControlPolicy torque_policy_; - PCWindCurrServiceResponseFuture pc_wind_curr_response_future_; - PCBiasCurrServiceResponseFuture pc_bias_curr_response_future_; - PCScaleServiceResponseFuture pc_scale_response_future_; - PCRetractServiceResponseFuture pc_retract_response_future_; + // PCWindCurrServiceResponseFuture pc_wind_curr_response_future_; + // PCBiasCurrServiceResponseFuture pc_bias_curr_response_future_; + // PCScaleServiceResponseFuture pc_scale_response_future_; + // PCRetractServiceResponseFuture pc_retract_response_future_; explicit PCROSNode(const std::string & node_name) : buoy_api::Interface(node_name) @@ -308,12 +308,13 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) EXPECT_NE(node->wind_curr_, wc); // Now send wind curr command - node->pc_wind_curr_response_future_ = node->send_pc_wind_curr_command(wc); - EXPECT_TRUE(node->pc_wind_curr_response_future_.valid()) << "Winding Current future invalid!"; - node->pc_wind_curr_response_future_.wait(); + PCROSNode::PCWindCurrServiceResponseFuture pc_wind_curr_response_future = + node->send_pc_wind_curr_command(wc); + EXPECT_TRUE(pc_wind_curr_response_future.valid()) << "Winding Current future invalid!"; + pc_wind_curr_response_future.wait(); EXPECT_EQ( - node->pc_wind_curr_response_future_.get()->result.value, - node->pc_wind_curr_response_future_.get()->result.OK); + pc_wind_curr_response_future.get()->result.value, + pc_wind_curr_response_future.get()->result.OK); // Run a bit for wind curr command to process fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); @@ -333,15 +334,21 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) EXPECT_NE(node->scale_, scale); // Now send scale command - node->pc_scale_response_future_ = node->send_pc_scale_command(scale); - EXPECT_TRUE(node->pc_scale_response_future_.valid()) << "Scale future invalid!"; - node->pc_scale_response_future_.wait(); + PCROSNode::PCScaleServiceResponseFuture pc_scale_response_future = + node->send_pc_scale_command(scale); + std::cout << "scale: after command" << std::endl; + EXPECT_TRUE(pc_scale_response_future.valid()) << "Scale future invalid!"; + std::cout << "scale: after expect valid future" << std::endl; + pc_scale_response_future.wait(); + std::cout << "scale: after wait future" << std::endl; EXPECT_EQ( - node->pc_scale_response_future_.get()->result.value, - node->pc_scale_response_future_.get()->result.OK); + pc_scale_response_future.get()->result.value, + pc_scale_response_future.get()->result.OK); + std::cout << "scale: after expect command OK" << std::endl; // Run a bit for scale command to process fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); + std::cout << "scale: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 3 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); @@ -358,20 +365,21 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) EXPECT_NE(node->retract_, retract); // Now send retract command - node->pc_retract_response_future_ = node->send_pc_retract_command(retract); - std::cout << "after retract command" << std::endl; - EXPECT_TRUE(node->pc_retract_response_future_.valid()) << "Retract future invalid!"; - std::cout << "after expect valid future" << std::endl; - node->pc_retract_response_future_.wait(); - std::cout << "after wait future" << std::endl; + PCROSNode::PCRetractServiceResponseFuture pc_retract_response_future = + node->send_pc_retract_command(retract); + std::cout << "retract: after command" << std::endl; + EXPECT_TRUE(pc_retract_response_future.valid()) << "Retract future invalid!"; + std::cout << "retract: after expect valid future" << std::endl; + pc_retract_response_future.wait(); + std::cout << "retract: after wait future" << std::endl; EXPECT_EQ( - node->pc_retract_response_future_.get()->result.value, - node->pc_retract_response_future_.get()->result.OK); - std::cout << "after expect retract command OK" << std::endl; + pc_retract_response_future.get()->result.value, + pc_retract_response_future.get()->result.OK); + std::cout << "retract: after expect command OK" << std::endl; // Run a bit for retract command to process fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); - std::cout << "after server run" << std::endl; + std::cout << "retract: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 4 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); @@ -412,12 +420,13 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) EXPECT_NE(node->bias_curr_, bc); // Now send bias curr command - node->pc_bias_curr_response_future_ = node->send_pc_bias_curr_command(bc); - EXPECT_TRUE(node->pc_bias_curr_response_future_.valid()) << "Bias Current future invalid!"; - node->pc_bias_curr_response_future_.wait(); + PCROSNode::PCBiasCurrServiceResponseFuture pc_bias_curr_response_future = + node->send_pc_bias_curr_command(bc); + EXPECT_TRUE(pc_bias_curr_response_future.valid()) << "Bias Current future invalid!"; + pc_bias_curr_response_future.wait(); EXPECT_EQ( - node->pc_bias_curr_response_future_.get()->result.value, - node->pc_bias_curr_response_future_.get()->result.OK); + pc_bias_curr_response_future.get()->result.value, + pc_bias_curr_response_future.get()->result.OK); // Run a bit for bias curr command to move piston int bias_curr_iterations{9000}, bias_curr_timeout_iterations{10000}; From b1c274c19ab7202a542b5392cacda50cfd0c3775 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 15:03:15 -0800 Subject: [PATCH 13/18] increase timeout; remove debugging prints Signed-off-by: Michael Anderson --- .../controllers/PowerController/PowerController.cpp | 4 ++-- buoy_tests/CMakeLists.txt | 4 ++-- buoy_tests/launch/pc_commands_ros_feedback.launch.py | 2 +- buoy_tests/tests/pc_commands_ros_feedback.cpp | 10 ---------- 4 files changed, 5 insertions(+), 15 deletions(-) diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 0bfe6113..5ea53d1e 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -344,7 +344,7 @@ struct PowerControllerPrivate services_->valid_scale_range_.from_value << ", " << services_->valid_scale_range_.to_value << "]"); } - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( ros_->node_->get_logger(), "[ROS 2 Power Control] PCScaleCommand Done [" << (int)response->result.value << "]"); }; @@ -377,7 +377,7 @@ struct PowerControllerPrivate services_->valid_retract_range_.from_value << ", " << services_->valid_retract_range_.to_value << "]"); } - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( ros_->node_->get_logger(), "[ROS 2 Power Control] PCRetractCommand Done [" << (int)response->result.value << "]"); }; diff --git a/buoy_tests/CMakeLists.txt b/buoy_tests/CMakeLists.txt index 5c387403..59a4cb1d 100644 --- a/buoy_tests/CMakeLists.txt +++ b/buoy_tests/CMakeLists.txt @@ -89,7 +89,7 @@ if(BUILD_TESTING) if(buoy_add_gtest_LAUNCH_TEST) add_launch_test(launch/${TEST_NAME}.launch.py - TIMEOUT 650 + TIMEOUT 1000 ) else() ament_add_gtest_test(${TEST_NAME}) @@ -128,7 +128,7 @@ if(BUILD_TESTING) endif() add_launch_test(launch/${PYTEST_NAME}_py.launch.py - TIMEOUT 650 + TIMEOUT 1000 ) endfunction() diff --git a/buoy_tests/launch/pc_commands_ros_feedback.launch.py b/buoy_tests/launch/pc_commands_ros_feedback.launch.py index b1df2ee5..28649d6e 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback.launch.py @@ -59,7 +59,7 @@ def generate_test_description(): class PCCommandsROSTest(unittest.TestCase): def test_termination(self, gazebo_test_fixture, proc_info): - proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=600) + proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=975) @launch_testing.post_shutdown_test() diff --git a/buoy_tests/tests/pc_commands_ros_feedback.cpp b/buoy_tests/tests/pc_commands_ros_feedback.cpp index 3019e556..17ad6a2b 100644 --- a/buoy_tests/tests/pc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/pc_commands_ros_feedback.cpp @@ -336,19 +336,14 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) // Now send scale command PCROSNode::PCScaleServiceResponseFuture pc_scale_response_future = node->send_pc_scale_command(scale); - std::cout << "scale: after command" << std::endl; EXPECT_TRUE(pc_scale_response_future.valid()) << "Scale future invalid!"; - std::cout << "scale: after expect valid future" << std::endl; pc_scale_response_future.wait(); - std::cout << "scale: after wait future" << std::endl; EXPECT_EQ( pc_scale_response_future.get()->result.value, pc_scale_response_future.get()->result.OK); - std::cout << "scale: after expect command OK" << std::endl; // Run a bit for scale command to process fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); - std::cout << "scale: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 3 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); @@ -367,19 +362,14 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) // Now send retract command PCROSNode::PCRetractServiceResponseFuture pc_retract_response_future = node->send_pc_retract_command(retract); - std::cout << "retract: after command" << std::endl; EXPECT_TRUE(pc_retract_response_future.valid()) << "Retract future invalid!"; - std::cout << "retract: after expect valid future" << std::endl; pc_retract_response_future.wait(); - std::cout << "retract: after wait future" << std::endl; EXPECT_EQ( pc_retract_response_future.get()->result.value, pc_retract_response_future.get()->result.OK); - std::cout << "retract: after expect command OK" << std::endl; // Run a bit for retract command to process fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); - std::cout << "retract: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 4 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); From 78ed18a490694829193fc549d0a4f936ee891be7 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 15:33:11 -0800 Subject: [PATCH 14/18] keep debugging Signed-off-by: Michael Anderson --- buoy_tests/tests/pc_commands_ros_feedback.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/buoy_tests/tests/pc_commands_ros_feedback.cpp b/buoy_tests/tests/pc_commands_ros_feedback.cpp index 17ad6a2b..8f25efd9 100644 --- a/buoy_tests/tests/pc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/pc_commands_ros_feedback.cpp @@ -311,13 +311,17 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) PCROSNode::PCWindCurrServiceResponseFuture pc_wind_curr_response_future = node->send_pc_wind_curr_command(wc); EXPECT_TRUE(pc_wind_curr_response_future.valid()) << "Winding Current future invalid!"; + std::cout << "wind curr: before wait future" << std::endl; pc_wind_curr_response_future.wait(); + std::cout << "wind curr: after wait future" << std::endl; EXPECT_EQ( pc_wind_curr_response_future.get()->result.value, pc_wind_curr_response_future.get()->result.OK); // Run a bit for wind curr command to process + std::cout << "wind curr: before server run" << std::endl; fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); + std::cout << "wind curr: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 2 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); @@ -337,13 +341,17 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) PCROSNode::PCScaleServiceResponseFuture pc_scale_response_future = node->send_pc_scale_command(scale); EXPECT_TRUE(pc_scale_response_future.valid()) << "Scale future invalid!"; + std::cout << "scale: before wait future" << std::endl; pc_scale_response_future.wait(); + std::cout << "scale: after wait future" << std::endl; EXPECT_EQ( pc_scale_response_future.get()->result.value, pc_scale_response_future.get()->result.OK); // Run a bit for scale command to process + std::cout << "scale: before server run" << std::endl; fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); + std::cout << "scale: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 3 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); @@ -363,13 +371,17 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) PCROSNode::PCRetractServiceResponseFuture pc_retract_response_future = node->send_pc_retract_command(retract); EXPECT_TRUE(pc_retract_response_future.valid()) << "Retract future invalid!"; + std::cout << "retract: before wait future" << std::endl; pc_retract_response_future.wait(); + std::cout << "retract: after wait future" << std::endl; EXPECT_EQ( pc_retract_response_future.get()->result.value, pc_retract_response_future.get()->result.OK); // Run a bit for retract command to process + std::cout << "retract: before server run" << std::endl; fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); + std::cout << "retract: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 4 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); @@ -383,9 +395,11 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) /////////////////////////////////////////////////////// // Check Return to Default Winding Current Damping int torque_timeout_iterations{2000}; + std::cout << "wind curr timeout: before server run" << std::endl; fixture->Server()->Run( true /*blocking*/, torque_timeout_iterations - 2 * feedbackCheckIterations, false /*paused*/); + std::cout << "wind curr timeout: after server run" << std::endl; EXPECT_EQ( preCmdIterations + 2 * feedbackCheckIterations + torque_timeout_iterations, iterations); @@ -413,14 +427,18 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) PCROSNode::PCBiasCurrServiceResponseFuture pc_bias_curr_response_future = node->send_pc_bias_curr_command(bc); EXPECT_TRUE(pc_bias_curr_response_future.valid()) << "Bias Current future invalid!"; + std::cout << "bias curr: before wait future" << std::endl; pc_bias_curr_response_future.wait(); + std::cout << "bias curr: after wait future" << std::endl; EXPECT_EQ( pc_bias_curr_response_future.get()->result.value, pc_bias_curr_response_future.get()->result.OK); // Run a bit for bias curr command to move piston int bias_curr_iterations{9000}, bias_curr_timeout_iterations{10000}; + std::cout << "bias curr: before server run" << std::endl; fixture->Server()->Run(true /*blocking*/, bias_curr_iterations, false /*paused*/); + std::cout << "bias curr: after server run" << std::endl; EXPECT_EQ( preCmdIterations + 2 * feedbackCheckIterations + torque_timeout_iterations + bias_curr_iterations, From b800a3adcd18892e051b655ae2e87bb0e65007b5 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 16:08:35 -0800 Subject: [PATCH 15/18] keep debugging Signed-off-by: Michael Anderson --- .github/workflows/build-and-test.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 52f43708..49f4852f 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -51,6 +51,6 @@ 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+ -launch_test install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback.launch.py +colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+ +# launch_test install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback.launch.py colcon test-result From 469aca9f3a066bf264a94f82022664103688ecc9 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 24 Jan 2023 16:50:13 -0800 Subject: [PATCH 16/18] keep debugging Signed-off-by: Michael Anderson --- .github/workflows/build-and-test.sh | 4 ++-- .../PowerController/PowerController.cpp | 24 +++++++++++++++---- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 49f4852f..109cd354 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -51,6 +51,6 @@ 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+ -# launch_test install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback.launch.py +#colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+ +launch_test install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback.launch.py colcon test-result diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 5ea53d1e..25e9403b 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -262,10 +262,20 @@ struct PowerControllerPrivate int8_t result = buoy_interfaces::msg::PBCommandResponse::OK; // high priority cmd access + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] Command waiting for 'next' lock"); std::unique_lock next_lock(services_->next_access_mutex_); + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] Command waiting for 'command' lock"); std::unique_lock cmd_lock(services_->command_mutex_); next_lock.unlock(); + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] Command processing"); + if (valid_range.from_value > command_value || command_value > valid_range.to_value) { @@ -296,7 +306,7 @@ struct PowerControllerPrivate [this](const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_DEBUG_STREAM( + RCLCPP_INFO_STREAM( ros_->node_->get_logger(), "[ROS 2 Power Control] PCWindCurrCommand Received [" << request->wind_curr << " Amps]"); @@ -314,6 +324,9 @@ struct PowerControllerPrivate services_->valid_wind_curr_range_.from_value << ", " << services_->valid_wind_curr_range_.to_value << "] Amps"); } + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] PCWindCurrCommand Done [" << (int)response->result.value << "]"); }; services_->torque_command_service_ = ros_->node_->create_service( @@ -344,7 +357,7 @@ struct PowerControllerPrivate services_->valid_scale_range_.from_value << ", " << services_->valid_scale_range_.to_value << "]"); } - RCLCPP_DEBUG_STREAM( + RCLCPP_INFO_STREAM( ros_->node_->get_logger(), "[ROS 2 Power Control] PCScaleCommand Done [" << (int)response->result.value << "]"); }; @@ -377,7 +390,7 @@ struct PowerControllerPrivate services_->valid_retract_range_.from_value << ", " << services_->valid_retract_range_.to_value << "]"); } - RCLCPP_DEBUG_STREAM( + RCLCPP_INFO_STREAM( ros_->node_->get_logger(), "[ROS 2 Power Control] PCRetractCommand Done [" << (int)response->result.value << "]"); }; @@ -392,7 +405,7 @@ struct PowerControllerPrivate [this](const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_DEBUG_STREAM( + RCLCPP_INFO_STREAM( ros_->node_->get_logger(), "[ROS 2 Power Control] PCBiasCurrCommand Received [" << request->bias_curr << " Amps]"); @@ -410,6 +423,9 @@ struct PowerControllerPrivate services_->valid_bias_curr_range_.from_value << ", " << services_->valid_bias_curr_range_.to_value << "]"); } + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] PCBiasCurrCommand Done [" << (int)response->result.value << "]"); }; services_->bias_curr_command_service_ = ros_->node_->create_service( From c45d4ea2bab298143b7fd0a2871c9637ff299b93 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 25 Jan 2023 10:43:55 -0800 Subject: [PATCH 17/18] remove debugging code Signed-off-by: Michael Anderson --- .github/workflows/build-and-test.sh | 3 +- .../PowerController/PowerController.cpp | 26 +------ buoy_tests/CMakeLists.txt | 4 +- .../launch/pc_commands_ros_feedback.launch.py | 2 +- buoy_tests/tests/pc_commands_ros_feedback.cpp | 70 +++++++------------ 5 files changed, 30 insertions(+), 75 deletions(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 109cd354..4f370844 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -51,6 +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+ -launch_test install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback.launch.py +colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+ colcon test-result diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 25e9403b..6e027b09 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -262,20 +262,10 @@ struct PowerControllerPrivate int8_t result = buoy_interfaces::msg::PBCommandResponse::OK; // high priority cmd access - RCLCPP_INFO_STREAM( - ros_->node_->get_logger(), - "[ROS 2 Power Control] Command waiting for 'next' lock"); std::unique_lock next_lock(services_->next_access_mutex_); - RCLCPP_INFO_STREAM( - ros_->node_->get_logger(), - "[ROS 2 Power Control] Command waiting for 'command' lock"); std::unique_lock cmd_lock(services_->command_mutex_); next_lock.unlock(); - RCLCPP_INFO_STREAM( - ros_->node_->get_logger(), - "[ROS 2 Power Control] Command processing"); - if (valid_range.from_value > command_value || command_value > valid_range.to_value) { @@ -306,7 +296,7 @@ struct PowerControllerPrivate [this](const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( ros_->node_->get_logger(), "[ROS 2 Power Control] PCWindCurrCommand Received [" << request->wind_curr << " Amps]"); @@ -324,9 +314,6 @@ struct PowerControllerPrivate services_->valid_wind_curr_range_.from_value << ", " << services_->valid_wind_curr_range_.to_value << "] Amps"); } - RCLCPP_INFO_STREAM( - ros_->node_->get_logger(), - "[ROS 2 Power Control] PCWindCurrCommand Done [" << (int)response->result.value << "]"); }; services_->torque_command_service_ = ros_->node_->create_service( @@ -357,9 +344,6 @@ struct PowerControllerPrivate services_->valid_scale_range_.from_value << ", " << services_->valid_scale_range_.to_value << "]"); } - RCLCPP_INFO_STREAM( - ros_->node_->get_logger(), - "[ROS 2 Power Control] PCScaleCommand Done [" << (int)response->result.value << "]"); }; services_->scale_command_service_ = ros_->node_->create_service( @@ -390,9 +374,6 @@ struct PowerControllerPrivate services_->valid_retract_range_.from_value << ", " << services_->valid_retract_range_.to_value << "]"); } - RCLCPP_INFO_STREAM( - ros_->node_->get_logger(), - "[ROS 2 Power Control] PCRetractCommand Done [" << (int)response->result.value << "]"); }; services_->retract_command_service_ = ros_->node_->create_service( @@ -405,7 +386,7 @@ struct PowerControllerPrivate [this](const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( ros_->node_->get_logger(), "[ROS 2 Power Control] PCBiasCurrCommand Received [" << request->bias_curr << " Amps]"); @@ -423,9 +404,6 @@ struct PowerControllerPrivate services_->valid_bias_curr_range_.from_value << ", " << services_->valid_bias_curr_range_.to_value << "]"); } - RCLCPP_INFO_STREAM( - ros_->node_->get_logger(), - "[ROS 2 Power Control] PCBiasCurrCommand Done [" << (int)response->result.value << "]"); }; services_->bias_curr_command_service_ = ros_->node_->create_service( diff --git a/buoy_tests/CMakeLists.txt b/buoy_tests/CMakeLists.txt index 59a4cb1d..5c387403 100644 --- a/buoy_tests/CMakeLists.txt +++ b/buoy_tests/CMakeLists.txt @@ -89,7 +89,7 @@ if(BUILD_TESTING) if(buoy_add_gtest_LAUNCH_TEST) add_launch_test(launch/${TEST_NAME}.launch.py - TIMEOUT 1000 + TIMEOUT 650 ) else() ament_add_gtest_test(${TEST_NAME}) @@ -128,7 +128,7 @@ if(BUILD_TESTING) endif() add_launch_test(launch/${PYTEST_NAME}_py.launch.py - TIMEOUT 1000 + TIMEOUT 650 ) endfunction() diff --git a/buoy_tests/launch/pc_commands_ros_feedback.launch.py b/buoy_tests/launch/pc_commands_ros_feedback.launch.py index 28649d6e..b1df2ee5 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback.launch.py @@ -59,7 +59,7 @@ def generate_test_description(): class PCCommandsROSTest(unittest.TestCase): def test_termination(self, gazebo_test_fixture, proc_info): - proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=975) + proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=600) @launch_testing.post_shutdown_test() diff --git a/buoy_tests/tests/pc_commands_ros_feedback.cpp b/buoy_tests/tests/pc_commands_ros_feedback.cpp index 8f25efd9..ffea2655 100644 --- a/buoy_tests/tests/pc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/pc_commands_ros_feedback.cpp @@ -74,10 +74,10 @@ class PCROSNode final : public buoy_api::Interface PBTorqueControlPolicy torque_policy_; - // PCWindCurrServiceResponseFuture pc_wind_curr_response_future_; - // PCBiasCurrServiceResponseFuture pc_bias_curr_response_future_; - // PCScaleServiceResponseFuture pc_scale_response_future_; - // PCRetractServiceResponseFuture pc_retract_response_future_; + PCWindCurrServiceResponseFuture pc_wind_curr_response_future_; + PCBiasCurrServiceResponseFuture pc_bias_curr_response_future_; + PCScaleServiceResponseFuture pc_scale_response_future_; + PCRetractServiceResponseFuture pc_retract_response_future_; explicit PCROSNode(const std::string & node_name) : buoy_api::Interface(node_name) @@ -308,20 +308,15 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) EXPECT_NE(node->wind_curr_, wc); // Now send wind curr command - PCROSNode::PCWindCurrServiceResponseFuture pc_wind_curr_response_future = - node->send_pc_wind_curr_command(wc); - EXPECT_TRUE(pc_wind_curr_response_future.valid()) << "Winding Current future invalid!"; - std::cout << "wind curr: before wait future" << std::endl; - pc_wind_curr_response_future.wait(); - std::cout << "wind curr: after wait future" << std::endl; + node->pc_wind_curr_response_future_ = node->send_pc_wind_curr_command(wc); + EXPECT_TRUE(node->pc_wind_curr_response_future_.valid()) << "Winding Current future invalid!"; + node->pc_wind_curr_response_future_.wait(); EXPECT_EQ( - pc_wind_curr_response_future.get()->result.value, - pc_wind_curr_response_future.get()->result.OK); + node->pc_wind_curr_response_future_.get()->result.value, + node->pc_wind_curr_response_future_.get()->result.OK); // Run a bit for wind curr command to process - std::cout << "wind curr: before server run" << std::endl; fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); - std::cout << "wind curr: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 2 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); @@ -338,20 +333,15 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) EXPECT_NE(node->scale_, scale); // Now send scale command - PCROSNode::PCScaleServiceResponseFuture pc_scale_response_future = - node->send_pc_scale_command(scale); - EXPECT_TRUE(pc_scale_response_future.valid()) << "Scale future invalid!"; - std::cout << "scale: before wait future" << std::endl; - pc_scale_response_future.wait(); - std::cout << "scale: after wait future" << std::endl; + node->pc_scale_response_future_ = node->send_pc_scale_command(scale); + EXPECT_TRUE(node->pc_scale_response_future_.valid()) << "Scale future invalid!"; + node->pc_scale_response_future_.wait(); EXPECT_EQ( - pc_scale_response_future.get()->result.value, - pc_scale_response_future.get()->result.OK); + node->pc_scale_response_future_.get()->result.value, + node->pc_scale_response_future_.get()->result.OK); // Run a bit for scale command to process - std::cout << "scale: before server run" << std::endl; fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); - std::cout << "scale: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 3 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); @@ -368,20 +358,15 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) EXPECT_NE(node->retract_, retract); // Now send retract command - PCROSNode::PCRetractServiceResponseFuture pc_retract_response_future = - node->send_pc_retract_command(retract); - EXPECT_TRUE(pc_retract_response_future.valid()) << "Retract future invalid!"; - std::cout << "retract: before wait future" << std::endl; - pc_retract_response_future.wait(); - std::cout << "retract: after wait future" << std::endl; + node->pc_retract_response_future_ = node->send_pc_retract_command(retract); + EXPECT_TRUE(node->pc_retract_response_future_.valid()) << "Retract future invalid!"; + node->pc_retract_response_future_.wait(); EXPECT_EQ( - pc_retract_response_future.get()->result.value, - pc_retract_response_future.get()->result.OK); + node->pc_retract_response_future_.get()->result.value, + node->pc_retract_response_future_.get()->result.OK); // Run a bit for retract command to process - std::cout << "retract: before server run" << std::endl; fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); - std::cout << "retract: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 4 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); @@ -395,11 +380,9 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) /////////////////////////////////////////////////////// // Check Return to Default Winding Current Damping int torque_timeout_iterations{2000}; - std::cout << "wind curr timeout: before server run" << std::endl; fixture->Server()->Run( true /*blocking*/, torque_timeout_iterations - 2 * feedbackCheckIterations, false /*paused*/); - std::cout << "wind curr timeout: after server run" << std::endl; EXPECT_EQ( preCmdIterations + 2 * feedbackCheckIterations + torque_timeout_iterations, iterations); @@ -424,21 +407,16 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) EXPECT_NE(node->bias_curr_, bc); // Now send bias curr command - PCROSNode::PCBiasCurrServiceResponseFuture pc_bias_curr_response_future = - node->send_pc_bias_curr_command(bc); - EXPECT_TRUE(pc_bias_curr_response_future.valid()) << "Bias Current future invalid!"; - std::cout << "bias curr: before wait future" << std::endl; - pc_bias_curr_response_future.wait(); - std::cout << "bias curr: after wait future" << std::endl; + node->pc_bias_curr_response_future_ = node->send_pc_bias_curr_command(bc); + EXPECT_TRUE(node->pc_bias_curr_response_future_.valid()) << "Bias Current future invalid!"; + node->pc_bias_curr_response_future_.wait(); EXPECT_EQ( - pc_bias_curr_response_future.get()->result.value, - pc_bias_curr_response_future.get()->result.OK); + node->pc_bias_curr_response_future_.get()->result.value, + node->pc_bias_curr_response_future_.get()->result.OK); // Run a bit for bias curr command to move piston int bias_curr_iterations{9000}, bias_curr_timeout_iterations{10000}; - std::cout << "bias curr: before server run" << std::endl; fixture->Server()->Run(true /*blocking*/, bias_curr_iterations, false /*paused*/); - std::cout << "bias curr: after server run" << std::endl; EXPECT_EQ( preCmdIterations + 2 * feedbackCheckIterations + torque_timeout_iterations + bias_curr_iterations, From 34f2855a91a87c98d2ca31ad3ccc64b030915169 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 25 Jan 2023 11:31:07 -0800 Subject: [PATCH 18/18] rebase to show diff with #125 Signed-off-by: Michael Anderson --- .github/workflows/build-and-test.sh | 3 +- .../PowerController/PowerController.cpp | 26 ++++++- buoy_tests/CMakeLists.txt | 4 +- .../launch/pc_commands_ros_feedback.launch.py | 2 +- buoy_tests/tests/pc_commands_ros_feedback.cpp | 70 ++++++++++++------- 5 files changed, 75 insertions(+), 30 deletions(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 4f370844..109cd354 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -51,5 +51,6 @@ 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+ +launch_test install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback.launch.py colcon test-result diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 6e027b09..25e9403b 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -262,10 +262,20 @@ struct PowerControllerPrivate int8_t result = buoy_interfaces::msg::PBCommandResponse::OK; // high priority cmd access + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] Command waiting for 'next' lock"); std::unique_lock next_lock(services_->next_access_mutex_); + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] Command waiting for 'command' lock"); std::unique_lock cmd_lock(services_->command_mutex_); next_lock.unlock(); + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] Command processing"); + if (valid_range.from_value > command_value || command_value > valid_range.to_value) { @@ -296,7 +306,7 @@ struct PowerControllerPrivate [this](const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_DEBUG_STREAM( + RCLCPP_INFO_STREAM( ros_->node_->get_logger(), "[ROS 2 Power Control] PCWindCurrCommand Received [" << request->wind_curr << " Amps]"); @@ -314,6 +324,9 @@ struct PowerControllerPrivate services_->valid_wind_curr_range_.from_value << ", " << services_->valid_wind_curr_range_.to_value << "] Amps"); } + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] PCWindCurrCommand Done [" << (int)response->result.value << "]"); }; services_->torque_command_service_ = ros_->node_->create_service( @@ -344,6 +357,9 @@ struct PowerControllerPrivate services_->valid_scale_range_.from_value << ", " << services_->valid_scale_range_.to_value << "]"); } + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] PCScaleCommand Done [" << (int)response->result.value << "]"); }; services_->scale_command_service_ = ros_->node_->create_service( @@ -374,6 +390,9 @@ struct PowerControllerPrivate services_->valid_retract_range_.from_value << ", " << services_->valid_retract_range_.to_value << "]"); } + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] PCRetractCommand Done [" << (int)response->result.value << "]"); }; services_->retract_command_service_ = ros_->node_->create_service( @@ -386,7 +405,7 @@ struct PowerControllerPrivate [this](const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_DEBUG_STREAM( + RCLCPP_INFO_STREAM( ros_->node_->get_logger(), "[ROS 2 Power Control] PCBiasCurrCommand Received [" << request->bias_curr << " Amps]"); @@ -404,6 +423,9 @@ struct PowerControllerPrivate services_->valid_bias_curr_range_.from_value << ", " << services_->valid_bias_curr_range_.to_value << "]"); } + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] PCBiasCurrCommand Done [" << (int)response->result.value << "]"); }; services_->bias_curr_command_service_ = ros_->node_->create_service( diff --git a/buoy_tests/CMakeLists.txt b/buoy_tests/CMakeLists.txt index 5c387403..59a4cb1d 100644 --- a/buoy_tests/CMakeLists.txt +++ b/buoy_tests/CMakeLists.txt @@ -89,7 +89,7 @@ if(BUILD_TESTING) if(buoy_add_gtest_LAUNCH_TEST) add_launch_test(launch/${TEST_NAME}.launch.py - TIMEOUT 650 + TIMEOUT 1000 ) else() ament_add_gtest_test(${TEST_NAME}) @@ -128,7 +128,7 @@ if(BUILD_TESTING) endif() add_launch_test(launch/${PYTEST_NAME}_py.launch.py - TIMEOUT 650 + TIMEOUT 1000 ) endfunction() diff --git a/buoy_tests/launch/pc_commands_ros_feedback.launch.py b/buoy_tests/launch/pc_commands_ros_feedback.launch.py index b1df2ee5..28649d6e 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback.launch.py @@ -59,7 +59,7 @@ def generate_test_description(): class PCCommandsROSTest(unittest.TestCase): def test_termination(self, gazebo_test_fixture, proc_info): - proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=600) + proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=975) @launch_testing.post_shutdown_test() diff --git a/buoy_tests/tests/pc_commands_ros_feedback.cpp b/buoy_tests/tests/pc_commands_ros_feedback.cpp index ffea2655..8f25efd9 100644 --- a/buoy_tests/tests/pc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/pc_commands_ros_feedback.cpp @@ -74,10 +74,10 @@ class PCROSNode final : public buoy_api::Interface PBTorqueControlPolicy torque_policy_; - PCWindCurrServiceResponseFuture pc_wind_curr_response_future_; - PCBiasCurrServiceResponseFuture pc_bias_curr_response_future_; - PCScaleServiceResponseFuture pc_scale_response_future_; - PCRetractServiceResponseFuture pc_retract_response_future_; + // PCWindCurrServiceResponseFuture pc_wind_curr_response_future_; + // PCBiasCurrServiceResponseFuture pc_bias_curr_response_future_; + // PCScaleServiceResponseFuture pc_scale_response_future_; + // PCRetractServiceResponseFuture pc_retract_response_future_; explicit PCROSNode(const std::string & node_name) : buoy_api::Interface(node_name) @@ -308,15 +308,20 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) EXPECT_NE(node->wind_curr_, wc); // Now send wind curr command - node->pc_wind_curr_response_future_ = node->send_pc_wind_curr_command(wc); - EXPECT_TRUE(node->pc_wind_curr_response_future_.valid()) << "Winding Current future invalid!"; - node->pc_wind_curr_response_future_.wait(); + PCROSNode::PCWindCurrServiceResponseFuture pc_wind_curr_response_future = + node->send_pc_wind_curr_command(wc); + EXPECT_TRUE(pc_wind_curr_response_future.valid()) << "Winding Current future invalid!"; + std::cout << "wind curr: before wait future" << std::endl; + pc_wind_curr_response_future.wait(); + std::cout << "wind curr: after wait future" << std::endl; EXPECT_EQ( - node->pc_wind_curr_response_future_.get()->result.value, - node->pc_wind_curr_response_future_.get()->result.OK); + pc_wind_curr_response_future.get()->result.value, + pc_wind_curr_response_future.get()->result.OK); // Run a bit for wind curr command to process + std::cout << "wind curr: before server run" << std::endl; fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); + std::cout << "wind curr: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 2 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); @@ -333,15 +338,20 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) EXPECT_NE(node->scale_, scale); // Now send scale command - node->pc_scale_response_future_ = node->send_pc_scale_command(scale); - EXPECT_TRUE(node->pc_scale_response_future_.valid()) << "Scale future invalid!"; - node->pc_scale_response_future_.wait(); + PCROSNode::PCScaleServiceResponseFuture pc_scale_response_future = + node->send_pc_scale_command(scale); + EXPECT_TRUE(pc_scale_response_future.valid()) << "Scale future invalid!"; + std::cout << "scale: before wait future" << std::endl; + pc_scale_response_future.wait(); + std::cout << "scale: after wait future" << std::endl; EXPECT_EQ( - node->pc_scale_response_future_.get()->result.value, - node->pc_scale_response_future_.get()->result.OK); + pc_scale_response_future.get()->result.value, + pc_scale_response_future.get()->result.OK); // Run a bit for scale command to process + std::cout << "scale: before server run" << std::endl; fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); + std::cout << "scale: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 3 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); @@ -358,15 +368,20 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) EXPECT_NE(node->retract_, retract); // Now send retract command - node->pc_retract_response_future_ = node->send_pc_retract_command(retract); - EXPECT_TRUE(node->pc_retract_response_future_.valid()) << "Retract future invalid!"; - node->pc_retract_response_future_.wait(); + PCROSNode::PCRetractServiceResponseFuture pc_retract_response_future = + node->send_pc_retract_command(retract); + EXPECT_TRUE(pc_retract_response_future.valid()) << "Retract future invalid!"; + std::cout << "retract: before wait future" << std::endl; + pc_retract_response_future.wait(); + std::cout << "retract: after wait future" << std::endl; EXPECT_EQ( - node->pc_retract_response_future_.get()->result.value, - node->pc_retract_response_future_.get()->result.OK); + pc_retract_response_future.get()->result.value, + pc_retract_response_future.get()->result.OK); // Run a bit for retract command to process + std::cout << "retract: before server run" << std::endl; fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/); + std::cout << "retract: after server run" << std::endl; EXPECT_EQ(preCmdIterations + 4 * feedbackCheckIterations, iterations); std::this_thread::sleep_for(500ms); @@ -380,9 +395,11 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) /////////////////////////////////////////////////////// // Check Return to Default Winding Current Damping int torque_timeout_iterations{2000}; + std::cout << "wind curr timeout: before server run" << std::endl; fixture->Server()->Run( true /*blocking*/, torque_timeout_iterations - 2 * feedbackCheckIterations, false /*paused*/); + std::cout << "wind curr timeout: after server run" << std::endl; EXPECT_EQ( preCmdIterations + 2 * feedbackCheckIterations + torque_timeout_iterations, iterations); @@ -407,16 +424,21 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) EXPECT_NE(node->bias_curr_, bc); // Now send bias curr command - node->pc_bias_curr_response_future_ = node->send_pc_bias_curr_command(bc); - EXPECT_TRUE(node->pc_bias_curr_response_future_.valid()) << "Bias Current future invalid!"; - node->pc_bias_curr_response_future_.wait(); + PCROSNode::PCBiasCurrServiceResponseFuture pc_bias_curr_response_future = + node->send_pc_bias_curr_command(bc); + EXPECT_TRUE(pc_bias_curr_response_future.valid()) << "Bias Current future invalid!"; + std::cout << "bias curr: before wait future" << std::endl; + pc_bias_curr_response_future.wait(); + std::cout << "bias curr: after wait future" << std::endl; EXPECT_EQ( - node->pc_bias_curr_response_future_.get()->result.value, - node->pc_bias_curr_response_future_.get()->result.OK); + pc_bias_curr_response_future.get()->result.value, + pc_bias_curr_response_future.get()->result.OK); // Run a bit for bias curr command to move piston int bias_curr_iterations{9000}, bias_curr_timeout_iterations{10000}; + std::cout << "bias curr: before server run" << std::endl; fixture->Server()->Run(true /*blocking*/, bias_curr_iterations, false /*paused*/); + std::cout << "bias curr: after server run" << std::endl; EXPECT_EQ( preCmdIterations + 2 * feedbackCheckIterations + torque_timeout_iterations + bias_curr_iterations,