From f5443fa2b20d5fa50b6c9e70d10b9283904ae6ca Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 17 Aug 2022 14:52:34 -0700 Subject: [PATCH 01/57] Upgrade to Humble + Garden Signed-off-by: Louise Poubel --- .github/workflows/build-and-test.sh | 6 ++ .github/workflows/ci.yml | 8 +- README.md | 2 +- .../models/mbari_wec_base/model.sdf.em | 6 +- .../models/mbari_wec_ros/model.sdf | 12 +-- buoy_description/package.xml | 2 +- buoy_gazebo/CMakeLists.txt | 26 ++--- buoy_gazebo/gazebo/server.config | 24 ++--- .../launch/electrohydraulic_pto.launch.py | 2 +- buoy_gazebo/launch/mbari_wec.launch.py | 8 +- .../launch/mbari_wec_debugger.launch.py | 2 +- buoy_gazebo/package.xml | 2 +- .../ElectroHydraulicPTO.cpp | 100 +++++++++--------- .../ElectroHydraulicPTO.hpp | 26 ++--- .../ElectroHydraulicState.hpp | 8 +- .../PolytropicPneumaticSpring/CMakeLists.txt | 6 +- .../PolytropicPneumaticSpring.cpp | 98 ++++++++--------- .../PolytropicPneumaticSpring.hpp | 24 ++--- .../PolytropicPneumaticSpring/SpringState.hpp | 8 +- .../PowerController/PowerController.cpp | 44 ++++---- .../PowerController/PowerController.hpp | 24 ++--- .../ServicesNotImplemented/NoOpController.cpp | 20 ++-- .../ServicesNotImplemented/NoOpController.hpp | 12 +-- .../SpringController/SpringController.cpp | 48 ++++----- .../SpringController/SpringController.hpp | 24 ++--- .../src/controllers/XBowAHRS/XBowAHRS.cpp | 48 ++++----- .../src/controllers/XBowAHRS/XBowAHRS.hpp | 18 ++-- buoy_gazebo/worlds/buoy_playground.sdf | 72 ++++++------- buoy_tests/CMakeLists.txt | 8 +- buoy_tests/launch/no_inputs_py.launch.py | 2 +- .../launch/no_inputs_ros_feedback.launch.py | 2 +- .../launch/pc_commands_ros_feedback.launch.py | 2 +- buoy_tests/launch/run_server.launch.py | 2 +- .../launch/sc_commands_ros_feedback.launch.py | 2 +- buoy_tests/package.xml | 2 +- buoy_tests/testing_utils/utils.py | 6 +- buoy_tests/tests/fixture_server.cpp | 38 +++---- buoy_tests/tests/gravity.sdf | 4 +- buoy_tests/tests/no_inputs.cpp | 38 +++---- buoy_tests/tests/no_inputs_ros_feedback.cpp | 38 +++---- buoy_tests/tests/pc_commands_ros_feedback.cpp | 40 +++---- buoy_tests/tests/sc_commands_ros_feedback.cpp | 42 ++++---- buoy_tests/tests/test_fixture.py | 10 +- 43 files changed, 461 insertions(+), 455 deletions(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index e86747fe..5794755f 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -11,10 +11,16 @@ mkdir -p $COLCON_WS_SRC apt update -qq apt install -qq -y lsb-release wget curl build-essential +# Garden only has nightlies for now +echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list +echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list +wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - + echo "deb http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - apt-get update -qq apt-get install -y git \ + gz-garden \ python3-colcon-common-extensions \ python3-rosdep \ python3-vcstool \ diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 328a8f3c..d5d85d08 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -5,14 +5,14 @@ on: [push, pull_request] jobs: tests: name: Build and test - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 strategy: matrix: include: - - gz-version: "fortress" - ros-distro: "galactic" + - gz-version: "garden" + ros-distro: "humble" container: - image: ubuntu:20.04 + image: ubuntu:22.04 steps: - name: Checkout uses: actions/checkout@v2 diff --git a/README.md b/README.md index 0099441c..1ad3ca89 100644 --- a/README.md +++ b/README.md @@ -14,4 +14,4 @@ and enable the profiler with `colcon`: colcon build --cmake-args ' -DBUILD_TESTING=false' ' -DENABLE_PROFILER=1' ``` -See more on [this tutorial](https://ignitionrobotics.org/api/common/4.4/profiler.html). +See more on [this tutorial](https://gazebosim.org/api/common/4.4/profiler.html). diff --git a/buoy_description/models/mbari_wec_base/model.sdf.em b/buoy_description/models/mbari_wec_base/model.sdf.em index f0d21744..bdb8f524 100644 --- a/buoy_description/models/mbari_wec_base/model.sdf.em +++ b/buoy_description/models/mbari_wec_base/model.sdf.em @@ -1,8 +1,8 @@ @{ -from ignition.math import Cylinderd -from ignition.math import MassMatrix3d -from ignition.math import Material +from gz.math import Cylinderd +from gz.math import MassMatrix3d +from gz.math import Material import math ############## diff --git a/buoy_description/models/mbari_wec_ros/model.sdf b/buoy_description/models/mbari_wec_ros/model.sdf index 0784ee32..cf049992 100644 --- a/buoy_description/models/mbari_wec_ros/model.sdf +++ b/buoy_description/models/mbari_wec_ros/model.sdf @@ -37,13 +37,13 @@ + filename="gz-sim-joint-state-publisher-system" + name="gz::sim::systems::JointStatePublisher"> + filename="gz-sim-pose-publisher-system" + name="gz::sim::systems::PosePublisher"> true true true @@ -51,8 +51,8 @@ + filename="gz-sim-odometry-publisher-system" + name="gz::sim::systems::OdometryPublisher"> 3 MBARI_WEC_ROS/odom MBARI_WEC_ROS diff --git a/buoy_description/package.xml b/buoy_description/package.xml index 09f3037c..02eda0b6 100644 --- a/buoy_description/package.xml +++ b/buoy_description/package.xml @@ -11,7 +11,7 @@ ament_cmake python3-empy - python3-ignition-math6 + python3-gz-math7 ament_lint_auto ament_lint_common diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt index c2805a21..4dd6fabf 100644 --- a/buoy_gazebo/CMakeLists.txt +++ b/buoy_gazebo/CMakeLists.txt @@ -6,25 +6,25 @@ project(buoy_gazebo) # endif() # Option to enable profiler -option(ENABLE_PROFILER "Enable Ignition Profiler" FALSE) +option(ENABLE_PROFILER "Enable Gazebo Profiler" FALSE) if(ENABLE_PROFILER) - add_definitions("-DIGN_PROFILER_ENABLE=1") + add_definitions("-DGZ_PROFILER_ENABLE=1") else() - add_definitions("-DIGN_PROFILER_ENABLE=0") + add_definitions("-DGZ_PROFILER_ENABLE=0") endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(buoy_msgs REQUIRED) -find_package(ignition-cmake2 REQUIRED) -find_package(ignition-plugin1 REQUIRED COMPONENTS register) -set(GZ_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) -find_package(ignition-common4 REQUIRED COMPONENTS profiler) -set(GZ_COMMON_VER ${ignition-common4_VERSION_MAJOR}) -find_package(ignition-gazebo6 REQUIRED) -set(GZ_SIM_VER ${ignition-gazebo6_VERSION_MAJOR}) +find_package(gz-cmake3 REQUIRED) +find_package(gz-plugin2 REQUIRED COMPONENTS register) +set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) +find_package(gz-common4 REQUIRED COMPONENTS profiler) +set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) +find_package(gz-sim7 REQUIRED) +set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR}) # Plugins @@ -68,9 +68,9 @@ function(gz_add_plugin plugin_name) ) target_link_libraries(${plugin_name} PUBLIC - ignition-gazebo${GZ_SIM_VER}::ignition-gazebo${GZ_SIM_VER} - ignition-plugin${GZ_PLUGIN_VER}::ignition-plugin${GZ_PLUGIN_VER} - ignition-common${GZ_COMMON_VER}::profiler + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} + gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} + gz-common${GZ_COMMON_VER}::profiler ${gz_add_plugin_PUBLIC_LINK_LIBS} PRIVATE ${gz_add_plugin_PRIVATE_LINK_LIBS} diff --git a/buoy_gazebo/gazebo/server.config b/buoy_gazebo/gazebo/server.config index 832cf59e..6189fa9b 100644 --- a/buoy_gazebo/gazebo/server.config +++ b/buoy_gazebo/gazebo/server.config @@ -3,23 +3,23 @@ + filename="gz-sim-physics-system" + name="gz::sim::systems::Physics"> + filename="gz-sim-user-commands-system" + name="gz::sim::systems::UserCommands"> + filename="gz-sim-scene-broadcaster-system" + name="gz::sim::systems::SceneBroadcaster"> + filename="gz-sim-buoyancy-system" + name="gz::sim::systems::Buoyancy"> 1025 @@ -30,13 +30,13 @@ + filename="gz-sim-forcetorque-system" + name="gz::sim::systems::ForceTorque"> + filename="gz-sim-imu-system" + name="gz::sim::systems::Imu"> diff --git a/buoy_gazebo/launch/electrohydraulic_pto.launch.py b/buoy_gazebo/launch/electrohydraulic_pto.launch.py index ad7a62f0..a0318e29 100644 --- a/buoy_gazebo/launch/electrohydraulic_pto.launch.py +++ b/buoy_gazebo/launch/electrohydraulic_pto.launch.py @@ -34,7 +34,7 @@ def generate_launch_description(): # For now, each field is published in its own topic as a double / float64 ros_msg_type = 'std_msgs/msg/Float64' - gz_msg_type = 'ignition.msgs.Double' + gz_msg_type = 'gz.msgs.Double' joint_name = 'HydraulicRam' model_name = 'Hydraulics_Test' diff --git a/buoy_gazebo/launch/mbari_wec.launch.py b/buoy_gazebo/launch/mbari_wec.launch.py index 4f728203..0f0a4b0f 100644 --- a/buoy_gazebo/launch/mbari_wec.launch.py +++ b/buoy_gazebo/launch/mbari_wec.launch.py @@ -70,13 +70,13 @@ def generate_launch_description(): executable='parameter_bridge', arguments=[ # Clock (Gazebo -> ROS2) - '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock', + '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock', # Joint states (Gazebo -> ROS2) ['/world/', LaunchConfiguration('world_name'), '/model/', model_name, '/joint_state', - '@', 'sensor_msgs/msg/JointState', '[', 'ignition.msgs.Model'], + '@', 'sensor_msgs/msg/JointState', '[', 'gz.msgs.Model'], # Link poses (Gazebo -> ROS2) - link_pose_gz_topic + '@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V', - link_pose_gz_topic + '_static@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V', + link_pose_gz_topic + '@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V', + link_pose_gz_topic + '_static@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V', ], remappings=[ (['/world/', LaunchConfiguration('world_name'), '/model/', model_name, '/joint_state'], diff --git a/buoy_gazebo/launch/mbari_wec_debugger.launch.py b/buoy_gazebo/launch/mbari_wec_debugger.launch.py index 50b2f33f..193c177b 100644 --- a/buoy_gazebo/launch/mbari_wec_debugger.launch.py +++ b/buoy_gazebo/launch/mbari_wec_debugger.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') return LaunchDescription([ diff --git a/buoy_gazebo/package.xml b/buoy_gazebo/package.xml index 66aa1fe0..59cc8048 100644 --- a/buoy_gazebo/package.xml +++ b/buoy_gazebo/package.xml @@ -13,7 +13,7 @@ buoy_description buoy_examples buoy_msgs - ignition-gazebo6 + gz-sim7 robot_state_publisher ros_ign_bridge ros_ign_gazebo diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index f02f4589..17e186fd 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -13,21 +13,21 @@ // limitations under the License. #include "ElectroHydraulicPTO.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include @@ -51,7 +51,7 @@ class ElectroHydraulicPTOPrivate { public: /// \brief Piston joint entity - ignition::gazebo::Entity PrismaticJointEntity{ignition::gazebo::kNullEntity}; + gz::sim::Entity PrismaticJointEntity{gz::sim::kNullEntity}; /// \brief Piston area double PistonArea{1.0}; @@ -60,7 +60,7 @@ class ElectroHydraulicPTOPrivate double RotorInertia{1.0}; /// \brief Model interface - ignition::gazebo::Model model{ignition::gazebo::kNullEntity}; + gz::sim::Model model{gz::sim::kNullEntity}; ElectroHydraulicSoln functor{}; @@ -75,7 +75,7 @@ class ElectroHydraulicPTOPrivate bool VelMode{false}; /// \brief Ignition communication node. - ignition::transport::Node node; + gz::transport::Node node; }; ////////////////////////////////////////////////// @@ -97,12 +97,12 @@ double SdfParamDouble( ////////////////////////////////////////////////// void ElectroHydraulicPTO::Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & /*_eventMgr*/) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & /*_eventMgr*/) { - this->dataPtr->model = ignition::gazebo::Model(_entity); + this->dataPtr->model = gz::sim::Model(_entity); if (!this->dataPtr->model.Valid(_ecm)) { ignerr << "ElectroHydraulicPTO plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; @@ -122,7 +122,7 @@ void ElectroHydraulicPTO::Configure( this->dataPtr->PrismaticJointEntity = this->dataPtr->model.JointByName( _ecm, PrismaticJointName); - if (this->dataPtr->PrismaticJointEntity == ignition::gazebo::kNullEntity) { + if (this->dataPtr->PrismaticJointEntity == gz::sim::kNullEntity) { ignerr << "Joint with name [" << PrismaticJointName << "] not found. " << "The ElectroHydraulicPTO may not influence this joint.\n"; return; @@ -143,63 +143,63 @@ void ElectroHydraulicPTO::Configure( std::string pistonvel_topic = std::string("/pistonvel_") + PrismaticJointName; - pistonvel_pub = node.Advertise(pistonvel_topic); + pistonvel_pub = node.Advertise(pistonvel_topic); if (!pistonvel_pub) { ignerr << "Error advertising topic [" << pistonvel_topic << "]" << std::endl; return; } std::string rpm_topic = std::string("/rpm_") + PrismaticJointName; - rpm_pub = node.Advertise(rpm_topic); + rpm_pub = node.Advertise(rpm_topic); if (!rpm_pub) { ignerr << "Error advertising topic [" << rpm_topic << "]" << std::endl; return; } std::string deltaP_topic = std::string("/deltaP_") + PrismaticJointName; - deltaP_pub = node.Advertise(deltaP_topic); + deltaP_pub = node.Advertise(deltaP_topic); if (!deltaP_pub) { ignerr << "Error advertising topic [" << deltaP_topic << "]" << std::endl; return; } std::string targwindcurr_topic = std::string("/targwindcurr_") + PrismaticJointName; - targwindcurr_pub = node.Advertise(targwindcurr_topic); + targwindcurr_pub = node.Advertise(targwindcurr_topic); if (!targwindcurr_pub) { ignerr << "Error advertising topic [" << targwindcurr_topic << "]" << std::endl; return; } std::string windcurr_topic = std::string("/windcurr_") + PrismaticJointName; - windcurr_pub = node.Advertise(windcurr_topic); + windcurr_pub = node.Advertise(windcurr_topic); if (!windcurr_pub) { ignerr << "Error advertising topic [" << windcurr_topic << "]" << std::endl; return; } std::string battcurr_topic = std::string("/battcurr_") + PrismaticJointName; - battcurr_pub = node.Advertise(battcurr_topic); + battcurr_pub = node.Advertise(battcurr_topic); if (!battcurr_pub) { ignerr << "Error advertising topic [" << battcurr_topic << "]" << std::endl; return; } std::string loadcurr_topic = std::string("/loadcurr_") + PrismaticJointName; - loadcurr_pub = node.Advertise(loadcurr_topic); + loadcurr_pub = node.Advertise(loadcurr_topic); if (!loadcurr_pub) { ignerr << "Error advertising topic [" << loadcurr_topic << "]" << std::endl; return; } std::string scalefactor_topic = std::string("/scalefactor_") + PrismaticJointName; - scalefactor_pub = node.Advertise(scalefactor_topic); + scalefactor_pub = node.Advertise(scalefactor_topic); if (!scalefactor_pub) { ignerr << "Error advertising topic [" << scalefactor_topic << "]" << std::endl; return; } std::string retractfactor_topic = std::string("/retractfactor_") + PrismaticJointName; - retractfactor_pub = node.Advertise(retractfactor_topic); + retractfactor_pub = node.Advertise(retractfactor_topic); if (!retractfactor_pub) { ignerr << "Error advertising topic [" << retractfactor_topic << "]" << std::endl; return; @@ -208,8 +208,8 @@ void ElectroHydraulicPTO::Configure( ////////////////////////////////////////////////// void ElectroHydraulicPTO::PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) { IGN_PROFILE("#ElectroHydraulicPTO::PreUpdate"); // Nothing left to do if paused. @@ -220,7 +220,7 @@ void ElectroHydraulicPTO::PreUpdate( auto SimTime = std::chrono::duration(_info.simTime).count(); // If the joints haven't been identified yet, the plugin is disabled - if (this->dataPtr->PrismaticJointEntity == ignition::gazebo::kNullEntity) { + if (this->dataPtr->PrismaticJointEntity == gz::sim::kNullEntity) { return; } @@ -233,11 +233,11 @@ void ElectroHydraulicPTO::PreUpdate( // Create joint velocity component for piston if one doesn't exist - auto prismaticJointVelComp = _ecm.Component( + auto prismaticJointVelComp = _ecm.Component( this->dataPtr->PrismaticJointEntity); if (prismaticJointVelComp == nullptr) { _ecm.CreateComponent( - this->dataPtr->PrismaticJointEntity, ignition::gazebo::components::JointVelocity()); + this->dataPtr->PrismaticJointEntity, gz::sim::components::JointVelocity()); } // We just created the joint velocity component, give one iteration for the // physics system to update its size @@ -370,41 +370,41 @@ void ElectroHydraulicPTO::PreUpdate( pto_state); - auto stampMsg = ignition::gazebo::convert(_info.simTime); + auto stampMsg = gz::sim::convert(_info.simTime); - ignition::msgs::Double pistonvel; + gz::msgs::Double pistonvel; pistonvel.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); pistonvel.set_data(xdot); - ignition::msgs::Double rpm; + gz::msgs::Double rpm; rpm.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); rpm.set_data(N); - ignition::msgs::Double deltap; + gz::msgs::Double deltap; deltap.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); deltap.set_data(deltaP); - ignition::msgs::Double targwindcurr; + gz::msgs::Double targwindcurr; targwindcurr.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); targwindcurr.set_data(this->dataPtr->TargetWindingCurrent); - ignition::msgs::Double windcurr; + gz::msgs::Double windcurr; windcurr.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); windcurr.set_data(this->dataPtr->WindingCurrent); - ignition::msgs::Double battcurr; + gz::msgs::Double battcurr; battcurr.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); battcurr.set_data(I_Batt); - ignition::msgs::Double loadcurr; + gz::msgs::Double loadcurr; loadcurr.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); loadcurr.set_data(I_Load); - ignition::msgs::Double scalefactor; + gz::msgs::Double scalefactor; scalefactor.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); scalefactor.set_data(this->dataPtr->functor.I_Wind.ScaleFactor); - ignition::msgs::Double retractfactor; + gz::msgs::Double retractfactor; retractfactor.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); retractfactor.set_data(this->dataPtr->functor.I_Wind.RetractFactor); @@ -450,12 +450,12 @@ void ElectroHydraulicPTO::PreUpdate( if (!this->dataPtr->VelMode) { double piston_force = deltaP * this->dataPtr->PistonArea; // Create new component for this entitiy in ECM (if it doesn't already exist) - auto forceComp = _ecm.Component( + auto forceComp = _ecm.Component( this->dataPtr->PrismaticJointEntity); if (forceComp == nullptr) { _ecm.CreateComponent( this->dataPtr->PrismaticJointEntity, - ignition::gazebo::components::JointForceCmd({piston_force})); // Create this iteration + gz::sim::components::JointForceCmd({piston_force})); // Create this iteration } else { forceComp->Data()[0] += piston_force; // Add force to existing forces. } @@ -465,6 +465,6 @@ void ElectroHydraulicPTO::PreUpdate( IGNITION_ADD_PLUGIN( buoy_gazebo::ElectroHydraulicPTO, - ignition::gazebo::System, + gz::sim::System, buoy_gazebo::ElectroHydraulicPTO::ISystemConfigure, buoy_gazebo::ElectroHydraulicPTO::ISystemPreUpdate); diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.hpp index 98605651..21568ca3 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.hpp @@ -17,8 +17,8 @@ #include -#include -#include +#include +#include #include namespace buoy_gazebo @@ -27,7 +27,7 @@ namespace buoy_gazebo class ElectroHydraulicPTOPrivate; /// \brief To use, several parameters are required. -/// Two ignition gazebo joints that are either prismatic or continous with 1DOF each, +/// Two Gazebo joints that are either prismatic or continous with 1DOF each, /// a desciption of the connectoins between actuator (joints), /// and an oil characteristic specification. /// @@ -47,9 +47,9 @@ class ElectroHydraulicPTOPrivate; /// For each actuator of revolute type, the following nested tags are required: /// /// Displacement per revolution of rotary pump/motor. -class ElectroHydraulicPTO : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPreUpdate +class ElectroHydraulicPTO : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate { public: /// \brief Constructor @@ -60,19 +60,19 @@ class ElectroHydraulicPTO : public ignition::gazebo::System, // Documentation inherited void Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & _eventMgr) override; + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; // Documentation inherited void PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) override; + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) override; private: - ignition::transport::Node node; - ignition::transport::Node::Publisher pistonvel_pub, rpm_pub, deltaP_pub, targwindcurr_pub, + gz::transport::Node node; + gz::transport::Node::Publisher pistonvel_pub, rpm_pub, deltaP_pub, targwindcurr_pub, windcurr_pub, battcurr_pub, loadcurr_pub, scalefactor_pub, retractfactor_pub; diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicState.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicState.hpp index c1ea3d15..27efd988 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicState.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicState.hpp @@ -15,9 +15,9 @@ #ifndef ELECTROHYDRAULICPTO__ELECTROHYDRAULICSTATE_HPP_ #define ELECTROHYDRAULICPTO__ELECTROHYDRAULICSTATE_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -98,7 +98,7 @@ namespace components /// \brief State data as component for power commands and feedback from sensors for PCRecord /// message in ROS2 using ElectroHydraulicState = - ignition::gazebo::components::Component; IGN_GAZEBO_REGISTER_COMPONENT( "buoy_gazebo.components.ElectroHydraulicState", diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt b/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt index 3e995b29..441f4322 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt @@ -1,5 +1,5 @@ -find_package(ignition-math6 REQUIRED) -set(GZ_MATH_VER ${ignition-math6_VERSION_MAJOR}) +find_package(gz-math6 REQUIRED) +set(GZ_MATH_VER ${gz-math6_VERSION_MAJOR}) gz_add_plugin(PolytropicPneumaticSpring SOURCES @@ -7,5 +7,5 @@ gz_add_plugin(PolytropicPneumaticSpring INCLUDE_DIRS .. PUBLIC_LINK_LIBS - ignition-math${GZ_MATH_VER}::ignition-math${GZ_MATH_VER} + gz-math${GZ_MATH_VER}::gz-math${GZ_MATH_VER} ) diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index 9a5cd24a..66a65511 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -14,20 +14,20 @@ #include "PolytropicPneumaticSpring.hpp" -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include +#include #include #include @@ -101,10 +101,10 @@ struct PolytropicPneumaticSpringConfig bool debug_prescribed_velocity{false}; /// \brief Joint Entity - ignition::gazebo::Entity jointEntity; + gz::sim::Entity jointEntity; /// \brief Model interface - ignition::gazebo::Model model{ignition::gazebo::kNullEntity}; + gz::sim::Model model{gz::sim::kNullEntity}; }; struct PolytropicPneumaticSpringPrivate @@ -261,10 +261,10 @@ void PolytropicPneumaticSpring::computeForce(const double & x, const double & v) ////////////////////////////////////////////////// void PolytropicPneumaticSpring::Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & /*_eventMgr*/) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & /*_eventMgr*/) { PolytropicPneumaticSpringConfig config; @@ -340,7 +340,7 @@ void PolytropicPneumaticSpring::Configure( igndbg << "mass: " << this->dataPtr->mass << std::endl; } - config.model = ignition::gazebo::Model(_entity); + config.model = gz::sim::Model(_entity); if (!config.model.Valid(_ecm)) { ignerr << "PolytropicPneumaticSpring plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; @@ -358,7 +358,7 @@ void PolytropicPneumaticSpring::Configure( config.jointEntity = config.model.JointByName( _ecm, jointName); - if (config.jointEntity == ignition::gazebo::kNullEntity) { + if (config.jointEntity == gz::sim::kNullEntity) { ignerr << "Joint with name[" << jointName << "] not found. " << "The PolytropicPneumaticSpring may not influence this joint.\n"; return; @@ -368,8 +368,8 @@ void PolytropicPneumaticSpring::Configure( std::string force_topic = std::string("/force_") + this->dataPtr->config_->name; force_pub = - node.Advertise( - ignition::transport::TopicUtils::AsValidTopic( + node.Advertise( + gz::transport::TopicUtils::AsValidTopic( force_topic)); if (!force_pub) { ignerr << "Error advertising topic [" << force_topic << "]" << std::endl; @@ -378,8 +378,8 @@ void PolytropicPneumaticSpring::Configure( std::string pressure_topic = std::string("/pressure_") + this->dataPtr->config_->name; pressure_pub = - node.Advertise( - ignition::transport::TopicUtils::AsValidTopic( + node.Advertise( + gz::transport::TopicUtils::AsValidTopic( pressure_topic)); if (!pressure_pub) { ignerr << "Error advertising topic [" << pressure_topic << "]" << std::endl; @@ -388,8 +388,8 @@ void PolytropicPneumaticSpring::Configure( std::string volume_topic = std::string("/volume_") + this->dataPtr->config_->name; volume_pub = - node.Advertise( - ignition::transport::TopicUtils::AsValidTopic( + node.Advertise( + gz::transport::TopicUtils::AsValidTopic( volume_topic)); if (!volume_pub) { ignerr << "Error advertising topic [" << volume_topic << "]" << std::endl; @@ -398,8 +398,8 @@ void PolytropicPneumaticSpring::Configure( std::string temperature_topic = std::string("/temperature_") + this->dataPtr->config_->name; temperature_pub = - node.Advertise( - ignition::transport::TopicUtils::AsValidTopic( + node.Advertise( + gz::transport::TopicUtils::AsValidTopic( temperature_topic)); if (!temperature_pub) { ignerr << "Error advertising topic [" << temperature_topic << "]" << std::endl; @@ -408,8 +408,8 @@ void PolytropicPneumaticSpring::Configure( std::string heat_rate_topic = std::string("/heat_rate_") + this->dataPtr->config_->name; heat_rate_pub = - node.Advertise( - ignition::transport::TopicUtils::AsValidTopic( + node.Advertise( + gz::transport::TopicUtils::AsValidTopic( heat_rate_topic)); if (!heat_rate_pub) { ignerr << "Error advertising topic [" << heat_rate_topic << "]" << std::endl; @@ -419,13 +419,13 @@ void PolytropicPneumaticSpring::Configure( ////////////////////////////////////////////////// void PolytropicPneumaticSpring::PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) { IGN_PROFILE("PolytropicPneumaticSpring::PreUpdate"); // If the joint hasn't been identified yet, the plugin is disabled - if (this->dataPtr->config_->jointEntity == ignition::gazebo::kNullEntity) { + if (this->dataPtr->config_->jointEntity == gz::sim::kNullEntity) { return; } @@ -443,11 +443,11 @@ void PolytropicPneumaticSpring::PreUpdate( // Create joint position component if one doesn't exist auto jointPosComp = - _ecm.Component( + _ecm.Component( this->dataPtr->config_->jointEntity); if (jointPosComp == nullptr) { _ecm.CreateComponent( - this->dataPtr->config_->jointEntity, ignition::gazebo::components::JointPosition()); + this->dataPtr->config_->jointEntity, gz::sim::components::JointPosition()); } // We just created the joint position component, give one iteration for the // physics system to update its size @@ -457,11 +457,11 @@ void PolytropicPneumaticSpring::PreUpdate( // Create joint velocity component if one doesn't exist auto jointVelComp = - _ecm.Component( + _ecm.Component( this->dataPtr->config_->jointEntity); if (jointVelComp == nullptr) { _ecm.CreateComponent( - this->dataPtr->config_->jointEntity, ignition::gazebo::components::JointVelocity()); + this->dataPtr->config_->jointEntity, gz::sim::components::JointVelocity()); } // We just created the joint velocity component, give one iteration for the // physics system to update its size @@ -535,7 +535,7 @@ void PolytropicPneumaticSpring::PreUpdate( this->dataPtr->F *= -1.0; } - auto stampMsg = ignition::gazebo::convert(_info.simTime); + auto stampMsg = gz::sim::convert(_info.simTime); if (this->dataPtr->config_->is_upper) { spring_state.range_finder = x; @@ -548,28 +548,28 @@ void PolytropicPneumaticSpring::PreUpdate( this->dataPtr->config_->jointEntity, spring_state); - ignition::msgs::Double force; + gz::msgs::Double force; force.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); force.set_data(this->dataPtr->F); // TODO(anyone): use sensor to access to P (pressure sensor) and T (thermometer) - ignition::msgs::Double pressure; + gz::msgs::Double pressure; pressure.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); pressure.set_data(this->dataPtr->P); - ignition::msgs::Double volume; + gz::msgs::Double volume; volume.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); volume.set_data(this->dataPtr->V); - ignition::msgs::Double temperature; + gz::msgs::Double temperature; temperature.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); temperature.set_data(this->dataPtr->T); - ignition::msgs::Double heat_rate; + gz::msgs::Double heat_rate; heat_rate.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); heat_rate.set_data(this->dataPtr->Q_rate); - ignition::msgs::Double piston_velocity; + gz::msgs::Double piston_velocity; piston_velocity.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); piston_velocity.set_data(v); @@ -596,12 +596,12 @@ void PolytropicPneumaticSpring::PreUpdate( static const bool debug_prescribed_velocity{this->dataPtr->config_->debug_prescribed_velocity}; if (!debug_prescribed_velocity) { auto forceComp = - _ecm.Component( + _ecm.Component( this->dataPtr->config_->jointEntity); if (forceComp == nullptr) { _ecm.CreateComponent( this->dataPtr->config_->jointEntity, - ignition::gazebo::components::JointForceCmd({this->dataPtr->F})); + gz::sim::components::JointForceCmd({this->dataPtr->F})); } else { forceComp->Data()[0] += this->dataPtr->F; // Add force to existing forces. } @@ -611,14 +611,14 @@ void PolytropicPneumaticSpring::PreUpdate( double piston_velocity = this->dataPtr->config_->stroke * cos( 2.0 * IGN_PI * std::chrono::duration_cast(_info.simTime).count() / period); - auto joint_vel = _ecm.Component( + auto joint_vel = _ecm.Component( this->dataPtr->config_->jointEntity); if (joint_vel == nullptr) { _ecm.CreateComponent( this->dataPtr->config_->jointEntity, - ignition::gazebo::components::JointVelocityCmd({piston_velocity})); + gz::sim::components::JointVelocityCmd({piston_velocity})); } else { - *joint_vel = ignition::gazebo::components::JointVelocityCmd({piston_velocity}); + *joint_vel = gz::sim::components::JointVelocityCmd({piston_velocity}); } } } @@ -626,6 +626,6 @@ void PolytropicPneumaticSpring::PreUpdate( IGNITION_ADD_PLUGIN( buoy_gazebo::PolytropicPneumaticSpring, - ignition::gazebo::System, + gz::sim::System, buoy_gazebo::PolytropicPneumaticSpring::ISystemConfigure, buoy_gazebo::PolytropicPneumaticSpring::ISystemPreUpdate) diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.hpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.hpp index 2f40af78..46b10efe 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.hpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.hpp @@ -15,8 +15,8 @@ #ifndef POLYTROPICPNEUMATICSPRING__POLYTROPICPNEUMATICSPRING_HPP_ #define POLYTROPICPNEUMATICSPRING__POLYTROPICPNEUMATICSPRING_HPP_ -#include -#include +#include +#include #include @@ -59,9 +59,9 @@ struct PolytropicPneumaticSpringPrivate; /// * ``: initial Pressure (Pa) for decreasing volume (if hysteresis) /// * ``: will force joint to move sinusoidally for debugging -class PolytropicPneumaticSpring : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPreUpdate +class PolytropicPneumaticSpring : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate { public: /// \brief Constructor @@ -72,15 +72,15 @@ class PolytropicPneumaticSpring : public ignition::gazebo::System, // Documentation inherited void Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & _eventMgr) override; + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; // Documentation inherited void PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) override; + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) override; private: /// \brief open valve to vent gas from lower to upper chamber @@ -103,8 +103,8 @@ class PolytropicPneumaticSpring : public ignition::gazebo::System, double & P2, const double & V2); void computeForce(const double & x, const double & v); - ignition::transport::Node node; - ignition::transport::Node::Publisher force_pub, pressure_pub, volume_pub, + gz::transport::Node node; + gz::transport::Node::Publisher force_pub, pressure_pub, volume_pub, temperature_pub, heat_rate_pub, piston_velocity_pub; /// \brief Private data pointer diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/SpringState.hpp b/buoy_gazebo/src/PolytropicPneumaticSpring/SpringState.hpp index 95f542f2..b47408b4 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/SpringState.hpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/SpringState.hpp @@ -15,9 +15,9 @@ #ifndef POLYTROPICPNEUMATICSPRING__SPRINGSTATE_HPP_ #define POLYTROPICPNEUMATICSPRING__SPRINGSTATE_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -68,7 +68,7 @@ struct SpringState namespace components { -using SpringState = ignition::gazebo::components::Component; IGN_GAZEBO_REGISTER_COMPONENT("buoy_gazebo.components.SpringState", SpringState) } // namespace components diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 28043c88..4c6a12e2 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -14,13 +14,13 @@ #include "PowerController.hpp" -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -147,9 +147,9 @@ const rcl_interfaces::msg::FloatingPointRange PowerControllerServices::valid_bia struct PowerControllerPrivate { - ignition::gazebo::Entity entity_{ignition::gazebo::kNullEntity}; - ignition::gazebo::Entity jointEntity_{ignition::gazebo::kNullEntity}; - ignition::transport::Node node_; + gz::sim::Entity entity_{gz::sim::kNullEntity}; + gz::sim::Entity jointEntity_{gz::sim::kNullEntity}; + gz::transport::Node node_; std::chrono::steady_clock::duration current_time_; std::mutex data_mutex_, next_access_mutex_, low_prio_mutex_; @@ -574,13 +574,13 @@ PowerController::~PowerController() } void PowerController::Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { // Make sure the controller is attached to a valid model - auto model = ignition::gazebo::Model(_entity); + auto model = gz::sim::Model(_entity); if (!model.Valid(_ecm)) { ignerr << "[ROS 2 Spring Control] Failed to initialize because [" << model.Name(_ecm) << "] is not a model." << std::endl << @@ -599,14 +599,14 @@ void PowerController::Configure( } this->dataPtr->jointEntity_ = model.JointByName(_ecm, jointName); - if (this->dataPtr->jointEntity_ == ignition::gazebo::kNullEntity) { + if (this->dataPtr->jointEntity_ == gz::sim::kNullEntity) { ignerr << "Joint with name[" << jointName << "] not found. " << "The PowerController may not influence this joint.\n"; return; } // controller scoped name - std::string scoped_name = ignition::gazebo::scopedName(_entity, _ecm, "/", false); + std::string scoped_name = gz::sim::scopedName(_entity, _ecm, "/", false); // ROS node std::string ns = _sdf->Get("namespace", scoped_name).first; @@ -674,8 +674,8 @@ void PowerController::Configure( ///////////////////////////////////////////////////////////////////////////////////////////// void PowerController::PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) { IGN_PROFILE("PowerController::PreUpdate"); @@ -710,8 +710,8 @@ void PowerController::PreUpdate( ///////////////////////////////////////////////////////////////////////////////////////////// void PowerController::PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) { IGN_PROFILE("PowerController::PostUpdate"); @@ -740,7 +740,7 @@ void PowerController::PostUpdate( std::unique_lock data(this->dataPtr->data_mutex_); next.unlock(); - auto sec_nsec = ignition::math::durationToSecNsec(this->dataPtr->current_time_); + auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_); this->dataPtr->ros_->pc_record_.header.stamp.sec = sec_nsec.first; this->dataPtr->ros_->pc_record_.header.stamp.nanosec = sec_nsec.second; @@ -773,7 +773,7 @@ void PowerController::PostUpdate( IGNITION_ADD_PLUGIN( buoy_gazebo::PowerController, - ignition::gazebo::System, + gz::sim::System, buoy_gazebo::PowerController::ISystemConfigure, buoy_gazebo::PowerController::ISystemPreUpdate, buoy_gazebo::PowerController::ISystemPostUpdate) diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.hpp b/buoy_gazebo/src/controllers/PowerController/PowerController.hpp index fe3da2ae..ad861297 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.hpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.hpp @@ -15,7 +15,7 @@ #ifndef CONTROLLERS__POWERCONTROLLER__POWERCONTROLLER_HPP_ #define CONTROLLERS__POWERCONTROLLER__POWERCONTROLLER_HPP_ -#include +#include #include namespace buoy_gazebo @@ -33,10 +33,10 @@ struct PowerControllerPrivate; /// * ``: ROS2 topic to publish to, defaults to "pc_record" /// * ``: ROS2 topic publish rate, defaults to 10Hz class PowerController - : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPreUpdate, - public ignition::gazebo::ISystemPostUpdate + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate { public: /// \brief Constructor @@ -47,20 +47,20 @@ class PowerController // Documentation inherited void Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & _eventMgr) override; + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; // Documentation inherited void PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) override; + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) override; // Documentation inherited void PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) override; + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) override; private: /// \brief Private data pointer. diff --git a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp index 82b43ca3..f2268f0e 100644 --- a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp +++ b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp @@ -14,10 +14,10 @@ #include "NoOpController.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -340,13 +340,13 @@ NoOpController::~NoOpController() } void NoOpController::Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { // Make sure the controller is attached to a valid model - auto model = ignition::gazebo::Model(_entity); + auto model = gz::sim::Model(_entity); if (!model.Valid(_ecm)) { ignerr << "[ROS 2 NoOp Controller] Failed to initialize because [" << model.Name(_ecm) << "] is not a model." << std::endl << @@ -355,7 +355,7 @@ void NoOpController::Configure( } // controller scoped name - std::string scoped_name = ignition::gazebo::scopedName(_entity, _ecm, "/", false); + std::string scoped_name = gz::sim::scopedName(_entity, _ecm, "/", false); // ROS node std::string ns = _sdf->Get("namespace", scoped_name).first; @@ -377,5 +377,5 @@ void NoOpController::Configure( IGNITION_ADD_PLUGIN( buoy_gazebo::NoOpController, - ignition::gazebo::System, + gz::sim::System, buoy_gazebo::NoOpController::ISystemConfigure) diff --git a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.hpp b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.hpp index 0c90430a..0b341b8c 100644 --- a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.hpp +++ b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.hpp @@ -15,7 +15,7 @@ #ifndef CONTROLLERS__SERVICESNOTIMPLEMENTED__NOOPCONTROLLER_HPP_ #define CONTROLLERS__SERVICESNOTIMPLEMENTED__NOOPCONTROLLER_HPP_ -#include +#include #include namespace buoy_gazebo @@ -29,8 +29,8 @@ struct NoOpControllerPrivate; /// * ``: Namespace for ROS node, defaults to scoped name /// * ``: ROS2 node name, defaults to "noop_controller" class NoOpController - : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure + : public gz::sim::System, + public gz::sim::ISystemConfigure { public: /// \brief Constructor @@ -41,10 +41,10 @@ class NoOpController // Documentation inherited void Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & _eventMgr) override; + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; private: /// \brief Private data pointer. diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp index b01a5760..7d530005 100644 --- a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp +++ b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp @@ -14,13 +14,13 @@ #include "SpringController.hpp" -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -78,10 +78,10 @@ struct SpringControllerServices struct SpringControllerPrivate { - ignition::gazebo::Entity entity_{ignition::gazebo::kNullEntity}; - ignition::gazebo::Entity jointEntity_{ignition::gazebo::kNullEntity}; - ignition::transport::Node node_; - std::function ft_cb_; + gz::sim::Entity entity_{gz::sim::kNullEntity}; + gz::sim::Entity jointEntity_{gz::sim::kNullEntity}; + gz::transport::Node node_; + std::function ft_cb_; std::chrono::steady_clock::duration current_time_; std::mutex data_mutex_, next_access_mutex_, low_prio_mutex_; @@ -444,13 +444,13 @@ SpringController::~SpringController() } void SpringController::Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { // Make sure the controller is attached to a valid model - auto model = ignition::gazebo::Model(_entity); + auto model = gz::sim::Model(_entity); if (!model.Valid(_ecm)) { ignerr << "[ROS 2 Spring Control] Failed to initialize because [" << model.Name(_ecm) << "] is not a model." << std::endl << @@ -469,14 +469,14 @@ void SpringController::Configure( } this->dataPtr->jointEntity_ = model.JointByName(_ecm, jointName); - if (this->dataPtr->jointEntity_ == ignition::gazebo::kNullEntity) { + if (this->dataPtr->jointEntity_ == gz::sim::kNullEntity) { ignerr << "Joint with name[" << jointName << "] not found. " << "The SpringController may not influence this joint.\n"; return; } // controller scoped name - std::string scoped_name = ignition::gazebo::scopedName(_entity, _ecm, "/", false); + std::string scoped_name = gz::sim::scopedName(_entity, _ecm, "/", false); // ROS node std::string ns = _sdf->Get("namespace", scoped_name).first; @@ -492,7 +492,7 @@ void SpringController::Configure( "[ROS 2 Spring Control] Setting up controller for [" << model.Name(_ecm) << "]"); // Force Torque Sensor - this->dataPtr->ft_cb_ = [this](const ignition::msgs::Wrench & _msg) + this->dataPtr->ft_cb_ = [this](const gz::msgs::Wrench & _msg) { // low prio data access std::unique_lock low(this->dataPtr->low_prio_mutex_); @@ -561,8 +561,8 @@ void SpringController::Configure( ///////////////////////////////////////////////////////////////////////////////////////////// void SpringController::PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) { IGN_PROFILE("SpringController::PreUpdate"); @@ -598,8 +598,8 @@ void SpringController::PreUpdate( ///////////////////////////////////////////////////////////////////////////////////////////// void SpringController::PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) { IGN_PROFILE("SpringController::PostUpdate"); @@ -628,7 +628,7 @@ void SpringController::PostUpdate( std::unique_lock data(this->dataPtr->data_mutex_); next.unlock(); - auto sec_nsec = ignition::math::durationToSecNsec(this->dataPtr->current_time_); + auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_); this->dataPtr->ros_->sc_record_.header.stamp.sec = sec_nsec.first; this->dataPtr->ros_->sc_record_.header.stamp.nanosec = sec_nsec.second; @@ -651,7 +651,7 @@ void SpringController::PostUpdate( IGNITION_ADD_PLUGIN( buoy_gazebo::SpringController, - ignition::gazebo::System, + gz::sim::System, buoy_gazebo::SpringController::ISystemConfigure, buoy_gazebo::SpringController::ISystemPreUpdate, buoy_gazebo::SpringController::ISystemPostUpdate) diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.hpp b/buoy_gazebo/src/controllers/SpringController/SpringController.hpp index 0d774534..ad90b04b 100644 --- a/buoy_gazebo/src/controllers/SpringController/SpringController.hpp +++ b/buoy_gazebo/src/controllers/SpringController/SpringController.hpp @@ -15,7 +15,7 @@ #ifndef CONTROLLERS__SPRINGCONTROLLER__SPRINGCONTROLLER_HPP_ #define CONTROLLERS__SPRINGCONTROLLER__SPRINGCONTROLLER_HPP_ -#include +#include #include namespace buoy_gazebo @@ -34,10 +34,10 @@ struct SpringControllerPrivate; /// * ``: ROS2 topic to publish to, defaults to "sc_record" /// * ``: ROS2 topic publish rate, defaults to 10Hz class SpringController - : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPreUpdate, - public ignition::gazebo::ISystemPostUpdate + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate { public: /// \brief Constructor @@ -48,20 +48,20 @@ class SpringController // Documentation inherited void Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & _eventMgr) override; + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; // Documentation inherited void PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) override; + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) override; // Documentation inherited void PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) override; + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) override; private: /// \brief Private data pointer. diff --git a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp index 68123c91..9d59270b 100644 --- a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp +++ b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp @@ -14,13 +14,13 @@ #include "XBowAHRS.hpp" -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include @@ -34,11 +34,11 @@ struct buoy_gazebo::XBowAHRSPrivate { - ignition::gazebo::Entity entity_; - ignition::gazebo::Entity linkEntity_; + gz::sim::Entity entity_; + gz::sim::Entity linkEntity_; rclcpp::Node::SharedPtr rosnode_{nullptr}; - ignition::transport::Node node_; - std::function imu_cb_; + gz::transport::Node node_; + std::function imu_cb_; rclcpp::Publisher::SharedPtr xb_pub_{nullptr}; rclcpp::Publisher::SharedPtr imu_pub_{nullptr}; double pub_rate_hz_{10.0}; @@ -51,7 +51,7 @@ struct buoy_gazebo::XBowAHRSPrivate rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; std::atomic stop_{false}, paused_{true}; - void set_xb_record_imu(const ignition::msgs::IMU & _imu) + void set_xb_record_imu(const gz::msgs::IMU & _imu) { xb_record_.header.stamp.sec = _imu.header().stamp().sec(); xb_record_.header.stamp.nanosec = _imu.header().stamp().nsec(); @@ -78,7 +78,7 @@ struct buoy_gazebo::XBowAHRSPrivate IGNITION_ADD_PLUGIN( buoy_gazebo::XBowAHRS, - ignition::gazebo::System, + gz::sim::System, buoy_gazebo::XBowAHRS::ISystemConfigure, buoy_gazebo::XBowAHRS::ISystemPostUpdate) @@ -100,13 +100,13 @@ XBowAHRS::~XBowAHRS() } void XBowAHRS::Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { // Make sure the controller is attached to a valid model - auto model = ignition::gazebo::Model(_entity); + auto model = gz::sim::Model(_entity); if (!model.Valid(_ecm)) { ignerr << "[ROS 2 XBow AHRS] Failed to initialize because [" << \ model.Name(_ecm) << "] is not a model." << std::endl << \ @@ -114,14 +114,14 @@ void XBowAHRS::Configure( return; } const auto link_entity = model.LinkByName(_ecm, "Buoy"); - ignition::gazebo::Link link(link_entity); + gz::sim::Link link(link_entity); link.EnableVelocityChecks(_ecm); this->dataPtr->entity_ = _entity; // Get params from SDF // controller scoped name - std::string scoped_name = ignition::gazebo::scopedName(this->dataPtr->entity_, _ecm, "/", false); + std::string scoped_name = gz::sim::scopedName(this->dataPtr->entity_, _ecm, "/", false); // ROS node std::string ns = _sdf->Get("namespace", scoped_name).first; @@ -152,7 +152,7 @@ void XBowAHRS::Configure( "[ROS 2 XBow AHRS] Setting up controller for [" << model.Name(_ecm) << "]"); // IMU Sensor - this->dataPtr->imu_cb_ = [this](const ignition::msgs::IMU & _imu) + this->dataPtr->imu_cb_ = [this](const gz::msgs::IMU & _imu) { // low prio data access std::unique_lock low(this->dataPtr->low_prio_mutex_); @@ -215,13 +215,13 @@ void XBowAHRS::Configure( ///////////////////////////////////////////////////////////////////////////////////////////// void XBowAHRS::PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) { this->dataPtr->paused_ = _info.paused; - auto model = ignition::gazebo::Model(this->dataPtr->entity_); + auto model = gz::sim::Model(this->dataPtr->entity_); const auto link_entity = model.LinkByName(_ecm, "Buoy"); - ignition::gazebo::Link link(link_entity); + gz::sim::Link link(link_entity); auto v_world = link.WorldLinearVelocity(_ecm); // assume x,y,z == ENU // low prio data access diff --git a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.hpp b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.hpp index aa7933f5..fdc97314 100644 --- a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.hpp +++ b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.hpp @@ -15,7 +15,7 @@ #ifndef CONTROLLERS__XBOWAHRS__XBOWAHRS_HPP_ #define CONTROLLERS__XBOWAHRS__XBOWAHRS_HPP_ -#include +#include #include namespace buoy_gazebo @@ -30,9 +30,9 @@ struct XBowAHRSPrivate; /// * ``: ROS2 topic to publish Imu, defaults to "xb_imu" /// * ``: ROS2 topic publish rate, defaults to 10Hz class XBowAHRS - : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPostUpdate + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate { public: /// \brief Constructor @@ -43,15 +43,15 @@ class XBowAHRS // Documentation inherited void Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & _eventMgr) override; + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; // Documentation inherited void PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) override; + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) override; private: /// \brief Private data pointer. diff --git a/buoy_gazebo/worlds/buoy_playground.sdf b/buoy_gazebo/worlds/buoy_playground.sdf index 0aa29162..1d44dd71 100644 --- a/buoy_gazebo/worlds/buoy_playground.sdf +++ b/buoy_gazebo/worlds/buoy_playground.sdf @@ -16,11 +16,11 @@ - + 3D View false docked - + ogre2 scene @@ -31,78 +31,78 @@ - + floating 5 5 false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 250 50 floating false #777777 - + false @@ -110,7 +110,7 @@ - + World control false false @@ -123,7 +123,7 @@ - + true true @@ -134,7 +134,7 @@ - + World stats false false @@ -147,7 +147,7 @@ - + true true @@ -156,22 +156,22 @@ - + docked - + MBARI_WEC - + docked_collapsed - + - + docked_collapsed - + @@ -215,8 +215,8 @@ + filename="gz-sim-joint-position-controller-system" + name="gz::sim::systems::JointPositionController"> HydraulicRam 10000 1000 @@ -228,13 +228,13 @@ + filename="gz-sim-joint-state-publisher-system" + name="gz::sim::systems::JointStatePublisher"> + filename="gz-sim-pose-publisher-system" + name="gz::sim::systems::PosePublisher"> true true true @@ -242,8 +242,8 @@ + filename="gz-sim-odometry-publisher-system" + name="gz::sim::systems::OdometryPublisher"> 3 MBARI_WEC/odom MBARI_WEC diff --git a/buoy_tests/CMakeLists.txt b/buoy_tests/CMakeLists.txt index e6c26d19..f4456fbf 100644 --- a/buoy_tests/CMakeLists.txt +++ b/buoy_tests/CMakeLists.txt @@ -10,8 +10,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} ) if(BUILD_TESTING) - find_package(ignition-gazebo6 REQUIRED) - set(GZ_SIM_VER ${ignition-gazebo6_VERSION_MAJOR}) + find_package(gz-sim7 REQUIRED) + set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR}) # Linters find_package(ament_lint_auto REQUIRED) @@ -42,7 +42,7 @@ if(BUILD_TESTING) tests/${TEST_NAME}.cpp ) target_link_libraries(${TEST_NAME} - ignition-gazebo${GZ_SIM_VER}::ignition-gazebo${GZ_SIM_VER} + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ) if(buoy_add_test_ROS) set(ROS_PKGS rclcpp buoy_msgs ${buoy_add_test_EXTRA_ROS_PKGS}) @@ -81,7 +81,7 @@ if(BUILD_TESTING) ) target_link_libraries(${GTEST_NAME} - ignition-gazebo${GZ_SIM_VER}::ignition-gazebo${GZ_SIM_VER} + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ) rosidl_target_interfaces(${GTEST_NAME} ${PROJECT_NAME} "rosidl_typesupport_cpp") ament_target_dependencies(${GTEST_NAME} rclcpp) diff --git a/buoy_tests/launch/no_inputs_py.launch.py b/buoy_tests/launch/no_inputs_py.launch.py index e43e3aef..67e9dfeb 100644 --- a/buoy_tests/launch/no_inputs_py.launch.py +++ b/buoy_tests/launch/no_inputs_py.launch.py @@ -40,7 +40,7 @@ def generate_test_description(): bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') return launch.LaunchDescription([ diff --git a/buoy_tests/launch/no_inputs_ros_feedback.launch.py b/buoy_tests/launch/no_inputs_ros_feedback.launch.py index 9998ddf6..7c0b3a22 100644 --- a/buoy_tests/launch/no_inputs_ros_feedback.launch.py +++ b/buoy_tests/launch/no_inputs_ros_feedback.launch.py @@ -34,7 +34,7 @@ def generate_test_description(): bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') return launch.LaunchDescription([ diff --git a/buoy_tests/launch/pc_commands_ros_feedback.launch.py b/buoy_tests/launch/pc_commands_ros_feedback.launch.py index 8ec6e69d..8026b07c 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback.launch.py @@ -34,7 +34,7 @@ def generate_test_description(): bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') return launch.LaunchDescription([ diff --git a/buoy_tests/launch/run_server.launch.py b/buoy_tests/launch/run_server.launch.py index 7318ce06..2b992d72 100644 --- a/buoy_tests/launch/run_server.launch.py +++ b/buoy_tests/launch/run_server.launch.py @@ -28,7 +28,7 @@ def generate_launch_description(): bridge = launchNode(package='ros_ign_bridge', executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') return launch.LaunchDescription([ diff --git a/buoy_tests/launch/sc_commands_ros_feedback.launch.py b/buoy_tests/launch/sc_commands_ros_feedback.launch.py index 58a51c3e..69e11714 100644 --- a/buoy_tests/launch/sc_commands_ros_feedback.launch.py +++ b/buoy_tests/launch/sc_commands_ros_feedback.launch.py @@ -34,7 +34,7 @@ def generate_test_description(): bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') return launch.LaunchDescription([ diff --git a/buoy_tests/package.xml b/buoy_tests/package.xml index 6959fa5e..370aa5b9 100644 --- a/buoy_tests/package.xml +++ b/buoy_tests/package.xml @@ -18,7 +18,7 @@ buoy_examples rclpy rclcpp - python3-ignition-gazebo6 + python3-gz-sim7 ament_cmake_gtest ament_cmake_pytest launch_testing_ament_cmake diff --git a/buoy_tests/testing_utils/utils.py b/buoy_tests/testing_utils/utils.py index 3a134633..668f334e 100644 --- a/buoy_tests/testing_utils/utils.py +++ b/buoy_tests/testing_utils/utils.py @@ -21,8 +21,8 @@ from buoy_tests.srv import RunServer # TODO(anyone) Put back when fixed upstream -# from ignition.common import set_verbosity -# from ignition.gazebo import TestFixture +# from gz.common import set_verbosity +# from gz.sim import TestFixture import launch import launch.actions @@ -49,7 +49,7 @@ def default_generate_test_description(): bridge = launchNode(package='ros_ign_bridge', executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') return launch.LaunchDescription([ diff --git a/buoy_tests/tests/fixture_server.cpp b/buoy_tests/tests/fixture_server.cpp index c09e08ef..91a75bab 100644 --- a/buoy_tests/tests/fixture_server.cpp +++ b/buoy_tests/tests/fixture_server.cpp @@ -16,12 +16,12 @@ #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include @@ -34,39 +34,39 @@ TEST(BuoyTests, RunServer) { // Skip debug messages to run faster - ignition::common::Console::SetVerbosity(3); + gz::common::Console::SetVerbosity(3); // Setup fixture - ignition::gazebo::ServerConfig config; + gz::sim::ServerConfig config; config.SetSdfFile("mbari_wec.sdf"); config.SetUpdateRate(0.0); size_t iterations{0U}; - ignition::gazebo::Entity buoyEntity{ignition::gazebo::kNullEntity}; - std::unique_ptr fixture = - std::make_unique(config); + gz::sim::Entity buoyEntity{gz::sim::kNullEntity}; + std::unique_ptr fixture = + std::make_unique(config); fixture-> OnConfigure( [&]( - const ignition::gazebo::Entity & _worldEntity, + const gz::sim::Entity & _worldEntity, const std::shared_ptr &, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { - auto world = ignition::gazebo::World(_worldEntity); + auto world = gz::sim::World(_worldEntity); buoyEntity = world.ModelByName(_ecm, "MBARI_WEC_ROS"); - EXPECT_NE(ignition::gazebo::kNullEntity, buoyEntity); + EXPECT_NE(gz::sim::kNullEntity, buoyEntity); }). OnPostUpdate( [&]( - const ignition::gazebo::UpdateInfo &, - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo &, + const gz::sim::EntityComponentManager & _ecm) { iterations++; - auto pose = ignition::gazebo::worldPose(buoyEntity, _ecm); + auto pose = gz::sim::worldPose(buoyEntity, _ecm); // Expect buoy to stay more or less in the same place horizontally. EXPECT_LT(-0.001, pose.Pos().X()); diff --git a/buoy_tests/tests/gravity.sdf b/buoy_tests/tests/gravity.sdf index ee1161fa..c2ae2a17 100644 --- a/buoy_tests/tests/gravity.sdf +++ b/buoy_tests/tests/gravity.sdf @@ -2,8 +2,8 @@ + filename="gz-sim-physics-system" + name="gz::sim::systems::Physics"> diff --git a/buoy_tests/tests/no_inputs.cpp b/buoy_tests/tests/no_inputs.cpp index 2d0505c8..9f51f385 100644 --- a/buoy_tests/tests/no_inputs.cpp +++ b/buoy_tests/tests/no_inputs.cpp @@ -14,12 +14,12 @@ #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include @@ -27,38 +27,38 @@ TEST(BuoyTests, NoInputs) { // Skip debug messages to run faster - ignition::common::Console::SetVerbosity(3); + gz::common::Console::SetVerbosity(3); // Setup fixture - ignition::gazebo::ServerConfig config; + gz::sim::ServerConfig config; config.SetSdfFile("mbari_wec.sdf"); config.SetUpdateRate(0.0); - ignition::gazebo::TestFixture fixture(config); + gz::sim::TestFixture fixture(config); int iterations{0}; - ignition::gazebo::Entity buoyEntity{ignition::gazebo::kNullEntity}; + gz::sim::Entity buoyEntity{gz::sim::kNullEntity}; fixture. OnConfigure( - [&](const ignition::gazebo::Entity & _worldEntity, + [&](const gz::sim::Entity & _worldEntity, const std::shared_ptr &, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { - auto world = ignition::gazebo::World(_worldEntity); + auto world = gz::sim::World(_worldEntity); buoyEntity = world.ModelByName(_ecm, "MBARI_WEC_ROS"); - EXPECT_NE(ignition::gazebo::kNullEntity, buoyEntity); + EXPECT_NE(gz::sim::kNullEntity, buoyEntity); }). OnPostUpdate( [&]( - const ignition::gazebo::UpdateInfo &, - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo &, + const gz::sim::EntityComponentManager & _ecm) { iterations++; - auto pose = ignition::gazebo::worldPose(buoyEntity, _ecm); + auto pose = gz::sim::worldPose(buoyEntity, _ecm); // Expect buoy to stay more or less in the same place horizontally. EXPECT_LT(-0.001, pose.Pos().X()); @@ -82,5 +82,5 @@ TEST(BuoyTests, NoInputs) // Sanity check that the test ran EXPECT_EQ(targetIterations, iterations); - EXPECT_NE(ignition::gazebo::kNullEntity, buoyEntity); + EXPECT_NE(gz::sim::kNullEntity, buoyEntity); } diff --git a/buoy_tests/tests/no_inputs_ros_feedback.cpp b/buoy_tests/tests/no_inputs_ros_feedback.cpp index b52339fd..bc8e6665 100644 --- a/buoy_tests/tests/no_inputs_ros_feedback.cpp +++ b/buoy_tests/tests/no_inputs_ros_feedback.cpp @@ -16,12 +16,12 @@ #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -88,39 +88,39 @@ class NoInputsROSNode final : public buoy_msgs::Interface TEST(BuoyTests, NoInputsROS) { // Skip debug messages to run faster - ignition::common::Console::SetVerbosity(3); + gz::common::Console::SetVerbosity(3); // Setup fixture - ignition::gazebo::ServerConfig config; + gz::sim::ServerConfig config; config.SetSdfFile("mbari_wec.sdf"); config.SetUpdateRate(0.0); - ignition::gazebo::TestFixture fixture(config); + gz::sim::TestFixture fixture(config); NoInputsROSNode node("test_no_inputs_ros"); int iterations{0}; - ignition::gazebo::Entity buoyEntity{ignition::gazebo::kNullEntity}; + gz::sim::Entity buoyEntity{gz::sim::kNullEntity}; fixture. OnConfigure( - [&](const ignition::gazebo::Entity & _worldEntity, + [&](const gz::sim::Entity & _worldEntity, const std::shared_ptr &, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { - auto world = ignition::gazebo::World(_worldEntity); + auto world = gz::sim::World(_worldEntity); buoyEntity = world.ModelByName(_ecm, "MBARI_WEC_ROS"); - EXPECT_NE(ignition::gazebo::kNullEntity, buoyEntity); + EXPECT_NE(gz::sim::kNullEntity, buoyEntity); }). OnPostUpdate( [&]( - const ignition::gazebo::UpdateInfo &, - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo &, + const gz::sim::EntityComponentManager & _ecm) { iterations++; - auto pose = ignition::gazebo::worldPose(buoyEntity, _ecm); + auto pose = gz::sim::worldPose(buoyEntity, _ecm); // Expect buoy to stay more or less in the same place horizontally. EXPECT_LT(-0.001, pose.Pos().X()); @@ -151,5 +151,5 @@ TEST(BuoyTests, NoInputsROS) // Sanity check that the test ran EXPECT_EQ(targetIterations, iterations); - EXPECT_NE(ignition::gazebo::kNullEntity, buoyEntity); + EXPECT_NE(gz::sim::kNullEntity, buoyEntity); } diff --git a/buoy_tests/tests/pc_commands_ros_feedback.cpp b/buoy_tests/tests/pc_commands_ros_feedback.cpp index f2a43790..b8549cb1 100644 --- a/buoy_tests/tests/pc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/pc_commands_ros_feedback.cpp @@ -16,12 +16,12 @@ #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -122,44 +122,44 @@ class BuoyPCTests : public ::testing::Test { protected: int iterations{0}; - std::unique_ptr fixture{nullptr}; + std::unique_ptr fixture{nullptr}; std::unique_ptr node{nullptr}; - ignition::gazebo::Entity buoyEntity{ignition::gazebo::kNullEntity}; + gz::sim::Entity buoyEntity{gz::sim::kNullEntity}; virtual void SetUp() { // Skip debug messages to run faster - ignition::common::Console::SetVerbosity(3); + gz::common::Console::SetVerbosity(3); // Setup fixture - ignition::gazebo::ServerConfig config; + gz::sim::ServerConfig config; config.SetSdfFile("mbari_wec.sdf"); config.SetUpdateRate(0.0); - fixture = std::make_unique(config); + fixture = std::make_unique(config); node = std::make_unique("test_pc_ros"); fixture-> OnConfigure( [&]( - const ignition::gazebo::Entity & _worldEntity, + const gz::sim::Entity & _worldEntity, const std::shared_ptr &, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { - auto world = ignition::gazebo::World(_worldEntity); + auto world = gz::sim::World(_worldEntity); buoyEntity = world.ModelByName(_ecm, "MBARI_WEC_ROS"); - EXPECT_NE(ignition::gazebo::kNullEntity, buoyEntity); + EXPECT_NE(gz::sim::kNullEntity, buoyEntity); }). OnPostUpdate( [&]( - const ignition::gazebo::UpdateInfo &, - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo &, + const gz::sim::EntityComponentManager & _ecm) { iterations++; - auto pose = ignition::gazebo::worldPose(buoyEntity, _ecm); + auto pose = gz::sim::worldPose(buoyEntity, _ecm); // Expect buoy to stay more or less in the same place horizontally. EXPECT_LT(-0.001, pose.Pos().X()); @@ -371,5 +371,5 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) node->stop(); // Sanity check that the test ran - EXPECT_NE(ignition::gazebo::kNullEntity, buoyEntity); + EXPECT_NE(gz::sim::kNullEntity, buoyEntity); } diff --git a/buoy_tests/tests/sc_commands_ros_feedback.cpp b/buoy_tests/tests/sc_commands_ros_feedback.cpp index 05d81cef..7234ad93 100644 --- a/buoy_tests/tests/sc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/sc_commands_ros_feedback.cpp @@ -16,12 +16,12 @@ #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include @@ -105,44 +105,44 @@ class BuoySCTests : public ::testing::Test { protected: int iterations{0}; - std::unique_ptr fixture{nullptr}; + std::unique_ptr fixture{nullptr}; std::unique_ptr node{nullptr}; - ignition::gazebo::Entity buoyEntity{ignition::gazebo::kNullEntity}; + gz::sim::Entity buoyEntity{gz::sim::kNullEntity}; virtual void SetUp() { // Skip debug messages to run faster - ignition::common::Console::SetVerbosity(3); + gz::common::Console::SetVerbosity(3); // Setup fixture - ignition::gazebo::ServerConfig config; + gz::sim::ServerConfig config; config.SetSdfFile("mbari_wec.sdf"); config.SetUpdateRate(0.0); - fixture = std::make_unique(config); + fixture = std::make_unique(config); node = std::make_unique("test_sc_ros"); fixture-> OnConfigure( [&]( - const ignition::gazebo::Entity & _worldEntity, + const gz::sim::Entity & _worldEntity, const std::shared_ptr &, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { - auto world = ignition::gazebo::World(_worldEntity); + auto world = gz::sim::World(_worldEntity); buoyEntity = world.ModelByName(_ecm, "MBARI_WEC_ROS"); - EXPECT_NE(ignition::gazebo::kNullEntity, buoyEntity); + EXPECT_NE(gz::sim::kNullEntity, buoyEntity); }). OnPostUpdate( [&]( - const ignition::gazebo::UpdateInfo &, - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo &, + const gz::sim::EntityComponentManager & _ecm) { iterations++; - auto pose = ignition::gazebo::worldPose(buoyEntity, _ecm); + auto pose = gz::sim::worldPose(buoyEntity, _ecm); // Expect buoy to stay more or less in the same place horizontally. EXPECT_LT(-0.001, pose.Pos().X()); @@ -294,7 +294,7 @@ TEST_F(BuoySCTests, SCValveROS) node->stop(); // Sanity check that the test ran - EXPECT_NE(ignition::gazebo::kNullEntity, buoyEntity); + EXPECT_NE(gz::sim::kNullEntity, buoyEntity); } ////////////////////////////////////////////////// @@ -429,5 +429,5 @@ TEST_F(BuoySCTests, SCPumpROS) node->stop(); // Sanity check that the test ran - EXPECT_NE(ignition::gazebo::kNullEntity, buoyEntity); + EXPECT_NE(gz::sim::kNullEntity, buoyEntity); } diff --git a/buoy_tests/tests/test_fixture.py b/buoy_tests/tests/test_fixture.py index ae982aac..f8edf1bc 100644 --- a/buoy_tests/tests/test_fixture.py +++ b/buoy_tests/tests/test_fixture.py @@ -25,8 +25,8 @@ import time import unittest -from ignition.common import set_verbosity -from ignition.gazebo import TestFixture, World, world_entity +from gz.common import set_verbosity +from gz.sim import TestFixture, World, world_entity import pytest @@ -62,9 +62,9 @@ def on_pre_udpate_cb(self, _info, _ecm): ^^ fails with: [224.080s] 6: E TypeError: Unable to convert function return value to a Python type! The signature was - [224.080s] 6: E (self: ignition.gazebo.World, - arg0: ignition.gazebo.EntityComponentManager) - -> Optional[ignition::math::v6::Vector3] + [224.080s] 6: E (self: gz.sim.World, + arg0: gz.sim.EntityComponentManager) + -> Optional[gz::math::v6::Vector3] """ # print('Gravity ', v) modelEntity = w.model_by_name(_ecm, 'falling') From e0ccb10242ec015cab421f577021e04a9c01adb6 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 17 Aug 2022 14:54:28 -0700 Subject: [PATCH 02/57] common5 Signed-off-by: Louise Poubel --- buoy_gazebo/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt index 4dd6fabf..0fe0456b 100644 --- a/buoy_gazebo/CMakeLists.txt +++ b/buoy_gazebo/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(buoy_msgs REQUIRED) find_package(gz-cmake3 REQUIRED) find_package(gz-plugin2 REQUIRED COMPONENTS register) set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) -find_package(gz-common4 REQUIRED COMPONENTS profiler) +find_package(gz-common5 REQUIRED COMPONENTS profiler) set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) find_package(gz-sim7 REQUIRED) set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR}) From 3f6edef0ed04a280c7591451e9981401406cf7eb Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 17 Aug 2022 14:55:49 -0700 Subject: [PATCH 03/57] math7 Signed-off-by: Louise Poubel --- buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt b/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt index 441f4322..660d0a42 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt @@ -1,5 +1,5 @@ -find_package(gz-math6 REQUIRED) -set(GZ_MATH_VER ${gz-math6_VERSION_MAJOR}) +find_package(gz-math7 REQUIRED) +set(GZ_MATH_VER ${gz-math7_VERSION_MAJOR}) gz_add_plugin(PolytropicPneumaticSpring SOURCES From 5d59ce2174bd18ed2d03d4f6edf44be2a1e3c1d5 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 29 Aug 2022 15:55:04 -0700 Subject: [PATCH 04/57] Humble + Garden Signed-off-by: Louise Poubel --- .github/workflows/ci.yml | 2 +- .../hooks/buoy_description.dsv.in | 4 +- buoy_gazebo/hooks/buoy_gazebo.dsv.in | 6 +-- buoy_gazebo/launch/debugger.launch.py | 4 +- .../launch/mbari_wec_sinusoidal.launch.py | 8 +-- .../ElectroHydraulicPTO.cpp | 50 +++++++++---------- .../ElectroHydraulicSoln.hpp | 2 +- .../ElectroHydraulicState.hpp | 2 +- .../PolytropicPneumaticSpring.cpp | 18 +++---- .../PolytropicPneumaticSpring/SpringState.hpp | 2 +- .../src/SinusoidalPiston/SinusoidalPiston.cpp | 42 ++++++++-------- .../src/SinusoidalPiston/SinusoidalPiston.hpp | 18 +++---- .../PowerController/PowerController.cpp | 6 +-- .../ServicesNotImplemented/NoOpController.cpp | 2 +- .../SpringController/SpringController.cpp | 10 ++-- .../src/controllers/XBowAHRS/XBowAHRS.cpp | 2 +- buoy_tests/CMakeLists.txt | 3 +- .../fixture_server_sinusoidal_piston.cpp | 38 +++++++------- 18 files changed, 110 insertions(+), 109 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d5d85d08..6dfcd6a5 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -19,5 +19,5 @@ jobs: - name: Build and Test run: .github/workflows/build-and-test.sh env: - IGNITION_VERSION: ${{ matrix.gz-version }} + GZ_VERSION: ${{ matrix.gz-version }} ROS_DISTRO: ${{ matrix.ros-distro }} diff --git a/buoy_description/hooks/buoy_description.dsv.in b/buoy_description/hooks/buoy_description.dsv.in index be98b347..e9214ccf 100644 --- a/buoy_description/hooks/buoy_description.dsv.in +++ b/buoy_description/hooks/buoy_description.dsv.in @@ -1,2 +1,2 @@ -prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share -prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share/buoy_description/models +prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share +prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/buoy_description/models diff --git a/buoy_gazebo/hooks/buoy_gazebo.dsv.in b/buoy_gazebo/hooks/buoy_gazebo.dsv.in index a1a1e0d9..689a68c6 100644 --- a/buoy_gazebo/hooks/buoy_gazebo.dsv.in +++ b/buoy_gazebo/hooks/buoy_gazebo.dsv.in @@ -1,4 +1,4 @@ -prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share/@PROJECT_NAME@/worlds -prepend-non-duplicate;IGN_GAZEBO_SYSTEM_PLUGIN_PATH;@CMAKE_INSTALL_PREFIX@/lib -prepend-non-duplicate;IGN_GAZEBO_SERVER_CONFIG_PATH;share/@PROJECT_NAME@/gazebo/server.config +prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/@PROJECT_NAME@/worlds +prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;@CMAKE_INSTALL_PREFIX@/lib +prepend-non-duplicate;GZ_SIM_SERVER_CONFIG_PATH;share/@PROJECT_NAME@/gazebo/server.config set-if-unset;MESA_GL_VERSION_OVERRIDE;3.3 diff --git a/buoy_gazebo/launch/debugger.launch.py b/buoy_gazebo/launch/debugger.launch.py index 06bb6800..970af216 100644 --- a/buoy_gazebo/launch/debugger.launch.py +++ b/buoy_gazebo/launch/debugger.launch.py @@ -23,8 +23,8 @@ def generate_launch_description(): - env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': - ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''), + env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': + ':'.join([environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''), environ.get('LD_LIBRARY_PATH', default='')])} return LaunchDescription([ diff --git a/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py b/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py index 35170c7d..cf95aae3 100644 --- a/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py +++ b/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py @@ -70,13 +70,13 @@ def generate_launch_description(): executable='parameter_bridge', arguments=[ # Clock (Gazebo -> ROS2) - '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock', + '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock', # Joint states (Gazebo -> ROS2) ['/world/', LaunchConfiguration('world_name'), '/model/', model_name, '/joint_state', - '@', 'sensor_msgs/msg/JointState', '[', 'ignition.msgs.Model'], + '@', 'sensor_msgs/msg/JointState', '[', 'gz.msgs.Model'], # Link poses (Gazebo -> ROS2) - link_pose_gz_topic + '@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V', - link_pose_gz_topic + '_static@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V', + link_pose_gz_topic + '@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V', + link_pose_gz_topic + '_static@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V', ], remappings=[ (['/world/', LaunchConfiguration('world_name'), '/model/', model_name, '/joint_state'], diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 17e186fd..78c334d0 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -31,7 +31,7 @@ #include -#include +//#include #include #include @@ -104,7 +104,7 @@ void ElectroHydraulicPTO::Configure( { this->dataPtr->model = gz::sim::Model(_entity); if (!this->dataPtr->model.Valid(_ecm)) { - ignerr << "ElectroHydraulicPTO plugin should be attached to a model entity. " << + gzerr << "ElectroHydraulicPTO plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; return; } @@ -113,7 +113,7 @@ void ElectroHydraulicPTO::Configure( // Get params from SDF for Prismatic Joint. auto PrismaticJointName = _sdf->Get("PrismaticJointName"); if (PrismaticJointName.empty()) { - ignerr << "ElectroHydraulicPTO found an empty PrismaticJointName parameter. " << + gzerr << "ElectroHydraulicPTO found an empty PrismaticJointName parameter. " << "Failed to initialize."; return; } @@ -123,7 +123,7 @@ void ElectroHydraulicPTO::Configure( _ecm, PrismaticJointName); if (this->dataPtr->PrismaticJointEntity == gz::sim::kNullEntity) { - ignerr << "Joint with name [" << PrismaticJointName << "] not found. " << + gzerr << "Joint with name [" << PrismaticJointName << "] not found. " << "The ElectroHydraulicPTO may not influence this joint.\n"; return; } else { @@ -145,63 +145,63 @@ void ElectroHydraulicPTO::Configure( std::string pistonvel_topic = std::string("/pistonvel_") + PrismaticJointName; pistonvel_pub = node.Advertise(pistonvel_topic); if (!pistonvel_pub) { - ignerr << "Error advertising topic [" << pistonvel_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << pistonvel_topic << "]" << std::endl; return; } std::string rpm_topic = std::string("/rpm_") + PrismaticJointName; rpm_pub = node.Advertise(rpm_topic); if (!rpm_pub) { - ignerr << "Error advertising topic [" << rpm_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << rpm_topic << "]" << std::endl; return; } std::string deltaP_topic = std::string("/deltaP_") + PrismaticJointName; deltaP_pub = node.Advertise(deltaP_topic); if (!deltaP_pub) { - ignerr << "Error advertising topic [" << deltaP_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << deltaP_topic << "]" << std::endl; return; } std::string targwindcurr_topic = std::string("/targwindcurr_") + PrismaticJointName; targwindcurr_pub = node.Advertise(targwindcurr_topic); if (!targwindcurr_pub) { - ignerr << "Error advertising topic [" << targwindcurr_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << targwindcurr_topic << "]" << std::endl; return; } std::string windcurr_topic = std::string("/windcurr_") + PrismaticJointName; windcurr_pub = node.Advertise(windcurr_topic); if (!windcurr_pub) { - ignerr << "Error advertising topic [" << windcurr_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << windcurr_topic << "]" << std::endl; return; } std::string battcurr_topic = std::string("/battcurr_") + PrismaticJointName; battcurr_pub = node.Advertise(battcurr_topic); if (!battcurr_pub) { - ignerr << "Error advertising topic [" << battcurr_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << battcurr_topic << "]" << std::endl; return; } std::string loadcurr_topic = std::string("/loadcurr_") + PrismaticJointName; loadcurr_pub = node.Advertise(loadcurr_topic); if (!loadcurr_pub) { - ignerr << "Error advertising topic [" << loadcurr_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << loadcurr_topic << "]" << std::endl; return; } std::string scalefactor_topic = std::string("/scalefactor_") + PrismaticJointName; scalefactor_pub = node.Advertise(scalefactor_topic); if (!scalefactor_pub) { - ignerr << "Error advertising topic [" << scalefactor_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << scalefactor_topic << "]" << std::endl; return; } std::string retractfactor_topic = std::string("/retractfactor_") + PrismaticJointName; retractfactor_pub = node.Advertise(retractfactor_topic); if (!retractfactor_pub) { - ignerr << "Error advertising topic [" << retractfactor_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << retractfactor_topic << "]" << std::endl; return; } } @@ -211,7 +211,7 @@ void ElectroHydraulicPTO::PreUpdate( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { - IGN_PROFILE("#ElectroHydraulicPTO::PreUpdate"); + GZ_PROFILE("#ElectroHydraulicPTO::PreUpdate"); // Nothing left to do if paused. if (_info.paused) { return; @@ -226,7 +226,7 @@ void ElectroHydraulicPTO::PreUpdate( // \TODO(anyone): Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { - ignwarn << "Detected jump back in time [" << + gzwarn << "Detected jump back in time [" << std::chrono::duration_cast(_info.dt).count() << "s]. System may not work properly." << std::endl; } @@ -409,39 +409,39 @@ void ElectroHydraulicPTO::PreUpdate( retractfactor.set_data(this->dataPtr->functor.I_Wind.RetractFactor); if (!pistonvel_pub.Publish(pistonvel)) { - ignerr << "could not publish pistonvel" << std::endl; + gzerr << "could not publish pistonvel" << std::endl; } if (!rpm_pub.Publish(rpm)) { - ignerr << "could not publish rpm" << std::endl; + gzerr << "could not publish rpm" << std::endl; } if (!deltaP_pub.Publish(deltap)) { - ignerr << "could not publish deltaP" << std::endl; + gzerr << "could not publish deltaP" << std::endl; } if (!targwindcurr_pub.Publish(targwindcurr)) { - ignerr << "could not publish targwindcurr" << std::endl; + gzerr << "could not publish targwindcurr" << std::endl; } if (!windcurr_pub.Publish(windcurr)) { - ignerr << "could not publish windcurr" << std::endl; + gzerr << "could not publish windcurr" << std::endl; } if (!battcurr_pub.Publish(battcurr)) { - ignerr << "could not publish battcurr" << std::endl; + gzerr << "could not publish battcurr" << std::endl; } if (!loadcurr_pub.Publish(loadcurr)) { - ignerr << "could not publish loadcurr" << std::endl; + gzerr << "could not publish loadcurr" << std::endl; } if (!scalefactor_pub.Publish(scalefactor)) { - ignerr << "could not publish scalefactor" << std::endl; + gzerr << "could not publish scalefactor" << std::endl; } if (!retractfactor_pub.Publish(retractfactor)) { - ignerr << "could not publish retractfactor" << std::endl; + gzerr << "could not publish retractfactor" << std::endl; } @@ -463,7 +463,7 @@ void ElectroHydraulicPTO::PreUpdate( } } // namespace buoy_gazebo -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( buoy_gazebo::ElectroHydraulicPTO, gz::sim::System, buoy_gazebo::ElectroHydraulicPTO::ISystemConfigure, diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp index 4750ed67..afdf8b6b 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicState.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicState.hpp index 27efd988..954c059e 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicState.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicState.hpp @@ -100,7 +100,7 @@ namespace components using ElectroHydraulicState = gz::sim::components::Component; -IGN_GAZEBO_REGISTER_COMPONENT( +GZ_SIM_REGISTER_COMPONENT( "buoy_gazebo.components.ElectroHydraulicState", ElectroHydraulicState) } // namespace components diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index 66a65511..16d9d525 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -168,10 +168,10 @@ void PolytropicPneumaticSpring::openValve( double & P1, const double & V1, double & P2, const double & V2) { - IGN_ASSERT(V1 >= this->dataPtr->config_->dead_volume, "volume of chamber must be >= dead volume"); - IGN_ASSERT(V2 >= this->dataPtr->config_->dead_volume, "volume of chamber must be >= dead volume"); + GZ_ASSERT(V1 >= this->dataPtr->config_->dead_volume, "volume of chamber must be >= dead volume"); + GZ_ASSERT(V2 >= this->dataPtr->config_->dead_volume, "volume of chamber must be >= dead volume"); - const double dt_sec = dt_nano * IGN_NANO_TO_SEC; + const double dt_sec = dt_nano * GZ_NANO_TO_SEC; // want piston to drop 1 inch per second // so let mass flow from lower chamber to upper @@ -208,10 +208,10 @@ void PolytropicPneumaticSpring::pumpOn( double & P1, const double & V1, double & P2, const double & V2) { - IGN_ASSERT(V1 >= this->dataPtr->config_->dead_volume, "volume of chamber must be >= dead volume"); - IGN_ASSERT(V2 >= this->dataPtr->config_->dead_volume, "volume of chamber must be >= dead volume"); + GZ_ASSERT(V1 >= this->dataPtr->config_->dead_volume, "volume of chamber must be >= dead volume"); + GZ_ASSERT(V2 >= this->dataPtr->config_->dead_volume, "volume of chamber must be >= dead volume"); - const double dt_sec = dt_nano * IGN_NANO_TO_SEC; + const double dt_sec = dt_nano * GZ_NANO_TO_SEC; // want piston to raise 2 inches per minute // so pump mass flow from upper chamber to lower @@ -422,7 +422,7 @@ void PolytropicPneumaticSpring::PreUpdate( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { - IGN_PROFILE("PolytropicPneumaticSpring::PreUpdate"); + GZ_PROFILE("PolytropicPneumaticSpring::PreUpdate"); // If the joint hasn't been identified yet, the plugin is disabled if (this->dataPtr->config_->jointEntity == gz::sim::kNullEntity) { @@ -609,7 +609,7 @@ void PolytropicPneumaticSpring::PreUpdate( double period = 2.0; // sec double piston_velocity = this->dataPtr->config_->stroke * cos( - 2.0 * IGN_PI * + 2.0 * GZ_PI * std::chrono::duration_cast(_info.simTime).count() / period); auto joint_vel = _ecm.Component( this->dataPtr->config_->jointEntity); @@ -624,7 +624,7 @@ void PolytropicPneumaticSpring::PreUpdate( } } // namespace buoy_gazebo -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( buoy_gazebo::PolytropicPneumaticSpring, gz::sim::System, buoy_gazebo::PolytropicPneumaticSpring::ISystemConfigure, diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/SpringState.hpp b/buoy_gazebo/src/PolytropicPneumaticSpring/SpringState.hpp index b47408b4..bc4487b6 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/SpringState.hpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/SpringState.hpp @@ -70,7 +70,7 @@ namespace components { using SpringState = gz::sim::components::Component; -IGN_GAZEBO_REGISTER_COMPONENT("buoy_gazebo.components.SpringState", SpringState) +GZ_SIM_REGISTER_COMPONENT("buoy_gazebo.components.SpringState", SpringState) } // namespace components } // namespace buoy_gazebo diff --git a/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.cpp b/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.cpp index a1a09d19..7ba38753 100644 --- a/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.cpp +++ b/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.cpp @@ -14,11 +14,11 @@ #include "SinusoidalPiston.hpp" -#include -#include +#include +#include -#include -#include +#include +#include #include #include @@ -34,10 +34,10 @@ struct SinusoidalPistonPrivate double stroke{2.03}; /// \brief Joint Entity - ignition::gazebo::Entity jointEntity; + gz::sim::Entity jointEntity; /// \brief Model interface - ignition::gazebo::Model model{ignition::gazebo::kNullEntity}; + gz::sim::Model model{gz::sim::kNullEntity}; }; ///////////////////////////////////////////////// @@ -57,12 +57,12 @@ SinusoidalPiston::SinusoidalPiston() ////////////////////////////////////////////////// void SinusoidalPiston::Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & /*_eventMgr*/) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & /*_eventMgr*/) { - this->dataPtr->model = ignition::gazebo::Model(_entity); + this->dataPtr->model = gz::sim::Model(_entity); if (!this->dataPtr->model.Valid(_ecm)) { ignerr << "SinusoidalPiston plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; @@ -82,7 +82,7 @@ void SinusoidalPiston::Configure( this->dataPtr->jointEntity = this->dataPtr->model.JointByName( _ecm, jointName); - if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity) { + if (this->dataPtr->jointEntity == gz::sim::kNullEntity) { ignerr << "Joint with name[" << jointName << "] not found. " << "The SinusoidalPiston may not influence this joint.\n"; return; @@ -91,13 +91,13 @@ void SinusoidalPiston::Configure( ////////////////////////////////////////////////// void SinusoidalPiston::PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) { - IGN_PROFILE("SinusoidalPiston::PreUpdate"); + GZ_PROFILE("SinusoidalPiston::PreUpdate"); // If the joint hasn't been identified yet, the plugin is disabled - if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity) { + if (this->dataPtr->jointEntity == gz::sim::kNullEntity) { return; } @@ -116,25 +116,25 @@ void SinusoidalPiston::PreUpdate( static const double T{10000.0}; // milliseconds static const double m{150.0}; // kg static const double shift{0.5}; // meters - const double a = -2.0 * IGN_PI * IGN_PI * this->dataPtr->stroke * (sin( - 2.0 * IGN_PI * + const double a = -2.0 * GZ_PI * GZ_PI * this->dataPtr->stroke * (sin( + 2.0 * GZ_PI * std::chrono::duration_cast( _info.simTime).count() / T) + shift); auto forceComp = - _ecm.Component( + _ecm.Component( this->dataPtr->jointEntity); if (forceComp == nullptr) { _ecm.CreateComponent( this->dataPtr->jointEntity, - ignition::gazebo::components::JointForceCmd({m * a})); + gz::sim::components::JointForceCmd({m * a})); } else { forceComp->Data()[0] += m * a; // Add force to existing forces. } } } // namespace buoy_gazebo -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( buoy_gazebo::SinusoidalPiston, - ignition::gazebo::System, + gz::sim::System, buoy_gazebo::SinusoidalPiston::ISystemConfigure, buoy_gazebo::SinusoidalPiston::ISystemPreUpdate) diff --git a/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.hpp b/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.hpp index 12dc8158..897b386e 100644 --- a/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.hpp +++ b/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.hpp @@ -15,7 +15,7 @@ #ifndef SINUSOIDALPISTON__SINUSOIDALPISTON_HPP_ #define SINUSOIDALPISTON__SINUSOIDALPISTON_HPP_ -#include +#include #include @@ -24,9 +24,9 @@ namespace buoy_gazebo { struct SinusoidalPistonPrivate; -class SinusoidalPiston : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPreUpdate +class SinusoidalPiston : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate { public: /// \brief Constructor @@ -37,15 +37,15 @@ class SinusoidalPiston : public ignition::gazebo::System, // Documentation inherited void Configure( - const ignition::gazebo::Entity & _entity, + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & _eventMgr) override; + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; // Documentation inherited void PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) override; + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) override; private: /// \brief Private data pointer diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index d0b852f5..ecb1d09a 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -680,7 +680,7 @@ void PowerController::PreUpdate( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { - IGN_PROFILE("PowerController::PreUpdate"); + GZ_PROFILE("PowerController::PreUpdate"); this->dataPtr->paused_ = _info.paused; this->dataPtr->current_time_ = _info.simTime; @@ -716,7 +716,7 @@ void PowerController::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - IGN_PROFILE("PowerController::PostUpdate"); + GZ_PROFILE("PowerController::PostUpdate"); this->dataPtr->paused_ = _info.paused; this->dataPtr->current_time_ = _info.simTime; @@ -774,7 +774,7 @@ void PowerController::PostUpdate( } } // namespace buoy_gazebo -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( buoy_gazebo::PowerController, gz::sim::System, buoy_gazebo::PowerController::ISystemConfigure, diff --git a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp index c25ad398..36d93fe1 100644 --- a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp +++ b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp @@ -384,7 +384,7 @@ void NoOpController::Configure( } // namespace buoy_gazebo -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( buoy_gazebo::NoOpController, gz::sim::System, buoy_gazebo::NoOpController::ISystemConfigure) diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp index 63a79a81..6f24fbb7 100644 --- a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp +++ b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp @@ -328,7 +328,7 @@ struct SpringControllerPrivate services_->command_watch_.ElapsedRunTime().seconds() << "s)"); igndbg << "piston moved: " << (state.range_finder - init_x) / - (services_->command_watch_.ElapsedRunTime().nanoseconds() * IGN_NANO_TO_SEC) << + (services_->command_watch_.ElapsedRunTime().nanoseconds() * GZ_NANO_TO_SEC) << " m/s" << std::endl; } @@ -346,7 +346,7 @@ struct SpringControllerPrivate services_->command_watch_.ElapsedRunTime().seconds() << "s)"); igndbg << "piston moved: " << (state.range_finder - init_x) / - (services_->command_watch_.ElapsedRunTime().nanoseconds() * IGN_NANO_TO_SEC) << + (services_->command_watch_.ElapsedRunTime().nanoseconds() * GZ_NANO_TO_SEC) << " m/s" << std::endl; } else { // set pump toggle -- linear pump servo drives back and forth @@ -564,7 +564,7 @@ void SpringController::PreUpdate( const gz::sim::UpdateInfo & _info, gz::sim::EntityComponentManager & _ecm) { - IGN_PROFILE("SpringController::PreUpdate"); + GZ_PROFILE("SpringController::PreUpdate"); this->dataPtr->paused_ = _info.paused; this->dataPtr->current_time_ = _info.simTime; @@ -601,7 +601,7 @@ void SpringController::PostUpdate( const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) { - IGN_PROFILE("SpringController::PostUpdate"); + GZ_PROFILE("SpringController::PostUpdate"); this->dataPtr->paused_ = _info.paused; this->dataPtr->current_time_ = _info.simTime; @@ -649,7 +649,7 @@ void SpringController::PostUpdate( } } // namespace buoy_gazebo -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( buoy_gazebo::SpringController, gz::sim::System, buoy_gazebo::SpringController::ISystemConfigure, diff --git a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp index e52c2708..a26f4d99 100644 --- a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp +++ b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp @@ -76,7 +76,7 @@ struct buoy_gazebo::XBowAHRSPrivate }; -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( buoy_gazebo::XBowAHRS, gz::sim::System, buoy_gazebo::XBowAHRS::ISystemConfigure, diff --git a/buoy_tests/CMakeLists.txt b/buoy_tests/CMakeLists.txt index b4526861..b1f205c2 100644 --- a/buoy_tests/CMakeLists.txt +++ b/buoy_tests/CMakeLists.txt @@ -84,7 +84,8 @@ if(BUILD_TESTING) target_link_libraries(${GTEST_NAME} gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ) - rosidl_target_interfaces(${GTEST_NAME} ${PROJECT_NAME} "rosidl_typesupport_cpp") + rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp") + target_link_libraries(${GTEST_NAME} "${cpp_typesupport_target}") ament_target_dependencies(${GTEST_NAME} rclcpp) include_directories(${CMAKE_CURRENT_BINARY_DIR}) diff --git a/buoy_tests/tests/fixture_server_sinusoidal_piston.cpp b/buoy_tests/tests/fixture_server_sinusoidal_piston.cpp index 1b6760f8..a46c5e87 100644 --- a/buoy_tests/tests/fixture_server_sinusoidal_piston.cpp +++ b/buoy_tests/tests/fixture_server_sinusoidal_piston.cpp @@ -16,12 +16,12 @@ #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include @@ -34,39 +34,39 @@ TEST(BuoyTests, RunServer) { // Skip debug messages to run faster - ignition::common::Console::SetVerbosity(3); + gz::common::Console::SetVerbosity(3); // Setup fixture - ignition::gazebo::ServerConfig config; + gz::sim::ServerConfig config; config.SetSdfFile("mbari_wec_sinusoidal_piston.sdf"); config.SetUpdateRate(0.0); size_t iterations{0U}; - ignition::gazebo::Entity buoyEntity{ignition::gazebo::kNullEntity}; - std::unique_ptr fixture = - std::make_unique(config); + gz::sim::Entity buoyEntity{gz::sim::kNullEntity}; + std::unique_ptr fixture = + std::make_unique(config); fixture-> OnConfigure( [&]( - const ignition::gazebo::Entity & _worldEntity, + const gz::sim::Entity & _worldEntity, const std::shared_ptr &, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { - auto world = ignition::gazebo::World(_worldEntity); + auto world = gz::sim::World(_worldEntity); buoyEntity = world.ModelByName(_ecm, "MBARI_WEC_SINUSOIDAL_PISTON"); - EXPECT_NE(ignition::gazebo::kNullEntity, buoyEntity); + EXPECT_NE(gz::sim::kNullEntity, buoyEntity); }). OnPostUpdate( [&]( - const ignition::gazebo::UpdateInfo &, - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo &, + const gz::sim::EntityComponentManager & _ecm) { iterations++; - auto pose = ignition::gazebo::worldPose(buoyEntity, _ecm); + auto pose = gz::sim::worldPose(buoyEntity, _ecm); // Expect buoy to stay more or less in the same place horizontally. EXPECT_LT(-0.001, pose.Pos().X()); From c79deaf23ee0f293453fcd89e137cb79d058f5be Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 29 Aug 2022 16:36:26 -0700 Subject: [PATCH 05/57] include order Signed-off-by: Louise Poubel --- .../ElectroHydraulicPTO.cpp | 26 +++++++++---------- .../ElectroHydraulicPTO.hpp | 4 +-- .../WindingCurrentTarget.hpp | 4 +-- .../PolytropicPneumaticSpring.cpp | 15 +++++------ .../PolytropicPneumaticSpring.hpp | 5 ++-- .../src/SinusoidalPiston/SinusoidalPiston.cpp | 6 ++--- .../src/SinusoidalPiston/SinusoidalPiston.hpp | 3 +-- .../src/buoy_utils/StopwatchSimTime.cpp | 7 +++-- .../src/buoy_utils/StopwatchSimTime.hpp | 3 +-- .../PowerController/PowerController.cpp | 18 ++++++------- .../PowerController/PowerController.hpp | 3 ++- .../ServicesNotImplemented/NoOpController.cpp | 7 +++-- .../ServicesNotImplemented/NoOpController.hpp | 3 ++- .../SpringController/SpringController.cpp | 19 +++++++------- .../SpringController/SpringController.hpp | 3 ++- .../src/controllers/XBowAHRS/XBowAHRS.cpp | 13 +++++----- .../src/controllers/XBowAHRS/XBowAHRS.hpp | 3 ++- 17 files changed, 70 insertions(+), 72 deletions(-) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 78c334d0..4bf9bc7d 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -12,7 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ElectroHydraulicPTO.hpp" +#include + +#include + +#include +#include +#include +#include +#include +#include + #include #include #include @@ -25,23 +35,13 @@ #include #include #include -#include #include #include -#include - -//#include - -#include -#include -#include -#include -#include -#include -#include "ElectroHydraulicState.hpp" +#include "ElectroHydraulicPTO.hpp" #include "ElectroHydraulicSoln.hpp" +#include "ElectroHydraulicState.hpp" #include "JustInterp/JustInterp.hpp" diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.hpp index 21568ca3..7408144c 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.hpp @@ -15,11 +15,11 @@ #ifndef ELECTROHYDRAULICPTO__ELECTROHYDRAULICPTO_HPP_ #define ELECTROHYDRAULICPTO__ELECTROHYDRAULICPTO_HPP_ +#include #include #include -#include -#include +#include namespace buoy_gazebo { diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp index d14ab192..bbf7d2fa 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp @@ -17,13 +17,13 @@ #include -#include - #include #include #include #include +#include + // Defines from Controller Firmware, behavior replicated here #define TORQUE_CONSTANT 0.438 // 0.62 N-m/ARMS 0.428N-m/AMPS Flux Current #define CURRENT_CMD_RATELIMIT 200 // A/second. Set to zero to disable feature diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index 16d9d525..864a9741 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -12,11 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "PolytropicPneumaticSpring.hpp" +#include +#include + +#include +#include +#include #include #include -#include #include #include #include @@ -27,12 +31,7 @@ #include #include -#include - -#include -#include -#include - +#include "PolytropicPneumaticSpring.hpp" #include "SpringState.hpp" diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.hpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.hpp index 46b10efe..50b3e5ee 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.hpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.hpp @@ -15,11 +15,10 @@ #ifndef POLYTROPICPNEUMATICSPRING__POLYTROPICPNEUMATICSPRING_HPP_ #define POLYTROPICPNEUMATICSPRING__POLYTROPICPNEUMATICSPRING_HPP_ -#include -#include - #include +#include +#include namespace buoy_gazebo { diff --git a/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.cpp b/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.cpp index 7ba38753..3ce21e0f 100644 --- a/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.cpp +++ b/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.cpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "SinusoidalPiston.hpp" +#include +#include #include #include @@ -20,8 +21,7 @@ #include #include -#include -#include +#include "SinusoidalPiston.hpp" using namespace std::chrono_literals; diff --git a/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.hpp b/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.hpp index 897b386e..86f73b03 100644 --- a/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.hpp +++ b/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.hpp @@ -15,10 +15,9 @@ #ifndef SINUSOIDALPISTON__SINUSOIDALPISTON_HPP_ #define SINUSOIDALPISTON__SINUSOIDALPISTON_HPP_ -#include - #include +#include namespace buoy_gazebo { diff --git a/buoy_gazebo/src/buoy_utils/StopwatchSimTime.cpp b/buoy_gazebo/src/buoy_utils/StopwatchSimTime.cpp index 1f468b96..5f56c72b 100644 --- a/buoy_gazebo/src/buoy_utils/StopwatchSimTime.cpp +++ b/buoy_gazebo/src/buoy_utils/StopwatchSimTime.cpp @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "StopwatchSimTime.hpp" - -#include - #include #include #include +#include + +#include "StopwatchSimTime.hpp" namespace buoy_utils { diff --git a/buoy_gazebo/src/buoy_utils/StopwatchSimTime.hpp b/buoy_gazebo/src/buoy_utils/StopwatchSimTime.hpp index d2eb4ab7..1356c4af 100644 --- a/buoy_gazebo/src/buoy_utils/StopwatchSimTime.hpp +++ b/buoy_gazebo/src/buoy_utils/StopwatchSimTime.hpp @@ -15,11 +15,10 @@ #ifndef BUOY_UTILS__STOPWATCHSIMTIME_HPP_ #define BUOY_UTILS__STOPWATCHSIMTIME_HPP_ -#include - #include #include +#include namespace buoy_utils { diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index ecb1d09a..0d0cc353 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -12,14 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "PowerController.hpp" +#include + +#include +#include +#include +#include +#include +#include #include #include #include #include #include -#include #include #include @@ -32,15 +38,9 @@ #include #include -#include -#include -#include -#include -#include -#include - #include "buoy_utils/StopwatchSimTime.hpp" #include "ElectroHydraulicPTO/ElectroHydraulicState.hpp" +#include "PowerController.hpp" using namespace std::chrono_literals; diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.hpp b/buoy_gazebo/src/controllers/PowerController/PowerController.hpp index ad861297..7346e302 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.hpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.hpp @@ -15,9 +15,10 @@ #ifndef CONTROLLERS__POWERCONTROLLER__POWERCONTROLLER_HPP_ #define CONTROLLERS__POWERCONTROLLER__POWERCONTROLLER_HPP_ -#include #include +#include + namespace buoy_gazebo { // Forward declarations. diff --git a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp index 36d93fe1..e1801e54 100644 --- a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp +++ b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "NoOpController.hpp" +#include +#include #include #include @@ -24,9 +25,6 @@ #include -#include -#include - #include "buoy_interfaces/srv/bc_reset_command.hpp" #include "buoy_interfaces/srv/bender_command.hpp" #include "buoy_interfaces/srv/bus_command.hpp" @@ -47,6 +45,7 @@ #include "buoy_interfaces/srv/tf_set_state_machine_command.hpp" #include "buoy_interfaces/srv/tf_watch_dog_command.hpp" +#include "NoOpController.hpp" #define CREATE_SERVICE(type, prefix, cmd_var, service, \ cmd_type, range_type) \ diff --git a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.hpp b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.hpp index 0b341b8c..24b982b8 100644 --- a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.hpp +++ b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.hpp @@ -15,9 +15,10 @@ #ifndef CONTROLLERS__SERVICESNOTIMPLEMENTED__NOOPCONTROLLER_HPP_ #define CONTROLLERS__SERVICESNOTIMPLEMENTED__NOOPCONTROLLER_HPP_ -#include #include +#include + namespace buoy_gazebo { // Forward declarations. diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp index 6f24fbb7..d44537f9 100644 --- a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp +++ b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp @@ -12,14 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "SpringController.hpp" +#include + +#include +#include +#include +#include +#include +#include #include #include #include #include #include -#include #include #include @@ -29,16 +35,9 @@ #include #include -#include -#include -#include -#include -#include -#include - #include "buoy_utils/StopwatchSimTime.hpp" #include "PolytropicPneumaticSpring/SpringState.hpp" - +#include "SpringController.hpp" using namespace std::chrono_literals; diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.hpp b/buoy_gazebo/src/controllers/SpringController/SpringController.hpp index ad90b04b..043c0e68 100644 --- a/buoy_gazebo/src/controllers/SpringController/SpringController.hpp +++ b/buoy_gazebo/src/controllers/SpringController/SpringController.hpp @@ -15,9 +15,10 @@ #ifndef CONTROLLERS__SPRINGCONTROLLER__SPRINGCONTROLLER_HPP_ #define CONTROLLERS__SPRINGCONTROLLER__SPRINGCONTROLLER_HPP_ -#include #include +#include + namespace buoy_gazebo { // Forward declarations. diff --git a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp index a26f4d99..f45dd684 100644 --- a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp +++ b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp @@ -12,14 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "XBowAHRS.hpp" +#include + +#include +#include +#include +#include #include #include #include #include #include -#include #include #include @@ -27,10 +31,7 @@ #include #include -#include -#include -#include -#include +#include "XBowAHRS.hpp" struct buoy_gazebo::XBowAHRSPrivate { diff --git a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.hpp b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.hpp index fdc97314..4122064a 100644 --- a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.hpp +++ b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.hpp @@ -15,9 +15,10 @@ #ifndef CONTROLLERS__XBOWAHRS__XBOWAHRS_HPP_ #define CONTROLLERS__XBOWAHRS__XBOWAHRS_HPP_ -#include #include +#include + namespace buoy_gazebo { // Forward declarations. From 9ca432efa2e28c0bc30a166481b833bda3bb0d3a Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 29 Aug 2022 16:44:25 -0700 Subject: [PATCH 06/57] more gz, uncrustify Signed-off-by: Louise Poubel --- .../PolytropicPneumaticSpring.cpp | 46 +++++++++---------- .../src/SinusoidalPiston/SinusoidalPiston.cpp | 8 ++-- .../PowerController/PowerController.cpp | 6 +-- .../ServicesNotImplemented/NoOpController.cpp | 2 +- .../SpringController/SpringController.cpp | 16 +++---- .../src/controllers/XBowAHRS/XBowAHRS.cpp | 4 +- 6 files changed, 41 insertions(+), 41 deletions(-) diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index 864a9741..d1d943d6 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -268,10 +268,10 @@ void PolytropicPneumaticSpring::Configure( PolytropicPneumaticSpringConfig config; config.name = _sdf->Get("chamber", "upper_adiabatic").first; - igndbg << "name: " << config.name << std::endl; + gzdbg << "name: " << config.name << std::endl; config.is_upper = _sdf->Get("is_upper"); - igndbg << "is upper? " << std::boolalpha << config.is_upper << std::noboolalpha << std::endl; + gzdbg << "is upper? " << std::boolalpha << config.is_upper << std::noboolalpha << std::endl; config.valve_absement = SdfParamDouble(_sdf, "valve_absement", config.valve_absement); config.pump_absement = SdfParamDouble(_sdf, "pump_absement", config.pump_absement); @@ -308,16 +308,16 @@ void PolytropicPneumaticSpring::Configure( config.V1 = config.dead_volume + config.x1 * config.piston_area; - igndbg << "V1: " << config.V1 << std::endl; + gzdbg << "V1: " << config.V1 << std::endl; config.V2 = config.dead_volume + config.x2 * config.piston_area; - igndbg << "V2: " << config.V2 << std::endl; + gzdbg << "V2: " << config.V2 << std::endl; this->dataPtr->V0 = config.V1; config.c = this->dataPtr->P1 * config.V1 / config.T0; - igndbg << "c: " << config.c << std::endl; + gzdbg << "c: " << config.c << std::endl; this->dataPtr->mass = config.c / config.R; - igndbg << "mass: " << this->dataPtr->mass << std::endl; + gzdbg << "mass: " << this->dataPtr->mass << std::endl; } else { // no hysteresis config.n0 = SdfParamDouble(_sdf, "n", PolytropicPneumaticSpringConfig::ADIABATIC_INDEX); if (fabs(config.n0 - PolytropicPneumaticSpringConfig::ADIABATIC_INDEX) < 1.0e-7) { @@ -332,16 +332,16 @@ void PolytropicPneumaticSpring::Configure( config.x0 * config.piston_area; this->dataPtr->V0 = config.V0; - igndbg << "V0: " << config.V0 << std::endl; + gzdbg << "V0: " << config.V0 << std::endl; config.c = this->dataPtr->P0 * config.V0 / config.T0; - igndbg << "c: " << config.c << std::endl; + gzdbg << "c: " << config.c << std::endl; this->dataPtr->mass = config.c / config.R; - igndbg << "mass: " << this->dataPtr->mass << std::endl; + gzdbg << "mass: " << this->dataPtr->mass << std::endl; } config.model = gz::sim::Model(_entity); if (!config.model.Valid(_ecm)) { - ignerr << "PolytropicPneumaticSpring plugin should be attached to a model entity. " << + gzerr << "PolytropicPneumaticSpring plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; return; } @@ -349,7 +349,7 @@ void PolytropicPneumaticSpring::Configure( // Get params from SDF auto jointName = _sdf->Get("JointName"); if (jointName.empty()) { - ignerr << "PolytropicPneumaticSpring found an empty jointName parameter. " << + gzerr << "PolytropicPneumaticSpring found an empty jointName parameter. " << "Failed to initialize."; return; } @@ -358,7 +358,7 @@ void PolytropicPneumaticSpring::Configure( _ecm, jointName); if (config.jointEntity == gz::sim::kNullEntity) { - ignerr << "Joint with name[" << jointName << "] not found. " << + gzerr << "Joint with name[" << jointName << "] not found. " << "The PolytropicPneumaticSpring may not influence this joint.\n"; return; } @@ -371,7 +371,7 @@ void PolytropicPneumaticSpring::Configure( gz::transport::TopicUtils::AsValidTopic( force_topic)); if (!force_pub) { - ignerr << "Error advertising topic [" << force_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << force_topic << "]" << std::endl; return; } @@ -381,7 +381,7 @@ void PolytropicPneumaticSpring::Configure( gz::transport::TopicUtils::AsValidTopic( pressure_topic)); if (!pressure_pub) { - ignerr << "Error advertising topic [" << pressure_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << pressure_topic << "]" << std::endl; return; } @@ -391,7 +391,7 @@ void PolytropicPneumaticSpring::Configure( gz::transport::TopicUtils::AsValidTopic( volume_topic)); if (!volume_pub) { - ignerr << "Error advertising topic [" << volume_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << volume_topic << "]" << std::endl; return; } @@ -401,7 +401,7 @@ void PolytropicPneumaticSpring::Configure( gz::transport::TopicUtils::AsValidTopic( temperature_topic)); if (!temperature_pub) { - ignerr << "Error advertising topic [" << temperature_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << temperature_topic << "]" << std::endl; return; } @@ -411,7 +411,7 @@ void PolytropicPneumaticSpring::Configure( gz::transport::TopicUtils::AsValidTopic( heat_rate_topic)); if (!heat_rate_pub) { - ignerr << "Error advertising topic [" << heat_rate_topic << "]" << std::endl; + gzerr << "Error advertising topic [" << heat_rate_topic << "]" << std::endl; return; } } @@ -430,7 +430,7 @@ void PolytropicPneumaticSpring::PreUpdate( // TODO(anyone): Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { - ignwarn << "Detected jump back in time [" << + gzwarn << "Detected jump back in time [" << std::chrono::duration_cast(_info.dt).count() << "s]. System may not work properly." << std::endl; } @@ -573,23 +573,23 @@ void PolytropicPneumaticSpring::PreUpdate( piston_velocity.set_data(v); if (!force_pub.Publish(force)) { - ignerr << "could not publish force" << std::endl; + gzerr << "could not publish force" << std::endl; } if (!pressure_pub.Publish(pressure)) { - ignerr << "could not publish pressure" << std::endl; + gzerr << "could not publish pressure" << std::endl; } if (!volume_pub.Publish(volume)) { - ignerr << "could not publish volume" << std::endl; + gzerr << "could not publish volume" << std::endl; } if (!temperature_pub.Publish(temperature)) { - ignerr << "could not publish temperature" << std::endl; + gzerr << "could not publish temperature" << std::endl; } if (!heat_rate_pub.Publish(heat_rate)) { - ignerr << "could not publish heat loss rate" << std::endl; + gzerr << "could not publish heat loss rate" << std::endl; } static const bool debug_prescribed_velocity{this->dataPtr->config_->debug_prescribed_velocity}; diff --git a/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.cpp b/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.cpp index 3ce21e0f..04d0b6ea 100644 --- a/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.cpp +++ b/buoy_gazebo/src/SinusoidalPiston/SinusoidalPiston.cpp @@ -64,7 +64,7 @@ void SinusoidalPiston::Configure( { this->dataPtr->model = gz::sim::Model(_entity); if (!this->dataPtr->model.Valid(_ecm)) { - ignerr << "SinusoidalPiston plugin should be attached to a model entity. " << + gzerr << "SinusoidalPiston plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; return; } @@ -74,7 +74,7 @@ void SinusoidalPiston::Configure( auto jointName = _sdf->Get("JointName"); if (jointName.empty()) { - ignerr << "SinusoidalPiston found an empty jointName parameter. " << + gzerr << "SinusoidalPiston found an empty jointName parameter. " << "Failed to initialize."; return; } @@ -83,7 +83,7 @@ void SinusoidalPiston::Configure( _ecm, jointName); if (this->dataPtr->jointEntity == gz::sim::kNullEntity) { - ignerr << "Joint with name[" << jointName << "] not found. " << + gzerr << "Joint with name[" << jointName << "] not found. " << "The SinusoidalPiston may not influence this joint.\n"; return; } @@ -103,7 +103,7 @@ void SinusoidalPiston::PreUpdate( // TODO(anyone): Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { - ignwarn << "Detected jump back in time [" << + gzwarn << "Detected jump back in time [" << std::chrono::duration_cast(_info.dt).count() << "s]. System may not work properly." << std::endl; } diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 0d0cc353..cbc7f239 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -585,7 +585,7 @@ void PowerController::Configure( // Make sure the controller is attached to a valid model auto model = gz::sim::Model(_entity); if (!model.Valid(_ecm)) { - ignerr << "[ROS 2 Spring Control] Failed to initialize because [" << + gzerr << "[ROS 2 Spring Control] Failed to initialize because [" << model.Name(_ecm) << "] is not a model." << std::endl << "Please make sure that ROS 2 Spring Control is attached to a valid model." << std::endl; return; @@ -596,14 +596,14 @@ void PowerController::Configure( // Get params from SDF auto jointName = _sdf->Get("JointName"); if (jointName.empty()) { - ignerr << "PowerController found an empty JointName parameter. " << + gzerr << "PowerController found an empty JointName parameter. " << "Failed to initialize."; return; } this->dataPtr->jointEntity_ = model.JointByName(_ecm, jointName); if (this->dataPtr->jointEntity_ == gz::sim::kNullEntity) { - ignerr << "Joint with name[" << jointName << "] not found. " << + gzerr << "Joint with name[" << jointName << "] not found. " << "The PowerController may not influence this joint.\n"; return; } diff --git a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp index e1801e54..81033d61 100644 --- a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp +++ b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp @@ -356,7 +356,7 @@ void NoOpController::Configure( // Make sure the controller is attached to a valid model auto model = gz::sim::Model(_entity); if (!model.Valid(_ecm)) { - ignerr << "[ROS 2 NoOp Controller] Failed to initialize because [" << + gzerr << "[ROS 2 NoOp Controller] Failed to initialize because [" << model.Name(_ecm) << "] is not a model." << std::endl << "Please make sure that ROS 2 NoOp Controller is attached to a valid model." << std::endl; return; diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp index d44537f9..05f7885b 100644 --- a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp +++ b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp @@ -326,8 +326,8 @@ struct SpringControllerPrivate "Valve closed after (" << services_->command_watch_.ElapsedRunTime().seconds() << "s)"); - igndbg << "piston moved: " << (state.range_finder - init_x) / - (services_->command_watch_.ElapsedRunTime().nanoseconds() * GZ_NANO_TO_SEC) << + gzdbg << "piston moved: " << (state.range_finder - init_x) / + (services_->command_watch_.ElapsedRunTime().nanoseconds() * GZ_NANO_TO_SEC) << " m/s" << std::endl; } @@ -344,8 +344,8 @@ struct SpringControllerPrivate "Pump off after (" << services_->command_watch_.ElapsedRunTime().seconds() << "s)"); - igndbg << "piston moved: " << (state.range_finder - init_x) / - (services_->command_watch_.ElapsedRunTime().nanoseconds() * GZ_NANO_TO_SEC) << + gzdbg << "piston moved: " << (state.range_finder - init_x) / + (services_->command_watch_.ElapsedRunTime().nanoseconds() * GZ_NANO_TO_SEC) << " m/s" << std::endl; } else { // set pump toggle -- linear pump servo drives back and forth @@ -451,7 +451,7 @@ void SpringController::Configure( // Make sure the controller is attached to a valid model auto model = gz::sim::Model(_entity); if (!model.Valid(_ecm)) { - ignerr << "[ROS 2 Spring Control] Failed to initialize because [" << + gzerr << "[ROS 2 Spring Control] Failed to initialize because [" << model.Name(_ecm) << "] is not a model." << std::endl << "Please make sure that ROS 2 Spring Control is attached to a valid model." << std::endl; return; @@ -462,14 +462,14 @@ void SpringController::Configure( // Get params from SDF auto jointName = _sdf->Get("JointName"); if (jointName.empty()) { - ignerr << "SpringController found an empty JointName parameter. " << + gzerr << "SpringController found an empty JointName parameter. " << "Failed to initialize."; return; } this->dataPtr->jointEntity_ = model.JointByName(_ecm, jointName); if (this->dataPtr->jointEntity_ == gz::sim::kNullEntity) { - ignerr << "Joint with name[" << jointName << "] not found. " << + gzerr << "Joint with name[" << jointName << "] not found. " << "The SpringController may not influence this joint.\n"; return; } @@ -503,7 +503,7 @@ void SpringController::Configure( data.unlock(); }; if (!this->dataPtr->node_.Subscribe("/Universal_joint/force_torque", this->dataPtr->ft_cb_)) { - ignerr << "Error subscribing to topic [" << "/Universal_joint/force_torque" << "]" << std::endl; + gzerr << "Error subscribing to topic [" << "/Universal_joint/force_torque" << "]" << std::endl; return; } diff --git a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp index f45dd684..2c4b3e3d 100644 --- a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp +++ b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp @@ -109,7 +109,7 @@ void XBowAHRS::Configure( // Make sure the controller is attached to a valid model auto model = gz::sim::Model(_entity); if (!model.Valid(_ecm)) { - ignerr << "[ROS 2 XBow AHRS] Failed to initialize because [" << \ + gzerr << "[ROS 2 XBow AHRS] Failed to initialize because [" << \ model.Name(_ecm) << "] is not a model." << std::endl << \ "Please make sure that ROS 2 XBow AHRS is attached to a valid model." << std::endl; return; @@ -165,7 +165,7 @@ void XBowAHRS::Configure( data.unlock(); }; if (!this->dataPtr->node_.Subscribe("/Buoy_link/xbow_imu", this->dataPtr->imu_cb_)) { - ignerr << "Error subscribing to topic [" << "/Buoy_link/xbow_imu" << "]" << std::endl; + gzerr << "Error subscribing to topic [" << "/Buoy_link/xbow_imu" << "]" << std::endl; return; } From db712bc8fafe0d0aa7ab863b0e3465abce6035df Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Mon, 19 Sep 2022 13:55:24 -0700 Subject: [PATCH 07/57] ign-gz in ros_gz bridge Signed-off-by: Dharini Dutia --- buoy_gazebo/launch/mbari_wec.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buoy_gazebo/launch/mbari_wec.launch.py b/buoy_gazebo/launch/mbari_wec.launch.py index 0f0a4b0f..9feaa95a 100644 --- a/buoy_gazebo/launch/mbari_wec.launch.py +++ b/buoy_gazebo/launch/mbari_wec.launch.py @@ -56,7 +56,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py'), ), - launch_arguments={'ign_args': PathJoinSubstitution([ + launch_arguments={'gz_args': PathJoinSubstitution([ pkg_buoy_gazebo, 'worlds', LaunchConfiguration('world_file') From 3aaac4e1e419e92e858249102297cc7fb94dbfe5 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Fri, 27 May 2022 12:11:35 +0100 Subject: [PATCH 08/57] Update to ROS2 Humble and Gazebo Garden - Update library and namespaces for ign -> gz migration. - Use `python -m em` rather than `empy` for macOS support. - Update dependencies to ros_gz_bridge and ros_gz_sim Signed-off-by: Rhys Mainwaring --- buoy_description/CMakeLists.txt | 2 +- buoy_gazebo/CMakeLists.txt | 2 +- buoy_gazebo/launch/debugger.launch.py | 18 +++++++++--------- .../launch/electrohydraulic_pto.launch.py | 8 ++++---- buoy_gazebo/launch/mbari_wec.launch.py | 6 +++--- .../launch/mbari_wec_debugger.launch.py | 6 +++--- .../launch/mbari_wec_sinusoidal.launch.py | 8 ++++---- buoy_gazebo/package.xml | 5 +++-- .../ElectroHydraulicPTO.cpp | 4 +++- .../ElectroHydraulicPTO.hpp | 2 +- .../PowerController/PowerController.hpp | 2 +- .../SpringController/SpringController.hpp | 2 +- buoy_tests/launch/no_inputs_py.launch.py | 2 +- .../launch/no_inputs_ros_feedback.launch.py | 2 +- .../launch/pc_commands_ros_feedback.launch.py | 2 +- buoy_tests/launch/run_server.launch.py | 2 +- .../launch/sc_commands_ros_feedback.launch.py | 2 +- buoy_tests/testing_utils/utils.py | 2 +- buoy_tests/tests/test_fixture.py | 2 +- 19 files changed, 41 insertions(+), 38 deletions(-) diff --git a/buoy_description/CMakeLists.txt b/buoy_description/CMakeLists.txt index 2fe7319e..578ad0fb 100644 --- a/buoy_description/CMakeLists.txt +++ b/buoy_description/CMakeLists.txt @@ -7,7 +7,7 @@ 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 - COMMAND empy3 + COMMAND python -m em ${CMAKE_CURRENT_SOURCE_DIR}${BUOY_BASE_MODEL_PATH}/model.sdf.em > ${CMAKE_CURRENT_BINARY_DIR}${BUOY_BASE_MODEL_PATH}/model.sdf ) diff --git a/buoy_gazebo/CMakeLists.txt b/buoy_gazebo/CMakeLists.txt index 8a039ea8..b841d111 100644 --- a/buoy_gazebo/CMakeLists.txt +++ b/buoy_gazebo/CMakeLists.txt @@ -138,7 +138,7 @@ if(BUILD_TESTING) ) if(gz_add_test_GAZEBO) target_link_libraries(${TEST_NAME} - ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER} + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ) endif() if(gz_add_test_ROS) diff --git a/buoy_gazebo/launch/debugger.launch.py b/buoy_gazebo/launch/debugger.launch.py index 970af216..e022910a 100644 --- a/buoy_gazebo/launch/debugger.launch.py +++ b/buoy_gazebo/launch/debugger.launch.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Launch Ignition Gazebo with command line arguments.""" +"""Launch Gazebo with command line arguments.""" from os import environ @@ -28,16 +28,16 @@ def generate_launch_description(): environ.get('LD_LIBRARY_PATH', default='')])} return LaunchDescription([ - DeclareLaunchArgument('ign_args', default_value='', - description='Arguments to be passed to Ignition Gazebo'), - # Ignition Gazebo's major version - DeclareLaunchArgument('ign_version', default_value='6', - description="Ignition Gazebo's major version"), + DeclareLaunchArgument('gz_args', default_value='', + description='Arguments to be passed to Gazebo'), + # Gazebo's major version + DeclareLaunchArgument('gz_version', default_value='6', + description="Gazebo's major version"), ExecuteProcess( - cmd=['ruby $(which ign) gazebo -rs', - LaunchConfiguration('ign_args'), + cmd=['ruby $(which gz) sim -rs', + LaunchConfiguration('gz_args'), '--force-version', - LaunchConfiguration('ign_version'), + LaunchConfiguration('gz_version'), ], prefix=['xterm -e gdb -ex run --args'], # prefix=['xterm -e valgrind'], diff --git a/buoy_gazebo/launch/electrohydraulic_pto.launch.py b/buoy_gazebo/launch/electrohydraulic_pto.launch.py index a0318e29..4c5e77b5 100644 --- a/buoy_gazebo/launch/electrohydraulic_pto.launch.py +++ b/buoy_gazebo/launch/electrohydraulic_pto.launch.py @@ -23,13 +23,13 @@ def generate_launch_description(): - pkg_ros_ign_gazebo = get_package_share_directory('ros_ign_gazebo') + pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py'), + os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'), ), - launch_arguments={'ign_args': '-r electrohydraulicPTO.sdf'}.items(), + launch_arguments={'gz_args': '-r electrohydraulicPTO.sdf'}.items(), ) # For now, each field is published in its own topic as a double / float64 @@ -42,7 +42,7 @@ def generate_launch_description(): ros_to_gz = ros_msg_type + ']' + gz_msg_type bridge = Node( - package='ros_ign_bridge', + package='ros_gz_bridge', executable='parameter_bridge', arguments=[ ['/battcurr_' + joint_name + '@' + gz_to_ros], diff --git a/buoy_gazebo/launch/mbari_wec.launch.py b/buoy_gazebo/launch/mbari_wec.launch.py index 9c541b08..e29126dd 100644 --- a/buoy_gazebo/launch/mbari_wec.launch.py +++ b/buoy_gazebo/launch/mbari_wec.launch.py @@ -27,7 +27,7 @@ def generate_launch_description(): - pkg_ros_ign_gazebo = get_package_share_directory('ros_ign_gazebo') + pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') pkg_buoy_gazebo = get_package_share_directory('buoy_gazebo') pkg_buoy_description = get_package_share_directory('buoy_description') model_dir = 'mbari_wec_ros' @@ -59,7 +59,7 @@ def generate_launch_description(): gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py'), + os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'), ), launch_arguments={'gz_args': PathJoinSubstitution([ pkg_buoy_gazebo, @@ -72,7 +72,7 @@ def generate_launch_description(): # Bridge to forward tf and joint states to ros2 link_pose_gz_topic = '/model/' + model_name + '/pose' bridge = Node( - package='ros_ign_bridge', + package='ros_gz_bridge', executable='parameter_bridge', arguments=[ # Clock (Gazebo -> ROS2) diff --git a/buoy_gazebo/launch/mbari_wec_debugger.launch.py b/buoy_gazebo/launch/mbari_wec_debugger.launch.py index 193c177b..76eb59b2 100644 --- a/buoy_gazebo/launch/mbari_wec_debugger.launch.py +++ b/buoy_gazebo/launch/mbari_wec_debugger.launch.py @@ -34,16 +34,16 @@ def generate_launch_description(): ), ) - bridge = Node(package='ros_ign_bridge', + bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') return LaunchDescription([ DeclareLaunchArgument( - 'ign_args', + 'gz_args', default_value=[os.path.join(pkg_buoy_gazebo, 'worlds', 'mbari_wec.sdf'), ''], - description='Ignition Gazebo arguments'), + description='Gazebo arguments'), gazebo, bridge ]) diff --git a/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py b/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py index e89cb9a4..25bc1bdd 100644 --- a/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py +++ b/buoy_gazebo/launch/mbari_wec_sinusoidal.launch.py @@ -27,7 +27,7 @@ def generate_launch_description(): - pkg_ros_ign_gazebo = get_package_share_directory('ros_ign_gazebo') + pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') pkg_buoy_gazebo = get_package_share_directory('buoy_gazebo') pkg_buoy_description = get_package_share_directory('buoy_description') model_dir = 'mbari_wec_ros' @@ -59,9 +59,9 @@ def generate_launch_description(): gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py'), + os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'), ), - launch_arguments={'ign_args': PathJoinSubstitution([ + launch_arguments={'gz_args': PathJoinSubstitution([ pkg_buoy_gazebo, 'worlds', LaunchConfiguration('world_file') @@ -72,7 +72,7 @@ def generate_launch_description(): # Bridge to forward tf and joint states to ros2 link_pose_gz_topic = '/model/' + model_name + '/pose' bridge = Node( - package='ros_ign_bridge', + package='ros_gz_bridge', executable='parameter_bridge', arguments=[ # Clock (Gazebo -> ROS2) diff --git a/buoy_gazebo/package.xml b/buoy_gazebo/package.xml index a7a2bff1..6208737a 100644 --- a/buoy_gazebo/package.xml +++ b/buoy_gazebo/package.xml @@ -11,12 +11,13 @@ ament_cmake buoy_description + buoy_examples buoy_interfaces gz-sim7 rclcpp robot_state_publisher - ros_ign_bridge - ros_ign_gazebo + ros_gz_bridge + ros_gz_sim rviz2 sdformat_urdf splinter_ros diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index da85e231..37fce619 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -73,7 +73,7 @@ class ElectroHydraulicPTOPrivate bool VelMode{false}; - /// \brief Ignition communication node. + /// \brief Gazebo communication node. gz::transport::Node node; }; @@ -218,6 +218,8 @@ void ElectroHydraulicPTO::PreUpdate( auto SimTime = std::chrono::duration(_info.simTime).count(); + GZ_PROFILE("#ElectroHydraulicPTO::PreUpdate"); + // If the joints haven't been identified yet, the plugin is disabled if (this->dataPtr->PrismaticJointEntity == gz::sim::kNullEntity) { return; diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.hpp index 7408144c..139e3659 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.hpp @@ -33,7 +33,7 @@ class ElectroHydraulicPTOPrivate; /// /// ## System Parameters /// -/// xml tags in Ignition Gazebo .sdf file define behavior as follows: +/// xml tags in Gazebo .sdf file define behavior as follows: /// /// \brief /// diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.hpp b/buoy_gazebo/src/controllers/PowerController/PowerController.hpp index 7346e302..898f4693 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.hpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.hpp @@ -26,7 +26,7 @@ struct PowerControllerPrivate; /// \brief ROS2 Power Controller node for publishing PCRecord and accepting power commands /// Uses parameter to set publish rate (PCPackRate). -/// Uses ros_ign_bridge and use_sim_time to get /clock from gazebo for command timing. +/// Uses ros_gz_bridge and use_sim_time to get /clock from gazebo for command timing. /// SDF parameters: /// * ``: Namespace for ROS node, defaults to scoped name diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.hpp b/buoy_gazebo/src/controllers/SpringController/SpringController.hpp index 043c0e68..5f3d7b40 100644 --- a/buoy_gazebo/src/controllers/SpringController/SpringController.hpp +++ b/buoy_gazebo/src/controllers/SpringController/SpringController.hpp @@ -27,7 +27,7 @@ struct SpringControllerPrivate; /// \brief ROS2 Spring Controller node for publishing SCRecord and accepting spring commands /// Currently accepts valve and pump command services. /// Uses parameter to set publish rate (SCPackRate). -/// Uses ros_ign_bridge and use_sim_time to get /clock from gazebo for command timing. +/// Uses ros_gz_bridge and use_sim_time to get /clock from gazebo for command timing. /// SDF parameters: /// * ``: Namespace for ROS node, defaults to scoped name diff --git a/buoy_tests/launch/no_inputs_py.launch.py b/buoy_tests/launch/no_inputs_py.launch.py index a946e83a..85f7f0fa 100644 --- a/buoy_tests/launch/no_inputs_py.launch.py +++ b/buoy_tests/launch/no_inputs_py.launch.py @@ -38,7 +38,7 @@ def generate_test_description(): output='screen' ) - bridge = Node(package='ros_ign_bridge', + bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') diff --git a/buoy_tests/launch/no_inputs_ros_feedback.launch.py b/buoy_tests/launch/no_inputs_ros_feedback.launch.py index 7c0b3a22..cff3fadc 100644 --- a/buoy_tests/launch/no_inputs_ros_feedback.launch.py +++ b/buoy_tests/launch/no_inputs_ros_feedback.launch.py @@ -32,7 +32,7 @@ def generate_test_description(): output='screen' ) - bridge = Node(package='ros_ign_bridge', + bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') diff --git a/buoy_tests/launch/pc_commands_ros_feedback.launch.py b/buoy_tests/launch/pc_commands_ros_feedback.launch.py index 4435865f..25f5eda8 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback.launch.py @@ -42,7 +42,7 @@ def generate_test_description(): parameters=[config] ) - bridge = Node(package='ros_ign_bridge', + bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') diff --git a/buoy_tests/launch/run_server.launch.py b/buoy_tests/launch/run_server.launch.py index 2b992d72..03229aa6 100644 --- a/buoy_tests/launch/run_server.launch.py +++ b/buoy_tests/launch/run_server.launch.py @@ -26,7 +26,7 @@ def generate_launch_description(): output='screen' ) - bridge = launchNode(package='ros_ign_bridge', + bridge = launchNode(package='ros_gz_bridge', executable='parameter_bridge', arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') diff --git a/buoy_tests/launch/sc_commands_ros_feedback.launch.py b/buoy_tests/launch/sc_commands_ros_feedback.launch.py index 69e11714..c2c0fe6f 100644 --- a/buoy_tests/launch/sc_commands_ros_feedback.launch.py +++ b/buoy_tests/launch/sc_commands_ros_feedback.launch.py @@ -32,7 +32,7 @@ def generate_test_description(): output='screen' ) - bridge = Node(package='ros_ign_bridge', + bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') diff --git a/buoy_tests/testing_utils/utils.py b/buoy_tests/testing_utils/utils.py index eb238113..1836d80d 100644 --- a/buoy_tests/testing_utils/utils.py +++ b/buoy_tests/testing_utils/utils.py @@ -49,7 +49,7 @@ def default_generate_test_description(server='fixture_server'): output='screen' ) - bridge = launchNode(package='ros_ign_bridge', + bridge = launchNode(package='ros_gz_bridge', executable='parameter_bridge', arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') diff --git a/buoy_tests/tests/test_fixture.py b/buoy_tests/tests/test_fixture.py index f8edf1bc..dbfa01e3 100644 --- a/buoy_tests/tests/test_fixture.py +++ b/buoy_tests/tests/test_fixture.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -# If you compiled Ignition Gazebo from source you should modify your +# If you compiled Gazebo from source you should modify your # `PYTHONPATH`: # # export PYTHONPATH=$PYTHONPATH:/install/lib/python From be068572163c8a021640396789d491f4b6cf7ad5 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Mon, 26 Sep 2022 02:02:05 -0700 Subject: [PATCH 09/57] updates after cherry-pick Signed-off-by: Dharini Dutia --- buoy_description/CMakeLists.txt | 2 +- buoy_gazebo/launch/debugger.launch.py | 2 +- buoy_gazebo/package.xml | 2 +- buoy_tests/tests/test_fixture.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/buoy_description/CMakeLists.txt b/buoy_description/CMakeLists.txt index 578ad0fb..be7cbb13 100644 --- a/buoy_description/CMakeLists.txt +++ b/buoy_description/CMakeLists.txt @@ -7,7 +7,7 @@ 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 - COMMAND python -m em + 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 ) diff --git a/buoy_gazebo/launch/debugger.launch.py b/buoy_gazebo/launch/debugger.launch.py index e022910a..af6c0979 100644 --- a/buoy_gazebo/launch/debugger.launch.py +++ b/buoy_gazebo/launch/debugger.launch.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Launch Gazebo with command line arguments.""" +"""Launch Gazebo Sim with command line arguments.""" from os import environ diff --git a/buoy_gazebo/package.xml b/buoy_gazebo/package.xml index 6208737a..644f3499 100644 --- a/buoy_gazebo/package.xml +++ b/buoy_gazebo/package.xml @@ -11,8 +11,8 @@ ament_cmake buoy_description - buoy_examples buoy_interfaces + buoy_msgs gz-sim7 rclcpp robot_state_publisher diff --git a/buoy_tests/tests/test_fixture.py b/buoy_tests/tests/test_fixture.py index dbfa01e3..1de26f03 100644 --- a/buoy_tests/tests/test_fixture.py +++ b/buoy_tests/tests/test_fixture.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -# If you compiled Gazebo from source you should modify your +# If you compiled Gazebo Sim from source you should modify your # `PYTHONPATH`: # # export PYTHONPATH=$PYTHONPATH:/install/lib/python From 788becabdeb4a2cc58c92e3e4a6545f2da439d95 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Mon, 26 Sep 2022 02:02:28 -0700 Subject: [PATCH 10/57] fix cpplint Signed-off-by: Dharini Dutia --- .../ElectroHydraulicPTO/ElectroHydraulicSoln.hpp | 6 +++--- .../ElectroHydraulicPTO/WindingCurrentTarget.hpp | 4 ++-- buoy_gazebo/src/buoy_utils/Stopwatch.cpp | 3 +-- buoy_gazebo/test/test_stopwatch.cpp | 10 +++++----- buoy_tests/tests/fixture_server.cpp | 10 +++++----- .../tests/fixture_server_sinusoidal_piston.cpp | 10 +++++----- buoy_tests/tests/no_inputs.cpp | 4 ++-- buoy_tests/tests/no_inputs_ros_feedback.cpp | 12 ++++++------ buoy_tests/tests/pc_commands_ros_feedback.cpp | 15 ++++++++------- buoy_tests/tests/sc_commands_ros_feedback.cpp | 14 +++++++------- 10 files changed, 44 insertions(+), 44 deletions(-) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp index 91355fca..d30d0ef1 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp @@ -19,9 +19,6 @@ #include -#include -#include - #include #include #include @@ -29,6 +26,9 @@ #include #include +#include +#include + #include "ElectroHydraulicState.hpp" #include "WindingCurrentTarget.hpp" diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp index 4f4e76e4..c78bc4e1 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp @@ -15,14 +15,14 @@ #ifndef ELECTROHYDRAULICPTO__WINDINGCURRENTTARGET_HPP_ #define ELECTROHYDRAULICPTO__WINDINGCURRENTTARGET_HPP_ -#include - #include #include #include #include #include +#include + // Defines from Controller Firmware, behavior replicated here #define TORQUE_CONSTANT 0.438 // 0.62 N-m/ARMS 0.428N-m/AMPS Flux Current diff --git a/buoy_gazebo/src/buoy_utils/Stopwatch.cpp b/buoy_gazebo/src/buoy_utils/Stopwatch.cpp index c74f6827..f16a652a 100644 --- a/buoy_gazebo/src/buoy_utils/Stopwatch.cpp +++ b/buoy_gazebo/src/buoy_utils/Stopwatch.cpp @@ -14,12 +14,11 @@ #include "Stopwatch.hpp" -#include - #include #include #include +#include namespace buoy_utils { diff --git a/buoy_gazebo/test/test_stopwatch.cpp b/buoy_gazebo/test/test_stopwatch.cpp index 162c4899..68faad1d 100644 --- a/buoy_gazebo/test/test_stopwatch.cpp +++ b/buoy_gazebo/test/test_stopwatch.cpp @@ -15,6 +15,11 @@ #include +#include +#include +#include +#include + #include #include @@ -22,11 +27,6 @@ #include -#include -#include -#include -#include - // NOLINTNEXTLINE using namespace std::chrono; diff --git a/buoy_tests/tests/fixture_server.cpp b/buoy_tests/tests/fixture_server.cpp index 91a75bab..fca0538b 100644 --- a/buoy_tests/tests/fixture_server.cpp +++ b/buoy_tests/tests/fixture_server.cpp @@ -14,7 +14,10 @@ #include -#include +#include +#include +#include +#include #include #include @@ -25,10 +28,7 @@ #include -#include -#include -#include -#include +#include TEST(BuoyTests, RunServer) diff --git a/buoy_tests/tests/fixture_server_sinusoidal_piston.cpp b/buoy_tests/tests/fixture_server_sinusoidal_piston.cpp index a46c5e87..9a0636fb 100644 --- a/buoy_tests/tests/fixture_server_sinusoidal_piston.cpp +++ b/buoy_tests/tests/fixture_server_sinusoidal_piston.cpp @@ -14,7 +14,10 @@ #include -#include +#include +#include +#include +#include #include #include @@ -25,10 +28,7 @@ #include -#include -#include -#include -#include +#include TEST(BuoyTests, RunServer) diff --git a/buoy_tests/tests/no_inputs.cpp b/buoy_tests/tests/no_inputs.cpp index 9f51f385..60a2fd7b 100644 --- a/buoy_tests/tests/no_inputs.cpp +++ b/buoy_tests/tests/no_inputs.cpp @@ -14,6 +14,8 @@ #include +#include + #include #include #include @@ -21,8 +23,6 @@ #include #include -#include - ////////////////////////////////////////////////// TEST(BuoyTests, NoInputs) { diff --git a/buoy_tests/tests/no_inputs_ros_feedback.cpp b/buoy_tests/tests/no_inputs_ros_feedback.cpp index 1ebf6b2f..6a96fcf8 100644 --- a/buoy_tests/tests/no_inputs_ros_feedback.cpp +++ b/buoy_tests/tests/no_inputs_ros_feedback.cpp @@ -12,10 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include +#include +#include +#include +#include + #include #include #include @@ -23,10 +26,7 @@ #include #include -#include -#include -#include -#include +#include class NoInputsROSNode final : public buoy_api::Interface diff --git a/buoy_tests/tests/pc_commands_ros_feedback.cpp b/buoy_tests/tests/pc_commands_ros_feedback.cpp index ce7fc4cb..2e331173 100644 --- a/buoy_tests/tests/pc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/pc_commands_ros_feedback.cpp @@ -12,10 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include +#include +#include +#include +#include + #include #include #include @@ -23,13 +26,11 @@ #include #include -#include + +#include #include -#include -#include -#include -#include +#include // NOLINTNEXTLINE diff --git a/buoy_tests/tests/sc_commands_ros_feedback.cpp b/buoy_tests/tests/sc_commands_ros_feedback.cpp index 5d94de32..3726c240 100644 --- a/buoy_tests/tests/sc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/sc_commands_ros_feedback.cpp @@ -12,10 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include +#include +#include +#include +#include + #include #include #include @@ -23,12 +26,9 @@ #include #include -#include +#include -#include -#include -#include -#include +#include // NOLINTNEXTLINE From 6617da66153cc1318795da8ffb6911a6fd610472 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 26 Sep 2022 15:48:05 -0500 Subject: [PATCH 11/57] gz.math -> gz.math7 Signed-off-by: Michael Carroll --- buoy_description/models/mbari_wec_base/model.sdf.em | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/buoy_description/models/mbari_wec_base/model.sdf.em b/buoy_description/models/mbari_wec_base/model.sdf.em index ef004ef4..53ccd5db 100644 --- a/buoy_description/models/mbari_wec_base/model.sdf.em +++ b/buoy_description/models/mbari_wec_base/model.sdf.em @@ -1,8 +1,8 @@ @{ -from gz.math import Cylinderd -from gz.math import MassMatrix3d -from gz.math import Material +from gz.math7 import Cylinderd +from gz.math7 import MassMatrix3d +from gz.math7 import Material import math ############## From 9232c0f5a5ca2baa0447fafe550870b9800e06f8 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 28 Oct 2022 08:15:41 -0500 Subject: [PATCH 12/57] Temporarily override versions for CI Signed-off-by: Michael Carroll --- .github/workflows/build-and-test.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 94c82061..d34c205a 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -28,7 +28,7 @@ apt-get install -y git \ cd $COLCON_WS_SRC cp -r $GITHUB_WORKSPACE $COLCON_WS_SRC -wget https://raw.githubusercontent.com/osrf/buoy_entrypoint/main/buoy_all.yaml +wget https://raw.githubusercontent.com/osrf/buoy_entrypoint/chapulina/humble_garden/buoy_all.yaml vcs import --skip-existing < buoy_all.yaml rm -rf buoy_examples From 672b0d679460a76fa269a2537919a628c8e2b502 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 31 Oct 2022 15:47:35 -0500 Subject: [PATCH 13/57] Fix merge Signed-off-by: Michael Carroll --- .../ElectroHydraulicPTO/ElectroHydraulicPTO.cpp | 4 ++-- .../PolytropicPneumaticSpring.cpp | 4 ++-- .../src/controllers/PowerController/CMakeLists.txt | 2 +- .../PowerController/PowerController.cpp | 14 +++++++------- .../controllers/SpringController/CMakeLists.txt | 2 +- .../SpringController/SpringController.cpp | 4 ++-- 6 files changed, 15 insertions(+), 15 deletions(-) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index f9fa44a8..03c1b288 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -192,11 +192,11 @@ void ElectroHydraulicPTO::PreUpdate( } // Create joint position component for piston if one doesn't exist - auto prismaticJointPosComp = _ecm.Component( + auto prismaticJointPosComp = _ecm.Component( this->dataPtr->PrismaticJointEntity); if (prismaticJointPosComp == nullptr) { _ecm.CreateComponent( - this->dataPtr->PrismaticJointEntity, ignition::gazebo::components::JointPosition()); + this->dataPtr->PrismaticJointEntity, gz::sim::components::JointPosition()); } // We just created the joint velocity component, give one iteration for the // physics system to update its size diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index 054ea5a2..d136698c 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -249,7 +249,7 @@ void PolytropicPneumaticSpring::computeLawOfCoolingForce(const double & x, const // Newton's Law of Cooling (non-dimensionalized): // Tdot = r*(T_env - T(t)) -> T[n] = dt*r*(Tenv - T[n-1]) + T[n-1] (using forward difference) - const double dt_sec = dt_nano * IGN_NANO_TO_SEC; + const double dt_sec = dt_nano * GZ_NANO_TO_SEC; const double dT = dt_sec * this->dataPtr->config_->r * (this->dataPtr->config_->Tenv - this->dataPtr->T); this->dataPtr->T += dT; @@ -391,7 +391,7 @@ void PolytropicPneumaticSpring::Configure( // Ideal Gas Law: T = P*V/(m*R) this->dataPtr->T = this->dataPtr->P * this->dataPtr->V / config.c; - config.model = ignition::gazebo::Model(_entity); + config.model = gz::sim::Model(_entity); if (!config.model.Valid(_ecm)) { gzerr << "PolytropicPneumaticSpring plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; diff --git a/buoy_gazebo/src/controllers/PowerController/CMakeLists.txt b/buoy_gazebo/src/controllers/PowerController/CMakeLists.txt index 56572369..7bb77269 100644 --- a/buoy_gazebo/src/controllers/PowerController/CMakeLists.txt +++ b/buoy_gazebo/src/controllers/PowerController/CMakeLists.txt @@ -5,5 +5,5 @@ gz_add_plugin(PowerController ../.. ROS EXTRA_ROS_PKGS - ros_ign_gazebo + ros_gz_sim ) diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 672a2631..1cf66877 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -31,7 +31,7 @@ #include #include -#include +#include #include #include @@ -81,7 +81,7 @@ struct PowerControllerServices std::function, std::shared_ptr)> torque_command_handler_; - ros_ign_gazebo::Stopwatch torque_command_watch_; + ros_gz_sim::Stopwatch torque_command_watch_; rclcpp::Duration torque_command_duration_{0, 0U}; static const rclcpp::Duration TORQUE_COMMAND_TIMEOUT; static const rcl_interfaces::msg::FloatingPointRange valid_wind_curr_range_; @@ -94,7 +94,7 @@ struct PowerControllerServices std::function, std::shared_ptr)> scale_command_handler_; - ros_ign_gazebo::Stopwatch scale_command_watch_; + ros_gz_sim::Stopwatch scale_command_watch_; rclcpp::Duration scale_command_duration_{0, 0U}; static const rclcpp::Duration SCALE_COMMAND_TIMEOUT; static const rcl_interfaces::msg::FloatingPointRange valid_scale_range_; @@ -108,7 +108,7 @@ struct PowerControllerServices std::function, std::shared_ptr)> retract_command_handler_; - ros_ign_gazebo::Stopwatch retract_command_watch_; + ros_gz_sim::Stopwatch retract_command_watch_; rclcpp::Duration retract_command_duration_{0, 0U}; static const rclcpp::Duration RETRACT_COMMAND_TIMEOUT; static const rcl_interfaces::msg::FloatingPointRange valid_retract_range_; @@ -122,7 +122,7 @@ struct PowerControllerServices std::function, std::shared_ptr)> bias_curr_command_handler_; - ros_ign_gazebo::Stopwatch bias_curr_command_watch_; + ros_gz_sim::Stopwatch bias_curr_command_watch_; rclcpp::Duration bias_curr_command_duration_{0, 0U}; static const rclcpp::Duration BIAS_CURR_COMMAND_TIMEOUT; static const rcl_interfaces::msg::FloatingPointRange valid_bias_curr_range_; @@ -423,7 +423,7 @@ struct PowerControllerPrivate void manageCommandTimer( const std::string & command_name, buoy_utils::CommandTriState<> & command, - ros_ign_gazebo::Stopwatch & watch, + ros_gz_sim::Stopwatch & watch, const rclcpp::Duration & duration) { // override @@ -488,7 +488,7 @@ struct PowerControllerPrivate std::atomic & services_command, std::atomic & new_command, const double & command_value, - ros_ign_gazebo::Stopwatch & watch, + ros_gz_sim::Stopwatch & watch, rclcpp::Duration & duration, const rclcpp::Duration & timeout) { diff --git a/buoy_gazebo/src/controllers/SpringController/CMakeLists.txt b/buoy_gazebo/src/controllers/SpringController/CMakeLists.txt index 46b57ed1..fa138e31 100644 --- a/buoy_gazebo/src/controllers/SpringController/CMakeLists.txt +++ b/buoy_gazebo/src/controllers/SpringController/CMakeLists.txt @@ -5,5 +5,5 @@ gz_add_plugin(SpringController ../.. ROS EXTRA_ROS_PKGS - ros_ign_gazebo + ros_gz_sim ) diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp index bd510436..f1446d09 100644 --- a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp +++ b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include @@ -68,7 +68,7 @@ struct SpringControllerServices std::function, std::shared_ptr)> pump_command_handler_; - ros_ign_gazebo::Stopwatch command_watch_; + ros_gz_sim::Stopwatch command_watch_; rclcpp::Duration command_duration_{0, 0U}; std::atomic valve_command_{false}, pump_command_{false}; From f852f2c44d3e8ccdacf3f15ef6c6ecaca19d23c9 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 2 Nov 2022 10:20:34 -0500 Subject: [PATCH 14/57] Remaining ignition references removed Signed-off-by: Michael Carroll --- .../experiment_comparison_select.launch.py | 2 +- buoy_tests/package.xml | 2 +- buoy_tests/tests/experiment_comparison.cpp | 66 +++++----- buoy_tests/tests/pc_commands_ros_feedback.cpp | 2 +- buoy_tests/worlds/TestMachine.sdf | 116 +++++++++++++----- 5 files changed, 121 insertions(+), 67 deletions(-) diff --git a/buoy_tests/launch/experiment_comparison_select.launch.py b/buoy_tests/launch/experiment_comparison_select.launch.py index cd545b09..9aec3d41 100644 --- a/buoy_tests/launch/experiment_comparison_select.launch.py +++ b/buoy_tests/launch/experiment_comparison_select.launch.py @@ -74,7 +74,7 @@ def generate_launch_description(): bridge = Node(package='ros_ign_bridge', executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') return launch.LaunchDescription([ diff --git a/buoy_tests/package.xml b/buoy_tests/package.xml index 14cd238d..11230771 100644 --- a/buoy_tests/package.xml +++ b/buoy_tests/package.xml @@ -24,7 +24,7 @@ ament_index_cpp libgnuplot-iostream-dev - python3-ignition-gazebo6 + python3-gz-sim7 ament_cmake_gtest ament_cmake_pytest launch_testing_ament_cmake diff --git a/buoy_tests/tests/experiment_comparison.cpp b/buoy_tests/tests/experiment_comparison.cpp index 72f88a58..a9d97e24 100644 --- a/buoy_tests/tests/experiment_comparison.cpp +++ b/buoy_tests/tests/experiment_comparison.cpp @@ -15,19 +15,19 @@ #include #include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include #include #include @@ -182,7 +182,7 @@ class BuoyExperimentComparison : public ::testing::Test static constexpr double stroke{2.03}; static constexpr double lower_area{0.0115}, lower_dead_volume{0.0463}; static constexpr double upper_area{0.0127}, upper_dead_volume{0.0226}; - static ignition::gazebo::Entity jointEntity; + static gz::sim::Entity jointEntity; static constexpr double timestep{0.01}; double epsilon{1e-2}; std::function comparator; @@ -285,38 +285,38 @@ class BuoyExperimentComparison : public ::testing::Test std::make_shared(InputData.seconds, InputData.PistonVel); // Skip debug messages to run faster - ignition::common::Console::SetVerbosity(3); + gz::common::Console::SetVerbosity(3); // Setup fixture - ignition::gazebo::ServerConfig config; + gz::sim::ServerConfig config; config.SetSdfFile("TestMachine.sdf"); config.SetUpdateRate(0.0); - ignition::gazebo::TestFixture fixture(config); + gz::sim::TestFixture fixture(config); fixture. OnConfigure( [&]( - const ignition::gazebo::Entity & _worldEntity, + const gz::sim::Entity & _worldEntity, const std::shared_ptr &, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager &) + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager &) { - auto world = ignition::gazebo::World(_worldEntity); - ignition::gazebo::Model pto(world.ModelByName(_ecm, "PTO")); + auto world = gz::sim::World(_worldEntity); + gz::sim::Model pto(world.ModelByName(_ecm, "PTO")); jointEntity = pto.JointByName(_ecm, "HydraulicRam"); - EXPECT_NE(ignition::gazebo::kNullEntity, jointEntity); + EXPECT_NE(gz::sim::kNullEntity, jointEntity); std::cerr << "Initializing piston position to [" << stroke - INCHES_TO_METERS * InputData.PistonPos.at(0) << "] meters (or [" << InputData.PistonPos.at(0) << "] inches)" << std::endl; - _ecm.SetComponentData( + _ecm.SetComponentData( jointEntity, {stroke - INCHES_TO_METERS * InputData.PistonPos.at(0)}); }). OnPreUpdate( [&]( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) { auto SimTime = std::chrono::duration(_info.simTime).count(); double piston_vel = @@ -328,27 +328,27 @@ class BuoyExperimentComparison : public ::testing::Test // Create new component for this entitiy in ECM (if it doesn't already // exist) auto joint_vel = - _ecm.Component(jointEntity); + _ecm.Component(jointEntity); if (joint_vel == nullptr) { _ecm.CreateComponent( jointEntity, - ignition::gazebo::components::JointVelocityCmd( + gz::sim::components::JointVelocityCmd( {piston_vel})); // Create this iteration } else { - *joint_vel = ignition::gazebo::components::JointVelocityCmd({piston_vel}); + *joint_vel = gz::sim::components::JointVelocityCmd({piston_vel}); } }). OnPostUpdate( [&]( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) + const gz::sim::UpdateInfo & _info, + const gz::sim::EntityComponentManager & _ecm) { bool got_vel{false}, got_spring_state{false}, got_pto_state{false}; auto SimTime = std::chrono::duration(_info.simTime).count(); auto prismaticJointVelComp = - _ecm.Component( + _ecm.Component( jointEntity); if (prismaticJointVelComp != nullptr) { if (!prismaticJointVelComp->Data().empty()) { @@ -479,7 +479,7 @@ bool BuoyExperimentComparison::manual_comparison{false}; std::shared_ptr BuoyExperimentComparison::PrescribedVel{nullptr}; int BuoyExperimentComparison::argc_; char ** BuoyExperimentComparison::argv_; -ignition::gazebo::Entity BuoyExperimentComparison::jointEntity{ignition::gazebo::kNullEntity}; +gz::sim::Entity BuoyExperimentComparison::jointEntity{gz::sim::kNullEntity}; TEST_F(BuoyExperimentComparison, Spring) diff --git a/buoy_tests/tests/pc_commands_ros_feedback.cpp b/buoy_tests/tests/pc_commands_ros_feedback.cpp index 13863fa7..cc6767db 100644 --- a/buoy_tests/tests/pc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/pc_commands_ros_feedback.cpp @@ -163,7 +163,7 @@ class BuoyPCTests : public ::testing::Test config.SetSdfFile("mbari_wec.sdf"); config.SetUpdateRate(0.0); - fixture = std::make_unique(config); + fixture = std::make_unique(config); node = std::make_unique("pb_torque_controller"); // same name as example to grab // params diff --git a/buoy_tests/worlds/TestMachine.sdf b/buoy_tests/worlds/TestMachine.sdf index 5b5b7fca..53bb5f6f 100644 --- a/buoy_tests/worlds/TestMachine.sdf +++ b/buoy_tests/worlds/TestMachine.sdf @@ -5,71 +5,125 @@ 10000.0 - - "libignition-physics-dartsim-plugin.so" - + + + - - + + - - + + 3D View - false - docked - - ogre + false + docked + + + ogre2 scene 1.0 1.0 1.0 0.8 0.8 0.8 -6 0 6 0 0.5 0 - - - - World control + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating false + + + + false - 72 - 121 - 1 + 5 + 5 floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating - + + true true true - /world/world_demo/control - /world/world_demo/stats + true + + - + World stats - false - false - 110 - 290 - 1 - floating + false + false + 110 + 290 + 1 + + floating - + + true true true true - /world/world_demo/stats + + + + + docked + + + - + + + docked + + + true 0 0 10 0 0 0 From 5a9ff92823814b9447b5bf98707ea9523ca1d4df Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 2 Nov 2022 12:55:48 -0500 Subject: [PATCH 15/57] Lint Signed-off-by: Michael Carroll --- .../experiment_comparison_select.launch.py | 2 +- buoy_tests/tests/experiment_comparison.cpp | 19 +++++++++---------- 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/buoy_tests/launch/experiment_comparison_select.launch.py b/buoy_tests/launch/experiment_comparison_select.launch.py index 9aec3d41..18d365a8 100644 --- a/buoy_tests/launch/experiment_comparison_select.launch.py +++ b/buoy_tests/launch/experiment_comparison_select.launch.py @@ -72,7 +72,7 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration('manual')) ) - bridge = Node(package='ros_ign_bridge', + bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') diff --git a/buoy_tests/tests/experiment_comparison.cpp b/buoy_tests/tests/experiment_comparison.cpp index a9d97e24..1df2dcab 100644 --- a/buoy_tests/tests/experiment_comparison.cpp +++ b/buoy_tests/tests/experiment_comparison.cpp @@ -15,6 +15,15 @@ #include #include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include #include @@ -36,16 +45,6 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include - - const double INCHES_TO_METERS{0.0254}; From 9d97f36fc20b50a4e7c110253faf98d6ea3cee00 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 2 Nov 2022 16:01:11 -0500 Subject: [PATCH 16/57] Fix tests Signed-off-by: Michael Carroll --- buoy_tests/hooks/buoy_tests.dsv.in | 2 +- buoy_tests/launch/sc_pump_ros_feedback_py.launch.py | 4 ++-- buoy_tests/tests/test_fixture.py | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/buoy_tests/hooks/buoy_tests.dsv.in b/buoy_tests/hooks/buoy_tests.dsv.in index 9818f321..b14eb216 100644 --- a/buoy_tests/hooks/buoy_tests.dsv.in +++ b/buoy_tests/hooks/buoy_tests.dsv.in @@ -1 +1 @@ -prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share/@PROJECT_NAME@/worlds +prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share/@PROJECT_NAME@/worlds diff --git a/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py b/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py index 1e8b1a9e..584bf451 100644 --- a/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py +++ b/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py @@ -110,10 +110,10 @@ def test_sc_pump_ros(self, gazebo_test_fixture, proc_info): if n % 2 == 1: self.assertFalse(self.node.sc_status_ & SCRecord.PUMP_TOGGLE, - 'SC Pump Toggle should be OFF') + f'SC Pump Toggle should be OFF: (0x{self.node.sc_status_:x})') else: self.assertTrue(self.node.sc_status_ & SCRecord.PUMP_TOGGLE, - 'SC Pump Toggle should be ON') + f'SC Pump Toggle should be ON: (0x{self.node.sc_status_:x})') # Check that valve command fails (controller returns BUSY) self.node.send_valve_command(2) diff --git a/buoy_tests/tests/test_fixture.py b/buoy_tests/tests/test_fixture.py index 1de26f03..6e8b0241 100644 --- a/buoy_tests/tests/test_fixture.py +++ b/buoy_tests/tests/test_fixture.py @@ -26,7 +26,7 @@ import unittest from gz.common import set_verbosity -from gz.sim import TestFixture, World, world_entity +from gz.sim7 import TestFixture, World, world_entity import pytest From 3f1c960c1746a6357c7c7fdb81b8ab5def5665f1 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 3 Nov 2022 08:23:53 -0500 Subject: [PATCH 17/57] Lint Signed-off-by: Michael Carroll --- buoy_tests/launch/sc_pump_ros_feedback_py.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py b/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py index 584bf451..4bdd0e14 100644 --- a/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py +++ b/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py @@ -110,10 +110,10 @@ def test_sc_pump_ros(self, gazebo_test_fixture, proc_info): if n % 2 == 1: self.assertFalse(self.node.sc_status_ & SCRecord.PUMP_TOGGLE, - f'SC Pump Toggle should be OFF: (0x{self.node.sc_status_:x})') + f'SC Pump Toggle should be OFF: (0x{self.node.sc_status_:x})') else: self.assertTrue(self.node.sc_status_ & SCRecord.PUMP_TOGGLE, - f'SC Pump Toggle should be ON: (0x{self.node.sc_status_:x})') + f'SC Pump Toggle should be ON: (0x{self.node.sc_status_:x})') # Check that valve command fails (controller returns BUSY) self.node.send_valve_command(2) From f0f79365c4e70d76321e7a4161f0758a8410c33b Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 7 Nov 2022 15:01:07 -0600 Subject: [PATCH 18/57] Use atomic for status Signed-off-by: Michael Carroll --- buoy_tests/tests/sc_commands_ros_feedback.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/buoy_tests/tests/sc_commands_ros_feedback.cpp b/buoy_tests/tests/sc_commands_ros_feedback.cpp index bc06a613..1359c302 100644 --- a/buoy_tests/tests/sc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/sc_commands_ros_feedback.cpp @@ -40,9 +40,8 @@ class SCROSNode final : public buoy_api::Interface { public: rclcpp::Clock::SharedPtr clock_{nullptr}; - float range_finder_{0.0F}; - uint16_t status_{0U}; + std::atomic status_{0U}; ValveServiceResponseFuture valve_response_future_; PumpServiceResponseFuture pump_response_future_; From 06f755b810907980f4a8473de021d581729c8143 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 29 Nov 2022 09:09:52 -0600 Subject: [PATCH 19/57] Explicitly link boost filesystem and system Signed-off-by: Michael Carroll --- buoy_tests/CMakeLists.txt | 6 +++--- buoy_tests/tests/eh_solver.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/buoy_tests/CMakeLists.txt b/buoy_tests/CMakeLists.txt index aaf5611e..f9924a3a 100644 --- a/buoy_tests/CMakeLists.txt +++ b/buoy_tests/CMakeLists.txt @@ -137,20 +137,20 @@ if(BUILD_TESTING) # Add gtests buoy_add_gtest(experiment_comparison ROS LAUNCH_TEST BOOST_COMPONENTS - iostreams + iostreams system filesystem EXTRA_ROS_PKGS buoy_gazebo simple_interp) buoy_add_gtest(eh_solver ROS LAUNCH_TEST BOOST_COMPONENTS - iostreams + iostreams system filesystem EXTRA_ROS_PKGS buoy_gazebo) buoy_add_gtest(eh_windtarget ROS LAUNCH_TEST BOOST_COMPONENTS - iostreams + iostreams system filesystem EXTRA_ROS_PKGS buoy_gazebo) diff --git a/buoy_tests/tests/eh_solver.cpp b/buoy_tests/tests/eh_solver.cpp index eeebcbf5..d33f93a6 100644 --- a/buoy_tests/tests/eh_solver.cpp +++ b/buoy_tests/tests/eh_solver.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include #include From c14571b69839b06a9d638c1850121d33a06e9e95 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 29 Nov 2022 09:39:12 -0600 Subject: [PATCH 20/57] Lint Signed-off-by: Michael Carroll --- .../ElectroHydraulicPTO/ElectroHydraulicPTO.cpp | 3 ++- .../ElectroHydraulicSoln.hpp | 4 ++-- .../WindingCurrentTarget.hpp | 6 +++--- .../PowerController/PowerController.cpp | 7 ------- .../SpringController/SpringController.cpp | 17 ++++++++--------- 5 files changed, 15 insertions(+), 22 deletions(-) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 643af5f8..a9bf6e9b 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include #include @@ -43,7 +45,6 @@ #include "ElectroHydraulicState.hpp" #include "ElectroHydraulicLoss.hpp" -#include namespace buoy_gazebo { diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp index 636800d6..dde5ef4d 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp @@ -15,6 +15,8 @@ #ifndef ELECTROHYDRAULICPTO__ELECTROHYDRAULICSOLN_HPP_ #define ELECTROHYDRAULICPTO__ELECTROHYDRAULICSOLN_HPP_ +#include + #include #include #include @@ -23,8 +25,6 @@ #include #include -#include - // Interpolation for efficiency maps #include #include diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp index 9b2f3d7b..c8d9d960 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp @@ -15,9 +15,6 @@ #ifndef ELECTROHYDRAULICPTO__WINDINGCURRENTTARGET_HPP_ #define ELECTROHYDRAULICPTO__WINDINGCURRENTTARGET_HPP_ -#include -#include - #include #include #include @@ -26,6 +23,9 @@ #include #include +#include +#include + // Defines from Controller Firmware, behavior replicated here #define TORQUE_CONSTANT 0.438 // 0.62 N-m/ARMS 0.428N-m/AMPS Flux Current #define CURRENT_CMD_RATELIMIT 200 // A/second. Set to zero to disable feature diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 1cf66877..27ce0060 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -40,13 +40,6 @@ #include #include -#include -#include -#include -#include -#include -#include - #include "ElectroHydraulicPTO/ElectroHydraulicState.hpp" #include "PowerController.hpp" diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp index f1446d09..eefbff85 100644 --- a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp +++ b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp @@ -12,14 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "SpringController.hpp" +#include + +#include +#include +#include +#include +#include +#include #include #include #include #include #include -#include #include #include @@ -31,13 +37,6 @@ #include #include -#include -#include -#include -#include -#include -#include - #include "PolytropicPneumaticSpring/SpringState.hpp" #include "SpringController.hpp" From b38a8c2c7e0168ab442c84bca12eeb98a437e90a Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 29 Nov 2022 16:05:53 -0600 Subject: [PATCH 21/57] Lint Signed-off-by: Michael Carroll --- .../launch/pc_commands_ros_feedback_py.launch.py | 4 ++-- buoy_tests/tests/eh_solver.cpp | 10 +++++----- buoy_tests/tests/eh_windtarget.cpp | 6 +++--- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py index b511aa39..58fe210e 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py @@ -62,8 +62,8 @@ class BuoyPCPyTest(BuoyPyTests): # Max amount to modify RPM in determining WindingCurrentLimit near ends of stroke MAX_RPM_ADJUSTMENT = 5000.0 - def winding_current_limiter(self, I): - LimitedI = I + def winding_current_limiter(self, current): + LimitedI = current AdjustedN = self.node.rpm_ RamPosition = (self.SC_RANGE_MAX - (self.node.range_finder_ / 0.0254)) if self.node.rpm_ >= 0.0: # Retracting diff --git a/buoy_tests/tests/eh_solver.cpp b/buoy_tests/tests/eh_solver.cpp index d33f93a6..c72c6dd1 100644 --- a/buoy_tests/tests/eh_solver.cpp +++ b/buoy_tests/tests/eh_solver.cpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. - -#include -#include -#include -#include #include #include +#include + #include #include #include #include +#include +#include +#include class EHSolver : public ::testing::Test { diff --git a/buoy_tests/tests/eh_windtarget.cpp b/buoy_tests/tests/eh_windtarget.cpp index 1630bd23..100c2aa8 100644 --- a/buoy_tests/tests/eh_windtarget.cpp +++ b/buoy_tests/tests/eh_windtarget.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. - -#include #include #include -#include #include #include #include #include +#include +#include + class EHWindTarget : public ::testing::Test { protected: From 0f45a5e5c3d81b1c46db9183db8e7bcb962e85b8 Mon Sep 17 00:00:00 2001 From: Andrew Hamilton Date: Tue, 27 Dec 2022 19:12:57 -0800 Subject: [PATCH 22/57] Added garden'ized version of PTOFriction --- buoy_gazebo/src/PTOFriction/CMakeLists.txt | 8 + buoy_gazebo/src/PTOFriction/PTOFriction.cpp | 160 ++++++++++++++++++++ buoy_gazebo/src/PTOFriction/PTOFriction.hpp | 57 +++++++ 3 files changed, 225 insertions(+) create mode 100644 buoy_gazebo/src/PTOFriction/CMakeLists.txt create mode 100644 buoy_gazebo/src/PTOFriction/PTOFriction.cpp create mode 100644 buoy_gazebo/src/PTOFriction/PTOFriction.hpp diff --git a/buoy_gazebo/src/PTOFriction/CMakeLists.txt b/buoy_gazebo/src/PTOFriction/CMakeLists.txt new file mode 100644 index 00000000..356dc639 --- /dev/null +++ b/buoy_gazebo/src/PTOFriction/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_plugin(PTOFriction + SOURCES + PTOFriction.cpp + INCLUDE_DIRS + .. + EXTRA_ROS_PKGS + simple_interp +) \ No newline at end of file diff --git a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp new file mode 100644 index 00000000..1f3bea3e --- /dev/null +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp @@ -0,0 +1,160 @@ +// 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. + +#include "PTOFriction.hpp" +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + + +namespace buoy_gazebo +{ +class PTOFrictionPrivate +{ +public: + /// \brief Piston joint entity + gz::sim::Entity PrismaticJointEntity{gz::sim::kNullEntity}; + + /// \brief Model interface + gz::sim::Model model{gz::sim::kNullEntity}; + + /// \brief Piston velocity + const std::vector pistonSpeed; // m/s + + /// \brief Piston mean friction force + const std::vector meanFriction; // N + + /// \brief Construct and approximation of friction model using linear spline + simple_interp::Interp1d pto_friction_model; + + PTOFrictionPrivate() + : pistonSpeed{-5.0, -0.4, -0.1, -0.05, -0.01, 0.0, + 0.01, 0.05, 0.1, 0.4, 5.0}, + meanFriction{12750.0, 1200.0, 700.0, 400.0, 160.0, 0.0, + -550.0, -750.0, -1000.0, -2900.0, -32033.0}, + pto_friction_model(pistonSpeed, meanFriction) + { + } +}; + +////////////////////////////////////////////////// +PTOFriction::PTOFriction() +: dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void PTOFriction::Configure( + const gz::sim::Entity & _entity, + const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & /*_eventMgr*/) +{ + this->dataPtr->model = gz::sim::Model(_entity); + if (!this->dataPtr->model.Valid(_ecm)) { + ignerr << "PTOFriction plugin should be attached to a model entity. " << + "Failed to initialize." << std::endl; + return; + } + + + // Get params from SDF for Prismatic Joint. + auto PrismaticJointName = _sdf->Get("PrismaticJointName"); + if (PrismaticJointName.empty()) { + ignerr << "PTOFriction found an empty PrismaticJointName parameter. " << + "Failed to initialize."; + return; + } + + + this->dataPtr->PrismaticJointEntity = this->dataPtr->model.JointByName( + _ecm, + PrismaticJointName); + if (this->dataPtr->PrismaticJointEntity == gz::sim::kNullEntity) { + ignerr << "Joint with name [" << PrismaticJointName << "] not found. " << + "The PTOFriction may not influence this joint.\n"; + return; + } +} + +////////////////////////////////////////////////// +void PTOFriction::PreUpdate( + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) +{ + GZ_PROFILE("PTOFriction::PreUpdate"); + // Nothing left to do if paused. + if (_info.paused) { + return; + } + + auto SimTime = std::chrono::duration(_info.simTime).count(); + + // If the joints haven't been identified yet, the plugin is disabled + if (this->dataPtr->PrismaticJointEntity == gz::sim::kNullEntity) { + return; + } + + // Create joint velocity component for piston if one doesn't exist + auto prismaticJointVelComp = _ecm.Component( + this->dataPtr->PrismaticJointEntity); + if (prismaticJointVelComp == nullptr) { + _ecm.CreateComponent( + this->dataPtr->PrismaticJointEntity, gz::sim::components::JointVelocity()); + } + // We just created the joint velocity component, give one iteration for the + // physics system to update its size + if (prismaticJointVelComp == nullptr || prismaticJointVelComp->Data().empty()) { + return; + } + + // Interpolate the new friction force based on current joint velocity + // Velocity and Force sign flipped to account for the direction difference between + // sim and physical buoy + double friction_force = + -this->dataPtr->pto_friction_model.eval( + -prismaticJointVelComp->Data().at(0)); + + // Create new component for applying force if it doesn't already exist + auto forceComp = _ecm.Component( + this->dataPtr->PrismaticJointEntity); + if (forceComp == nullptr) { + _ecm.CreateComponent( + this->dataPtr->PrismaticJointEntity, + gz::sim::components::JointForceCmd({friction_force})); // Create this iteration + } else { + forceComp->Data()[0] += friction_force; // Add friction to existing forces + } +} + +} // namespace buoy_gazebo + +GZ_ADD_PLUGIN( + buoy_gazebo::PTOFriction, + gz::sim::System, + buoy_gazebo::PTOFriction::ISystemConfigure, + buoy_gazebo::PTOFriction::ISystemPreUpdate); diff --git a/buoy_gazebo/src/PTOFriction/PTOFriction.hpp b/buoy_gazebo/src/PTOFriction/PTOFriction.hpp new file mode 100644 index 00000000..e185bfdb --- /dev/null +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.hpp @@ -0,0 +1,57 @@ +// 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. + +#ifndef PTOFRICTION__PTOFRICTION_HPP_ +#define PTOFRICTION__PTOFRICTION_HPP_ + +#include + +#include +#include + + +namespace buoy_gazebo +{ +// Forward declaration +class PTOFrictionPrivate; +class PTOFriction : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate +{ +public: + /// \brief Constructor + PTOFriction(); + + /// \brief Destructor + ~PTOFriction() override = default; + + // Documentation inherited + void Configure( + const gz::sim::Entity & _entity, + const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, + gz::sim::EventManager & _eventMgr) override; + + // Documentation inherited + void PreUpdate( + const gz::sim::UpdateInfo & _info, + gz::sim::EntityComponentManager & _ecm) override; + +private: + /// \brief Private data pointer + std::unique_ptr dataPtr; +}; +} // namespace buoy_gazebo + +#endif // PTOFRICTION__PTOFRICTION_HPP_ From a597df4a3627937eef5086ff62e53efb27c8e21e Mon Sep 17 00:00:00 2001 From: Andrew Hamilton Date: Tue, 27 Dec 2022 19:21:57 -0800 Subject: [PATCH 23/57] linters.. --- buoy_gazebo/src/PTOFriction/PTOFriction.cpp | 15 +++++++-------- buoy_gazebo/src/PTOFriction/PTOFriction.hpp | 3 +-- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp index 1f3bea3e..2dd52e87 100644 --- a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp @@ -12,7 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "PTOFriction.hpp" +#include +#include +#include +#include +#include + #include #include #include @@ -21,15 +26,9 @@ #include #include -#include - #include -#include -#include -#include -#include - +#include "PTOFriction.hpp" namespace buoy_gazebo { diff --git a/buoy_gazebo/src/PTOFriction/PTOFriction.hpp b/buoy_gazebo/src/PTOFriction/PTOFriction.hpp index e185bfdb..b7a1720c 100644 --- a/buoy_gazebo/src/PTOFriction/PTOFriction.hpp +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.hpp @@ -15,11 +15,10 @@ #ifndef PTOFRICTION__PTOFRICTION_HPP_ #define PTOFRICTION__PTOFRICTION_HPP_ -#include - #include #include +#include namespace buoy_gazebo { From 4706ab00cf7cec61236318a2e4508b41a2f55c31 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Mon, 9 Jan 2023 12:57:34 -0800 Subject: [PATCH 24/57] fix sim clock to ros issues in tests; clean up prints Signed-off-by: Michael Anderson --- .../controllers/PowerController/PowerController.cpp | 12 +++++++----- .../ServicesNotImplemented/NoOpController.cpp | 2 ++ .../SpringController/SpringController.cpp | 3 ++- buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp | 2 ++ buoy_tests/tests/no_inputs_ros_feedback.cpp | 2 ++ buoy_tests/tests/pc_commands_ros_feedback.cpp | 2 ++ buoy_tests/tests/sc_commands_ros_feedback.cpp | 2 ++ 7 files changed, 19 insertions(+), 6 deletions(-) diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 27ce0060..96592948 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -243,8 +243,10 @@ struct PowerControllerPrivate auto spin = [this]() { + rclcpp::Rate rate(50.0); while (rclcpp::ok() && !stop_) { ros_->executor_->spin_once(); + rate.sleep(); } }; thread_executor_spin_ = std::thread(spin); @@ -290,7 +292,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]"); @@ -386,7 +388,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]"); @@ -422,7 +424,7 @@ struct PowerControllerPrivate // override if (command) { if (!watch.Running()) { - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( ros_->node_->get_logger(), "Override " << command_name << " (" << duration.seconds() << "s)"); @@ -435,7 +437,7 @@ struct PowerControllerPrivate watch.Stop(); command = false; - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( ros_->node_->get_logger(), "Stopped overriding " << command_name << " after (" << watch.ElapsedRunTime().seconds() << "s)"); @@ -500,7 +502,7 @@ struct PowerControllerPrivate command = command_value; duration = timeout + watch.ElapsedRunTime(); - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( ros_->node_->get_logger(), "Continue Override " << command_name << " (" << duration.seconds() << "s)"); diff --git a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp index 81033d61..c24596a5 100644 --- a/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp +++ b/buoy_gazebo/src/controllers/ServicesNotImplemented/NoOpController.cpp @@ -193,8 +193,10 @@ struct NoOpControllerPrivate auto spin = [this]() { + rclcpp::Rate rate(50.0); while (rclcpp::ok() && !stop_) { ros_->executor_->spin_once(); + rate.sleep(); } }; thread_executor_spin_ = std::thread(spin); diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp index eefbff85..43e8da91 100644 --- a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp +++ b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp @@ -160,8 +160,10 @@ struct SpringControllerPrivate auto spin = [this]() { + rclcpp::Rate rate(50.0); while (rclcpp::ok() && !stop_) { ros_->executor_->spin_once(); + rate.sleep(); } }; thread_executor_spin_ = std::thread(spin); @@ -641,7 +643,6 @@ void SpringController::PostUpdate( // this->dataPtr->sc_record_.salinity = spring_state_comp->Data().salinity_; // this->dataPtr->sc_record_.temperature = spring_state_comp->Data().temperature_; - // TODO(andermi) set the bits for this this->dataPtr->ros_->sc_record_.status = spring_state_comp->Data().status; this->dataPtr->spring_data_valid_ = true; diff --git a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp index 2c4b3e3d..3f4757b1 100644 --- a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp +++ b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp @@ -142,8 +142,10 @@ void XBowAHRS::Configure( auto spin = [this]() { + rclcpp::Rate rate(50.0); while (rclcpp::ok() && !this->dataPtr->stop_) { this->dataPtr->executor_->spin_once(); + rate.sleep(); } }; this->dataPtr->thread_executor_spin_ = std::thread(spin); diff --git a/buoy_tests/tests/no_inputs_ros_feedback.cpp b/buoy_tests/tests/no_inputs_ros_feedback.cpp index 6a96fcf8..b4e7df58 100644 --- a/buoy_tests/tests/no_inputs_ros_feedback.cpp +++ b/buoy_tests/tests/no_inputs_ros_feedback.cpp @@ -48,8 +48,10 @@ class NoInputsROSNode final : public buoy_api::Interface auto spin = [this]() { + rclcpp::Rate rate(50.0); while (rclcpp::ok() && !stop_) { executor_->spin_once(); + rate.sleep(); } }; thread_executor_spin_ = std::thread(spin); diff --git a/buoy_tests/tests/pc_commands_ros_feedback.cpp b/buoy_tests/tests/pc_commands_ros_feedback.cpp index 210fb0ec..eb04f5ee 100644 --- a/buoy_tests/tests/pc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/pc_commands_ros_feedback.cpp @@ -98,8 +98,10 @@ class PCROSNode final : public buoy_api::Interface auto spin = [this]() { + rclcpp::Rate rate(50.0); while (rclcpp::ok() && !stop_) { executor_->spin_once(); + rate.sleep(); } }; thread_executor_spin_ = std::thread(spin); diff --git a/buoy_tests/tests/sc_commands_ros_feedback.cpp b/buoy_tests/tests/sc_commands_ros_feedback.cpp index 1359c302..bf190032 100644 --- a/buoy_tests/tests/sc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/sc_commands_ros_feedback.cpp @@ -63,8 +63,10 @@ class SCROSNode final : public buoy_api::Interface auto spin = [this]() { + rclcpp::Rate rate(50.0); while (rclcpp::ok() && !stop_) { executor_->spin_once(); + rate.sleep(); } }; thread_executor_spin_ = std::thread(spin); From b889d47ba2a6c19080e630abbb5cec318fad885b Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Mon, 9 Jan 2023 18:12:54 -0800 Subject: [PATCH 25/57] linters Signed-off-by: Michael Anderson --- buoy_gazebo/src/PTOFriction/PTOFriction.cpp | 14 ++++++-------- buoy_gazebo/src/PTOFriction/PTOFriction.hpp | 4 ++-- buoy_tests/tests/test_pto_friction.cpp | 4 ++-- 3 files changed, 10 insertions(+), 12 deletions(-) diff --git a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp index 594d7f75..2327b6d0 100644 --- a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp @@ -12,9 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -<<<<<<< HEAD #include "PTOFriction.hpp" +#include +#include +#include +#include +#include + #include #include #include @@ -23,15 +28,8 @@ #include #include -#include - #include -#include -#include -#include -#include - namespace buoy_gazebo { diff --git a/buoy_gazebo/src/PTOFriction/PTOFriction.hpp b/buoy_gazebo/src/PTOFriction/PTOFriction.hpp index e185bfdb..74d652c6 100644 --- a/buoy_gazebo/src/PTOFriction/PTOFriction.hpp +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.hpp @@ -15,11 +15,11 @@ #ifndef PTOFRICTION__PTOFRICTION_HPP_ #define PTOFRICTION__PTOFRICTION_HPP_ -#include - #include #include +#include + namespace buoy_gazebo { diff --git a/buoy_tests/tests/test_pto_friction.cpp b/buoy_tests/tests/test_pto_friction.cpp index 96de676f..28ff7cd9 100644 --- a/buoy_tests/tests/test_pto_friction.cpp +++ b/buoy_tests/tests/test_pto_friction.cpp @@ -15,10 +15,10 @@ #include -#include - #include +#include + // NOLINTNEXTLINE class PTOFrictionTest : public ::testing::Test From ea2b1cc4b7d0cc8a84ea6734385fd6b4bb6fd344 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 10 Jan 2023 10:04:19 -0800 Subject: [PATCH 26/57] temp -- testing failed test Signed-off-by: Michael Anderson --- .github/workflows/build-and-test.sh | 3 ++- buoy_tests/launch/pc_commands_ros_feedback_py.launch.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index d34c205a..1e300022 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -44,5 +44,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 $COLCON_WS/install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py colcon test-result diff --git a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py index 58fe210e..1b35dd47 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py @@ -222,7 +222,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_timeout_iterations, self.test_helper.iterations) - time.sleep(0.5) + time.sleep(5.5) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) From 09eaceb64fb2528c71c71309112512f7440c24a1 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 10 Jan 2023 13:55:30 -0800 Subject: [PATCH 27/57] fixing timing issues in python tests Signed-off-by: Michael Anderson --- .../launch/pc_commands_ros_feedback_py.launch.py | 2 +- buoy_tests/testing_utils/utils.py | 15 ++++++++++++++- buoy_tests/tests/fixture_server.cpp | 4 ++++ 3 files changed, 19 insertions(+), 2 deletions(-) diff --git a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py index 1b35dd47..58fe210e 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py @@ -222,7 +222,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_timeout_iterations, self.test_helper.iterations) - time.sleep(5.5) + time.sleep(0.5) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) diff --git a/buoy_tests/testing_utils/utils.py b/buoy_tests/testing_utils/utils.py index c6bcb927..b50da2b6 100644 --- a/buoy_tests/testing_utils/utils.py +++ b/buoy_tests/testing_utils/utils.py @@ -13,7 +13,9 @@ # limitations under the License. import asyncio +import threading from threading import Thread +import time import unittest from buoy_api import Interface @@ -181,14 +183,25 @@ def setUp(self): self.executor = MultiThreadedExecutor() self.executor.add_node(self.node) self.executor.add_node(self.test_helper) - self.executor_thread = Thread(target=self.executor.spin) + self.executor_thread = Thread(target=self.spin) + self.executor_thread.stop = False self.executor_thread.daemon = True self.executor_thread.start() + + def spin(self): + # TODO(anyone) rate doesn't update from sim /clock this way and blocks forever + # rate = self.node.create_rate(25.0) + t = threading.currentThread() + while rclpy.ok() and not getattr(t, "stop", False): + self.executor.spin_once(1.0) + # rate.sleep() + time.sleep(1.0/50.0) # TODO(anyone) put back when TestFixture fixed upstream # self.node.start() # set_verbosity(3) def tearDown(self): + self.executor_thread.stop = True rclpy.shutdown() self.assertFalse(rclpy.ok()) self.executor.shutdown() diff --git a/buoy_tests/tests/fixture_server.cpp b/buoy_tests/tests/fixture_server.cpp index fca0538b..6b933280 100644 --- a/buoy_tests/tests/fixture_server.cpp +++ b/buoy_tests/tests/fixture_server.cpp @@ -31,6 +31,9 @@ #include +// NOLINTNEXTLINE +using namespace std::chrono; + TEST(BuoyTests, RunServer) { // Skip debug messages to run faster @@ -127,5 +130,6 @@ TEST(BuoyTests, RunServer) RCLCPP_INFO(rclcpp::get_logger("run_server"), "Ready to run test server."); rclcpp::spin(node); + std::this_thread::sleep_for(1s); // need this for launch_test to know we shut down RCLCPP_INFO(rclcpp::get_logger("run_server"), "Shutting down test server."); } From 7d9cffff7d5020724bf7245235fa86c09c711434 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 10 Jan 2023 14:23:56 -0800 Subject: [PATCH 28/57] Revert "fixing timing issues in python tests" This reverts commit 09eaceb64fb2528c71c71309112512f7440c24a1. --- .../launch/pc_commands_ros_feedback_py.launch.py | 2 +- buoy_tests/testing_utils/utils.py | 15 +-------------- buoy_tests/tests/fixture_server.cpp | 4 ---- 3 files changed, 2 insertions(+), 19 deletions(-) diff --git a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py index 58fe210e..1b35dd47 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py @@ -222,7 +222,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_timeout_iterations, self.test_helper.iterations) - time.sleep(0.5) + time.sleep(5.5) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) diff --git a/buoy_tests/testing_utils/utils.py b/buoy_tests/testing_utils/utils.py index b50da2b6..c6bcb927 100644 --- a/buoy_tests/testing_utils/utils.py +++ b/buoy_tests/testing_utils/utils.py @@ -13,9 +13,7 @@ # limitations under the License. import asyncio -import threading from threading import Thread -import time import unittest from buoy_api import Interface @@ -183,25 +181,14 @@ def setUp(self): self.executor = MultiThreadedExecutor() self.executor.add_node(self.node) self.executor.add_node(self.test_helper) - self.executor_thread = Thread(target=self.spin) - self.executor_thread.stop = False + self.executor_thread = Thread(target=self.executor.spin) self.executor_thread.daemon = True self.executor_thread.start() - - def spin(self): - # TODO(anyone) rate doesn't update from sim /clock this way and blocks forever - # rate = self.node.create_rate(25.0) - t = threading.currentThread() - while rclpy.ok() and not getattr(t, "stop", False): - self.executor.spin_once(1.0) - # rate.sleep() - time.sleep(1.0/50.0) # TODO(anyone) put back when TestFixture fixed upstream # self.node.start() # set_verbosity(3) def tearDown(self): - self.executor_thread.stop = True rclpy.shutdown() self.assertFalse(rclpy.ok()) self.executor.shutdown() diff --git a/buoy_tests/tests/fixture_server.cpp b/buoy_tests/tests/fixture_server.cpp index 6b933280..fca0538b 100644 --- a/buoy_tests/tests/fixture_server.cpp +++ b/buoy_tests/tests/fixture_server.cpp @@ -31,9 +31,6 @@ #include -// NOLINTNEXTLINE -using namespace std::chrono; - TEST(BuoyTests, RunServer) { // Skip debug messages to run faster @@ -130,6 +127,5 @@ TEST(BuoyTests, RunServer) RCLCPP_INFO(rclcpp::get_logger("run_server"), "Ready to run test server."); rclcpp::spin(node); - std::this_thread::sleep_for(1s); // need this for launch_test to know we shut down RCLCPP_INFO(rclcpp::get_logger("run_server"), "Shutting down test server."); } From 946dcec4cf70334ed1cc95bbe2997a02bca380a3 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 10 Jan 2023 14:31:34 -0800 Subject: [PATCH 29/57] try to fix with sleeps :( Signed-off-by: Michael Anderson --- .../launch/pc_commands_ros_feedback_py.launch.py | 16 ++++++++-------- buoy_tests/tests/fixture_server.cpp | 4 ++++ 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py index 1b35dd47..0b23b3f2 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py @@ -105,7 +105,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_policy_ = PBTorqueControlPolicy() self.set_params(torque_policy_) - time.sleep(0.5) + time.sleep(2.0) clock = self.node.get_clock() t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, 0) @@ -120,7 +120,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertTrue(self.test_helper.run_status) self.assertEqual(self.test_helper.iterations, feedbackCheckIterations) - time.sleep(0.5) + time.sleep(2.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -137,7 +137,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertTrue(self.test_helper.run_status) self.assertEqual(self.test_helper.iterations, preCmdIterations + feedbackCheckIterations) - time.sleep(0.5) + time.sleep(2.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -157,7 +157,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertEqual(preCmdIterations + 2 * feedbackCheckIterations, self.test_helper.iterations) - time.sleep(0.5) + time.sleep(2.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -180,7 +180,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertEqual(preCmdIterations + 3 * feedbackCheckIterations, self.test_helper.iterations) - time.sleep(0.5) + time.sleep(2.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -203,7 +203,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertEqual(preCmdIterations + 4 * feedbackCheckIterations, self.test_helper.iterations) - time.sleep(0.5) + time.sleep(2.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -253,7 +253,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_timeout_iterations + bias_curr_iterations, self.test_helper.iterations) - time.sleep(0.5) + time.sleep(2.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -270,7 +270,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_timeout_iterations + bias_curr_timeout_iterations, self.test_helper.iterations) - time.sleep(0.5) + time.sleep(2.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) diff --git a/buoy_tests/tests/fixture_server.cpp b/buoy_tests/tests/fixture_server.cpp index fca0538b..d77490a2 100644 --- a/buoy_tests/tests/fixture_server.cpp +++ b/buoy_tests/tests/fixture_server.cpp @@ -31,6 +31,9 @@ #include +// NOLINTNEXTLINE +using namespace std::chrono; + TEST(BuoyTests, RunServer) { // Skip debug messages to run faster @@ -127,5 +130,6 @@ TEST(BuoyTests, RunServer) RCLCPP_INFO(rclcpp::get_logger("run_server"), "Ready to run test server."); rclcpp::spin(node); + std::this_thread::sleep_for(1s); // needed for launch_test to know we shut down RCLCPP_INFO(rclcpp::get_logger("run_server"), "Shutting down test server."); } From 71ce5dedea314a06c84174afb5e66f2cc29eaf71 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 10 Jan 2023 14:48:56 -0800 Subject: [PATCH 30/57] try to fix with sleeps :( Signed-off-by: Michael Anderson --- .../launch/pc_commands_ros_feedback_py.launch.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py index 0b23b3f2..514a23c0 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py @@ -105,7 +105,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_policy_ = PBTorqueControlPolicy() self.set_params(torque_policy_) - time.sleep(2.0) + time.sleep(10.0) clock = self.node.get_clock() t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, 0) @@ -120,7 +120,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertTrue(self.test_helper.run_status) self.assertEqual(self.test_helper.iterations, feedbackCheckIterations) - time.sleep(2.0) + time.sleep(10.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -137,7 +137,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertTrue(self.test_helper.run_status) self.assertEqual(self.test_helper.iterations, preCmdIterations + feedbackCheckIterations) - time.sleep(2.0) + time.sleep(10.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -157,7 +157,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertEqual(preCmdIterations + 2 * feedbackCheckIterations, self.test_helper.iterations) - time.sleep(2.0) + time.sleep(10.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -180,7 +180,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertEqual(preCmdIterations + 3 * feedbackCheckIterations, self.test_helper.iterations) - time.sleep(2.0) + time.sleep(10.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -203,7 +203,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertEqual(preCmdIterations + 4 * feedbackCheckIterations, self.test_helper.iterations) - time.sleep(2.0) + time.sleep(10.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -253,7 +253,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_timeout_iterations + bias_curr_iterations, self.test_helper.iterations) - time.sleep(2.0) + time.sleep(10.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -270,7 +270,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_timeout_iterations + bias_curr_timeout_iterations, self.test_helper.iterations) - time.sleep(2.0) + time.sleep(10.0) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) From ca56bac26e50db91d9ab893affa4371ba2e9a2e4 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 10 Jan 2023 17:19:49 -0800 Subject: [PATCH 31/57] enable rosbag for testing ci Signed-off-by: Michael Anderson --- .../pc_commands_ros_feedback_py.launch.py | 20 +++++++++---------- buoy_tests/testing_utils/utils.py | 15 +++++++++++++- 2 files changed, 24 insertions(+), 11 deletions(-) diff --git a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py index 514a23c0..2848123c 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py @@ -27,7 +27,7 @@ def generate_test_description(): - return default_generate_test_description() + return default_generate_test_description(enable_rosbag=True) config = os.path.join( @@ -105,7 +105,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_policy_ = PBTorqueControlPolicy() self.set_params(torque_policy_) - time.sleep(10.0) + time.sleep(0.5) clock = self.node.get_clock() t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, 0) @@ -120,7 +120,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertTrue(self.test_helper.run_status) self.assertEqual(self.test_helper.iterations, feedbackCheckIterations) - time.sleep(10.0) + time.sleep(0.5) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -137,7 +137,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertTrue(self.test_helper.run_status) self.assertEqual(self.test_helper.iterations, preCmdIterations + feedbackCheckIterations) - time.sleep(10.0) + time.sleep(0.5) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -157,7 +157,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertEqual(preCmdIterations + 2 * feedbackCheckIterations, self.test_helper.iterations) - time.sleep(10.0) + time.sleep(0.5) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -180,7 +180,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertEqual(preCmdIterations + 3 * feedbackCheckIterations, self.test_helper.iterations) - time.sleep(10.0) + time.sleep(0.5) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -203,7 +203,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): self.assertEqual(preCmdIterations + 4 * feedbackCheckIterations, self.test_helper.iterations) - time.sleep(10.0) + time.sleep(0.5) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -222,7 +222,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_timeout_iterations, self.test_helper.iterations) - time.sleep(5.5) + time.sleep(0.5) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -253,7 +253,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_timeout_iterations + bias_curr_iterations, self.test_helper.iterations) - time.sleep(10.0) + time.sleep(0.5) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) @@ -270,7 +270,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): torque_timeout_iterations + bias_curr_timeout_iterations, self.test_helper.iterations) - time.sleep(10.0) + time.sleep(0.5) t, _ = clock.now().seconds_nanoseconds() self.assertEqual(t, self.test_helper.iterations // 1000) diff --git a/buoy_tests/testing_utils/utils.py b/buoy_tests/testing_utils/utils.py index c6bcb927..d110e4aa 100644 --- a/buoy_tests/testing_utils/utils.py +++ b/buoy_tests/testing_utils/utils.py @@ -40,7 +40,7 @@ from rclpy.parameter import Parameter -def default_generate_test_description(server='fixture_server'): +def default_generate_test_description(server='fixture_server', enable_rosbag=False): # Test fixture gazebo_test_fixture = launchNode( @@ -54,6 +54,19 @@ def default_generate_test_description(server='fixture_server'): arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], output='screen') + if enable_rosbag: + rosbag = launch.actions.ExecuteProcess( + cmd=['ros2', 'bag', 'record', '-a'], + output='screen' + ) + + return launch.LaunchDescription([ + gazebo_test_fixture, + bridge, + rosbag, + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest() + ]), locals() return launch.LaunchDescription([ gazebo_test_fixture, bridge, From 363dc498de83dd3a965c665a7428f42f0700ddc1 Mon Sep 17 00:00:00 2001 From: andermi Date: Tue, 10 Jan 2023 21:25:19 -0500 Subject: [PATCH 32/57] debug rosbag in CI --- .github/workflows/build-and-test.sh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 1e300022..94ac1df5 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -45,5 +45,7 @@ source $COLCON_WS/install/setup.bash # Test all buoy packages #colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+ +ros2 -h +ls /opt/ros/humble/bin/ros2 launch_test $COLCON_WS/install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py colcon test-result From 21dc44ed22fece0060c226b46cc519c5dd3a9e8d Mon Sep 17 00:00:00 2001 From: andermi Date: Tue, 10 Jan 2023 21:42:16 -0500 Subject: [PATCH 33/57] put back --- .github/workflows/build-and-test.sh | 2 -- 1 file changed, 2 deletions(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 94ac1df5..1e300022 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -45,7 +45,5 @@ source $COLCON_WS/install/setup.bash # Test all buoy packages #colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+ -ros2 -h -ls /opt/ros/humble/bin/ros2 launch_test $COLCON_WS/install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py colcon test-result From 271053bf69d36a437b170c5c9807a6edd701bb1c Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 10 Jan 2023 18:43:32 -0800 Subject: [PATCH 34/57] get rosbag working in ci Signed-off-by: Michael Anderson --- buoy_tests/testing_utils/utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buoy_tests/testing_utils/utils.py b/buoy_tests/testing_utils/utils.py index d110e4aa..dd81d397 100644 --- a/buoy_tests/testing_utils/utils.py +++ b/buoy_tests/testing_utils/utils.py @@ -56,7 +56,7 @@ def default_generate_test_description(server='fixture_server', enable_rosbag=Fal if enable_rosbag: rosbag = launch.actions.ExecuteProcess( - cmd=['ros2', 'bag', 'record', '-a'], + cmd=['/opt/ros/humble/bin/ros2', 'bag', 'record', '-a'], output='screen' ) From 74f4348751ff7c2aa78525d3124752a40b9e7b4d Mon Sep 17 00:00:00 2001 From: andermi Date: Tue, 10 Jan 2023 22:09:48 -0500 Subject: [PATCH 35/57] install ros2 cli --- .github/workflows/build-and-test.sh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 1e300022..917e6c3d 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -37,6 +37,8 @@ rosdep init rosdep update rosdep install --from-paths ./ -i -y -r --rosdistro $ROS_DISTRO +apt install -y ros-humble-ros2cli + # Build everything up to buoy_gazebo source /opt/ros/$ROS_DISTRO/setup.bash cd $COLCON_WS From 182a8d4cc471943faed50b7296e39c8ba1f1c5c1 Mon Sep 17 00:00:00 2001 From: andermi Date: Tue, 10 Jan 2023 22:18:53 -0500 Subject: [PATCH 36/57] install rosbag --- .github/workflows/build-and-test.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 917e6c3d..f16f25f1 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -37,7 +37,7 @@ rosdep init rosdep update rosdep install --from-paths ./ -i -y -r --rosdistro $ROS_DISTRO -apt install -y ros-humble-ros2cli +apt install -y ros-humble-ros2cli ros-humble-rosbag2 ros-humble-rosbag2-transport # Build everything up to buoy_gazebo source /opt/ros/$ROS_DISTRO/setup.bash From 7eb591c068a184740b8a7d9b9a8c1ef80060f45e Mon Sep 17 00:00:00 2001 From: andermi Date: Wed, 11 Jan 2023 01:10:29 -0500 Subject: [PATCH 37/57] archive rosbag --- .github/workflows/ci.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 6dfcd6a5..8673b8ab 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -21,3 +21,8 @@ jobs: env: GZ_VERSION: ${{ matrix.gz-version }} ROS_DISTRO: ${{ matrix.ros-distro }} + - name: Archive rosbag + uses: actions/upload-artifact@v3 + with: + name: rosbag + path: rosbag/ From b6e86e9b653eccf2461dcf13be8d539780b9eb55 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 10 Jan 2023 22:13:23 -0800 Subject: [PATCH 38/57] set name of rosbag Signed-off-by: Michael Anderson --- buoy_tests/testing_utils/utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buoy_tests/testing_utils/utils.py b/buoy_tests/testing_utils/utils.py index dd81d397..a9c92457 100644 --- a/buoy_tests/testing_utils/utils.py +++ b/buoy_tests/testing_utils/utils.py @@ -56,7 +56,7 @@ def default_generate_test_description(server='fixture_server', enable_rosbag=Fal if enable_rosbag: rosbag = launch.actions.ExecuteProcess( - cmd=['/opt/ros/humble/bin/ros2', 'bag', 'record', '-a'], + cmd=['ros2', 'bag', 'record', '-o', 'rosbag', '-a'], output='screen' ) From c518057876a56d488de8cf1c712d2bf9b6809eee Mon Sep 17 00:00:00 2001 From: andermi Date: Wed, 11 Jan 2023 01:16:20 -0500 Subject: [PATCH 39/57] fix rosbag path --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8673b8ab..97697e81 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -25,4 +25,4 @@ jobs: uses: actions/upload-artifact@v3 with: name: rosbag - path: rosbag/ + path: ~/ws/rosbag/ From 98420cdb8f8830339d81df37a70dc56a8d15a3ed Mon Sep 17 00:00:00 2001 From: andermi Date: Wed, 11 Jan 2023 09:27:49 -0800 Subject: [PATCH 40/57] debug rosbag archive --- .github/workflows/build-and-test.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index f16f25f1..3cd9124d 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -49,3 +49,4 @@ source $COLCON_WS/install/setup.bash #colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+ launch_test $COLCON_WS/install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py colcon test-result +ls From 8671ad182305205ccd33a273c37317474ad3efd9 Mon Sep 17 00:00:00 2001 From: andermi Date: Wed, 11 Jan 2023 10:10:03 -0800 Subject: [PATCH 41/57] always upload rosbag --- .github/workflows/ci.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 97697e81..bff70587 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -22,6 +22,7 @@ jobs: GZ_VERSION: ${{ matrix.gz-version }} ROS_DISTRO: ${{ matrix.ros-distro }} - name: Archive rosbag + if: success() || failure() uses: actions/upload-artifact@v3 with: name: rosbag From f6159d6124a59d77f31bb2a3857e078a88ffc83e Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 11 Jan 2023 10:59:21 -0800 Subject: [PATCH 42/57] debug pc commands py test Signed-off-by: Michael Anderson --- buoy_tests/launch/pc_commands_ros_feedback_py.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py index 2848123c..3deb2354 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py @@ -212,7 +212,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info): ################################################## # Check return to default winding current damping - torque_timeout_iterations = 2000 + torque_timeout_iterations = 2500 # Run to let winding current finish self.test_helper.run(torque_timeout_iterations - 2 * feedbackCheckIterations) From 989d93180403f65bba96dd2ab295513358f0a618 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 11 Jan 2023 11:45:13 -0800 Subject: [PATCH 43/57] debug pc commands py test Signed-off-by: Michael Anderson --- .../PowerController/PowerController.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index d9a5fcf6..1b4862d2 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -296,7 +296,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]"); @@ -426,7 +426,7 @@ struct PowerControllerPrivate // override if (command) { if (!watch.Running()) { - RCLCPP_DEBUG_STREAM( + RCLCPP_INFO_STREAM( ros_->node_->get_logger(), "Override " << command_name << " (" << duration.seconds() << "s)"); @@ -439,7 +439,7 @@ struct PowerControllerPrivate watch.Stop(); command = false; - RCLCPP_DEBUG_STREAM( + RCLCPP_INFO_STREAM( ros_->node_->get_logger(), "Stopped overriding " << command_name << " after (" << watch.ElapsedRunTime().seconds() << "s)"); @@ -509,7 +509,7 @@ struct PowerControllerPrivate command = command_value; duration = timeout + watch.ElapsedRunTime(); - RCLCPP_DEBUG_STREAM( + RCLCPP_INFO_STREAM( ros_->node_->get_logger(), "Continue Override " << command_name << " (" << duration.seconds() << "s)"); @@ -717,6 +717,13 @@ void PowerController::PreUpdate( this->dataPtr->manageCommandTimers(pto_state); this->dataPtr->manageCommandStates(pto_state); + + if (this->dataPtr->services_->torque_command_watch_.Running()) { + RCLCPP_INFO_STREAM( + this->dataPtr->ros_->node_->get_logger(), + "Winding Current (Torque) Override has been running for (" << + this->dataPtr->services_->torque_command_watch_.ElapsedRunTime().seconds() << " s)"); + } _ecm.SetComponentData( this->dataPtr->jointEntity_, From 9bec6a152571fd85abdea69c8cbe5e457140bc90 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 11 Jan 2023 12:16:55 -0800 Subject: [PATCH 44/57] debug pc commands py test Signed-off-by: Michael Anderson --- buoy_tests/tests/fixture_server.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/buoy_tests/tests/fixture_server.cpp b/buoy_tests/tests/fixture_server.cpp index d77490a2..a2e14c44 100644 --- a/buoy_tests/tests/fixture_server.cpp +++ b/buoy_tests/tests/fixture_server.cpp @@ -92,6 +92,7 @@ TEST(BuoyTests, RunServer) } std::shared_ptr node = rclcpp::Node::make_shared("gz_fixture_server"); + std::atomic stop = false; rclcpp::Service::SharedPtr service = node->create_service( @@ -103,6 +104,7 @@ TEST(BuoyTests, RunServer) RCLCPP_INFO( rclcpp::get_logger("run_server"), "Incoming request to shutdown"); + stop = true; rclcpp::shutdown(); response->success = true; return; @@ -129,7 +131,22 @@ TEST(BuoyTests, RunServer) RCLCPP_INFO(rclcpp::get_logger("run_server"), "Ready to run test server."); - rclcpp::spin(node); + // rclcpp::spin(node); + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor = + std::make_shared(); + executor->add_node(node); + + rclcpp::Rate rate(50.0); + while (rclcpp::ok() && !stop) { + executor->spin_once(); + rate.sleep(); + } + + if (executor) { + executor->cancel(); + } + std::this_thread::sleep_for(1s); // needed for launch_test to know we shut down + RCLCPP_INFO(rclcpp::get_logger("run_server"), "Shutting down test server."); } From a49bcbeb19081cf596aee5fd2f3d69818aecf4e0 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 11 Jan 2023 14:22:20 -0800 Subject: [PATCH 45/57] start cleaning up debugging Signed-off-by: Michael Anderson --- .github/workflows/build-and-test.sh | 5 ++--- .github/workflows/ci.yml | 6 ++++-- .../PowerController/PowerController.cpp | 12 +++++++----- .../pc_commands_ros_feedback_py.launch.py | 3 ++- buoy_tests/testing_utils/utils.py | 18 +++++++++++++----- 5 files changed, 28 insertions(+), 16 deletions(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 3cd9124d..2f3e8b99 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -37,6 +37,7 @@ rosdep init rosdep update rosdep install --from-paths ./ -i -y -r --rosdistro $ROS_DISTRO +# For rosbag2 test artifacts apt install -y ros-humble-ros2cli ros-humble-rosbag2 ros-humble-rosbag2-transport # Build everything up to buoy_gazebo @@ -46,7 +47,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 $COLCON_WS/install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py +colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+ colcon test-result -ls diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index bff70587..264c5fb8 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -25,5 +25,7 @@ jobs: if: success() || failure() uses: actions/upload-artifact@v3 with: - name: rosbag - path: ~/ws/rosbag/ + name: rosbags + path: ~/ws/build/buoy_tests/rosbag* + if-no-files-found: ignore + retention-days: 2 diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 1b4862d2..6e027b09 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -296,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]"); @@ -426,7 +426,7 @@ struct PowerControllerPrivate // override if (command) { if (!watch.Running()) { - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( ros_->node_->get_logger(), "Override " << command_name << " (" << duration.seconds() << "s)"); @@ -439,7 +439,7 @@ struct PowerControllerPrivate watch.Stop(); command = false; - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( ros_->node_->get_logger(), "Stopped overriding " << command_name << " after (" << watch.ElapsedRunTime().seconds() << "s)"); @@ -509,7 +509,7 @@ struct PowerControllerPrivate command = command_value; duration = timeout + watch.ElapsedRunTime(); - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( ros_->node_->get_logger(), "Continue Override " << command_name << " (" << duration.seconds() << "s)"); @@ -717,13 +717,15 @@ void PowerController::PreUpdate( this->dataPtr->manageCommandTimers(pto_state); this->dataPtr->manageCommandStates(pto_state); - + + /* if (this->dataPtr->services_->torque_command_watch_.Running()) { RCLCPP_INFO_STREAM( this->dataPtr->ros_->node_->get_logger(), "Winding Current (Torque) Override has been running for (" << this->dataPtr->services_->torque_command_watch_.ElapsedRunTime().seconds() << " s)"); } + */ _ecm.SetComponentData( this->dataPtr->jointEntity_, diff --git a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py index 3deb2354..4623c3f7 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py @@ -27,7 +27,8 @@ def generate_test_description(): - return default_generate_test_description(enable_rosbag=True) + return default_generate_test_description(enable_rosbag=True, + rosbag_name='rosbag2_pc_cmds_py') config = os.path.join( diff --git a/buoy_tests/testing_utils/utils.py b/buoy_tests/testing_utils/utils.py index a9c92457..3d7e439b 100644 --- a/buoy_tests/testing_utils/utils.py +++ b/buoy_tests/testing_utils/utils.py @@ -40,7 +40,9 @@ from rclpy.parameter import Parameter -def default_generate_test_description(server='fixture_server', enable_rosbag=False): +def default_generate_test_description(server='fixture_server', + enable_rosbag=False, + rosbag_name=None): # Test fixture gazebo_test_fixture = launchNode( @@ -55,10 +57,16 @@ def default_generate_test_description(server='fixture_server', enable_rosbag=Fal output='screen') if enable_rosbag: - rosbag = launch.actions.ExecuteProcess( - cmd=['ros2', 'bag', 'record', '-o', 'rosbag', '-a'], - output='screen' - ) + if rosbag_name is not None: + rosbag = launch.actions.ExecuteProcess( + cmd=['ros2', 'bag', 'record', '-o', rosbag_name, '-a'], + output='screen' + ) + else: + rosbag = launch.actions.ExecuteProcess( + cmd=['ros2', 'bag', 'record', '-a'], + output='screen' + ) return launch.LaunchDescription([ gazebo_test_fixture, From 85ae883dc3e76a888c30922e84dddae022effa3f Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 11 Jan 2023 15:51:44 -0800 Subject: [PATCH 46/57] debug no_inputs timeout Signed-off-by: Michael Anderson --- .github/workflows/build-and-test.sh | 3 ++- .github/workflows/ci.yml | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 2f3e8b99..68c950c0 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -47,5 +47,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+ +$COLCON_WS/install/buoy_tests/lib/buoy_tests/no_inputs colcon test-result diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 264c5fb8..3de53208 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -21,7 +21,7 @@ jobs: env: GZ_VERSION: ${{ matrix.gz-version }} ROS_DISTRO: ${{ matrix.ros-distro }} - - name: Archive rosbag + - name: Archive rosbags if: success() || failure() uses: actions/upload-artifact@v3 with: From d8b08060abf1c35691abbc4e512412d4942a883e Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 11 Jan 2023 16:20:49 -0800 Subject: [PATCH 47/57] no_inputs had intermittent failure; debug sc pump py test Signed-off-by: Michael Anderson --- .github/workflows/build-and-test.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 68c950c0..0f31e153 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -48,5 +48,5 @@ 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_WS/install/buoy_tests/lib/buoy_tests/no_inputs +launch_test $COLCON_WS/install/buoy_tests/share/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py colcon test-result From 3d1dd7e3a9c5ef0cfa444267cceac5527e9c3322 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 11 Jan 2023 17:12:54 -0800 Subject: [PATCH 48/57] debug sc pump py test Signed-off-by: Michael Anderson --- buoy_tests/launch/sc_pump_ros_feedback_py.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py b/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py index ca2572c5..65aa5f3f 100644 --- a/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py +++ b/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py @@ -22,7 +22,8 @@ def generate_test_description(): - return default_generate_test_description() + return default_generate_test_description(enable_rosbag=True, + rosbag_name='rosbag2_sc_pump_py') class BuoySCPumpPyTest(BuoyPyTests): From 48f8226e6454d32f752445ee4cea532f2fa46b39 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 11 Jan 2023 17:35:26 -0800 Subject: [PATCH 49/57] debug sc pump py test Signed-off-by: Michael Anderson --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 3de53208..fac25c11 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -26,6 +26,6 @@ jobs: uses: actions/upload-artifact@v3 with: name: rosbags - path: ~/ws/build/buoy_tests/rosbag* + path: ~/ws/rosbag* if-no-files-found: ignore retention-days: 2 From dcce4897a4c92cd4418dd1ecaf32610c07a53090 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Thu, 12 Jan 2023 09:21:49 -0800 Subject: [PATCH 50/57] intermittent sc pump py test also; put back to testing all Signed-off-by: Michael Anderson --- .github/workflows/build-and-test.sh | 3 +-- .github/workflows/ci.yml | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 0f31e153..2f3e8b99 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -47,6 +47,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 $COLCON_WS/install/buoy_tests/share/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py +colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+ colcon test-result diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index fac25c11..3de53208 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -26,6 +26,6 @@ jobs: uses: actions/upload-artifact@v3 with: name: rosbags - path: ~/ws/rosbag* + path: ~/ws/build/buoy_tests/rosbag* if-no-files-found: ignore retention-days: 2 From b2016b81e57441c82986890239c6f8b16221f2b9 Mon Sep 17 00:00:00 2001 From: andermi Date: Thu, 19 Jan 2023 14:24:42 -0500 Subject: [PATCH 51/57] change RMW to cyclone --- .github/workflows/build-and-test.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 2f3e8b99..c245d9cf 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -43,6 +43,7 @@ apt install -y ros-humble-ros2cli ros-humble-rosbag2 ros-humble-rosbag2-transpor # Build everything up to buoy_gazebo source /opt/ros/$ROS_DISTRO/setup.bash cd $COLCON_WS +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp colcon build --packages-up-to buoy_tests --event-handlers console_direct+ source $COLCON_WS/install/setup.bash From 546de38c1013638f94db002890b4157eb4f0d50b Mon Sep 17 00:00:00 2001 From: andermi Date: Thu, 19 Jan 2023 14:41:33 -0500 Subject: [PATCH 52/57] install cyclonedds --- .github/workflows/build-and-test.sh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index c245d9cf..df71fa81 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -40,6 +40,9 @@ rosdep install --from-paths ./ -i -y -r --rosdistro $ROS_DISTRO # For rosbag2 test artifacts apt install -y ros-humble-ros2cli ros-humble-rosbag2 ros-humble-rosbag2-transport +# for cyclonedds rmw implementation +apt install -y ros-humble-rmw-cyclonedds-cpp + # Build everything up to buoy_gazebo source /opt/ros/$ROS_DISTRO/setup.bash cd $COLCON_WS From 10d23d010143f414bed0d964cd44f86d24e96dbf Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Thu, 19 Jan 2023 15:57:46 -0800 Subject: [PATCH 53/57] try a sleep to fix timeout Signed-off-by: Michael Anderson --- buoy_tests/tests/pc_commands_ros_feedback.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/buoy_tests/tests/pc_commands_ros_feedback.cpp b/buoy_tests/tests/pc_commands_ros_feedback.cpp index eb04f5ee..f2f9f0c1 100644 --- a/buoy_tests/tests/pc_commands_ros_feedback.cpp +++ b/buoy_tests/tests/pc_commands_ros_feedback.cpp @@ -458,4 +458,6 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback) // Sanity check that the test ran EXPECT_NE(gz::sim::kNullEntity, buoyEntity); + + std::this_thread::sleep_for(1s); // needed for launch_test to know we shut down } From 7ecf43d00085f471e71e53755474d1eea93221a3 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Fri, 20 Jan 2023 17:05:23 -0800 Subject: [PATCH 54/57] disable rosbags for pc and sc pump tests Signed-off-by: Michael Anderson --- buoy_tests/launch/pc_commands_ros_feedback_py.launch.py | 3 +-- buoy_tests/launch/sc_pump_ros_feedback_py.launch.py | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py index 4623c3f7..2710f447 100644 --- a/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py +++ b/buoy_tests/launch/pc_commands_ros_feedback_py.launch.py @@ -27,8 +27,7 @@ def generate_test_description(): - return default_generate_test_description(enable_rosbag=True, - rosbag_name='rosbag2_pc_cmds_py') + return default_generate_test_description() config = os.path.join( diff --git a/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py b/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py index 65aa5f3f..ca2572c5 100644 --- a/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py +++ b/buoy_tests/launch/sc_pump_ros_feedback_py.launch.py @@ -22,8 +22,7 @@ def generate_test_description(): - return default_generate_test_description(enable_rosbag=True, - rosbag_name='rosbag2_sc_pump_py') + return default_generate_test_description() class BuoySCPumpPyTest(BuoyPyTests): From 4b0ce7df5e9e01da0f8e5e42d473e8a96713aaa0 Mon Sep 17 00:00:00 2001 From: andermi Date: Fri, 20 Jan 2023 21:39:33 -0500 Subject: [PATCH 55/57] Update buoy_tests/worlds/TestMachine.sdf Co-authored-by: Mabel Zhang --- buoy_tests/worlds/TestMachine.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buoy_tests/worlds/TestMachine.sdf b/buoy_tests/worlds/TestMachine.sdf index 49a109a5..30e19133 100644 --- a/buoy_tests/worlds/TestMachine.sdf +++ b/buoy_tests/worlds/TestMachine.sdf @@ -26,7 +26,7 @@ 0.8 0.8 0.8 -6 0 6 0 0.5 0 - + floating From 93c838416514a5c445185a43bacf4842108c1f8d Mon Sep 17 00:00:00 2001 From: andermi Date: Fri, 20 Jan 2023 22:49:10 -0500 Subject: [PATCH 56/57] swap buoy_all.yaml back to main --- .github/workflows/build-and-test.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index df71fa81..4f370844 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -28,7 +28,7 @@ apt-get install -y git \ cd $COLCON_WS_SRC cp -r $GITHUB_WORKSPACE $COLCON_WS_SRC -wget https://raw.githubusercontent.com/osrf/buoy_entrypoint/chapulina/humble_garden/buoy_all.yaml +wget https://raw.githubusercontent.com/osrf/buoy_entrypoint/main/buoy_all.yaml vcs import --skip-existing < buoy_all.yaml rm -rf buoy_examples From 0577b1d261a778516bfc15a2584f3f88705176bc Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Sat, 21 Jan 2023 00:26:08 -0800 Subject: [PATCH 57/57] Revert "swap buoy_all.yaml back to main" This reverts commit 93c838416514a5c445185a43bacf4842108c1f8d. --- .github/workflows/build-and-test.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 4f370844..df71fa81 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -28,7 +28,7 @@ apt-get install -y git \ cd $COLCON_WS_SRC cp -r $GITHUB_WORKSPACE $COLCON_WS_SRC -wget https://raw.githubusercontent.com/osrf/buoy_entrypoint/main/buoy_all.yaml +wget https://raw.githubusercontent.com/osrf/buoy_entrypoint/chapulina/humble_garden/buoy_all.yaml vcs import --skip-existing < buoy_all.yaml rm -rf buoy_examples