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_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 77bf5b2a..cdb2bea2 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 +}@ + @@ -11,6 +18,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 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..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..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/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..935746df --- /dev/null +++ b/buoy_gazebo/scripts/example_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.4] diff --git a/buoy_gazebo/scripts/mbari_wec_batch b/buoy_gazebo/scripts/mbari_wec_batch new file mode 100755 index 00000000..b91e0c4b --- /dev/null +++ b/buoy_gazebo/scripts/mbari_wec_batch @@ -0,0 +1,107 @@ +#!/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 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 + + +def generate_simulations(sim_params_yaml): + with open(sim_params_yaml, 'r') as fd: + sim_params = yaml.load(fd, Loader=yaml.CLoader) + + 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(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') + + 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(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, + 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}'}.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 + ]) + + 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 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) 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_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/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..28649d6e 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', @@ -58,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/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); } 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,