From d5722806dd8ad23905fbd90e3fba83cfb43169d7 Mon Sep 17 00:00:00 2001 From: andermi Date: Mon, 29 May 2023 15:03:51 -0700 Subject: [PATCH] add support for pblog (#140) * add support for pblog Signed-off-by: Michael Anderson * change default pblog root; add latest_rosbag symlink Signed-off-by: Michael Anderson * Fixed issue by which wave-random seed wasn't being passed to Incident wave generator * Fix logging rates (#146) * add sim-time-aware Rate to controllers for publishing Signed-off-by: Michael Anderson * forgot shutdowns to stop hanging Signed-off-by: Michael Anderson * add copyright Signed-off-by: Michael Anderson * linters Signed-off-by: Michael Anderson * add async proxy to await future for pack rate calls (#147) Signed-off-by: Michael Anderson * use sim time from postupdate instead of imu time Signed-off-by: Michael Anderson * enable use_sim_time for AHRS Signed-off-by: Michael Anderson --------- Signed-off-by: Michael Anderson * linters Signed-off-by: Michael Anderson * moved tf sensors to heavecone link; rotated imu to match physical Signed-off-by: Michael Anderson * add gps Signed-off-by: Michael Anderson --------- Signed-off-by: Michael Anderson Co-authored-by: Andrew Hamilton --- .../models/mbari_wec/model.sdf.em | 2 +- .../models/mbari_wec_base/model.sdf.em | 45 ++++--- buoy_gazebo/gazebo/server.config | 5 + buoy_gazebo/launch/mbari_wec.launch.py | 22 ++++ buoy_gazebo/package.xml | 1 + buoy_gazebo/scripts/mbari_wec_batch | 21 +++- .../src/IncidentWaves/IncidentWaves.cpp | 5 + buoy_gazebo/src/buoy_utils/Rate.hpp | 111 ++++++++++++++++++ .../BatteryController/BatteryController.cpp | 12 +- .../PowerController/PowerController.cpp | 34 +++++- .../SpringController/SpringController.cpp | 90 ++++++++++---- .../TrefoilController/TrefoilController.cpp | 31 +++-- .../src/controllers/XBowAHRS/CMakeLists.txt | 2 + .../src/controllers/XBowAHRS/XBowAHRS.cpp | 78 ++++++++++-- buoy_gazebo/worlds/mbari_wec.sdf.em | 1 + 15 files changed, 391 insertions(+), 69 deletions(-) create mode 100644 buoy_gazebo/src/buoy_utils/Rate.hpp diff --git a/buoy_description/models/mbari_wec/model.sdf.em b/buoy_description/models/mbari_wec/model.sdf.em index b0606468..a93360d9 100644 --- a/buoy_description/models/mbari_wec/model.sdf.em +++ b/buoy_description/models/mbari_wec/model.sdf.em @@ -10,7 +10,7 @@ except NameError: try: inc_wave_seed except NameError: - inc_wave_seed = 42 # not defined so default + inc_wave_seed = 0 # not defined so default to 0 which changes seed every run # Check if battery state (battery_soc or battery_emf) was passed in via empy try: diff --git a/buoy_description/models/mbari_wec_base/model.sdf.em b/buoy_description/models/mbari_wec_base/model.sdf.em index 298b1a5d..dc6acfab 100644 --- a/buoy_description/models/mbari_wec_base/model.sdf.em +++ b/buoy_description/models/mbari_wec_base/model.sdf.em @@ -176,6 +176,15 @@ buoyancy_radius = ((3*buoyancy_disp)/(4*math.pi))**(1/3) 1 true + + Buoy_link/xbow_gps + 50 + + + + 1 + true + @@ -483,6 +492,25 @@ buoyancy_radius = ((3*buoyancy_disp)/(4*math.pi))**(1/3) + + + 0.0 0.0 0.0 0.0 1.570796 0.0 + HeaveCone_link/tf_imu + 50 + + + ENU + + + 1 + true + + + HeaveCone_link/tf_mag + 50 + 1 + true + @@ -512,23 +540,6 @@ buoyancy_radius = ((3*buoyancy_disp)/(4*math.pi))**(1/3) 0.1 0.1 .1 1 - - Trefoil_link/trefoil_imu - 50 - - - ENU - - - 1 - true - - - Trefoil_link/trefoil_mag - 50 - 1 - true - diff --git a/buoy_gazebo/gazebo/server.config b/buoy_gazebo/gazebo/server.config index 71795fef..af56d8f0 100644 --- a/buoy_gazebo/gazebo/server.config +++ b/buoy_gazebo/gazebo/server.config @@ -34,6 +34,11 @@ filename="gz-sim-imu-system" name="gz::sim::systems::Imu"> + + buoy_description buoy_interfaces + sim_pblog gz-sim7 rclcpp robot_state_publisher diff --git a/buoy_gazebo/scripts/mbari_wec_batch b/buoy_gazebo/scripts/mbari_wec_batch index fa7a94eb..08723bcb 100755 --- a/buoy_gazebo/scripts/mbari_wec_batch +++ b/buoy_gazebo/scripts/mbari_wec_batch @@ -355,7 +355,7 @@ def generate_simulations(sim_params_yaml): os.path.join(batch_results_dir, 'batch_runs.log')) with open(os.path.join(batch_results_dir, 'batch_runs.log'), 'w') as fd: fd.write(f'# Generated {len(batch_params)} simulation runs\n') - fd.write('RunIndex, SimReturnCode, StartTime, rosbag2FileName' + + fd.write('RunIndex, SimReturnCode, StartTime, rosbag2Filename, pblogFilename' + ', PhysicsStep, PhysicsRTF' + ', Seed, Duration, DoorState, ScaleFactor, BatteryState' + ', MeanPistonPosition, IncWaveSpectrumType;IncWaveSpectrumParams\n') @@ -446,11 +446,17 @@ def generate_simulations(sim_params_yaml): # Dynamically create launch file start_time = time.strftime("%Y%m%d%H%M%S") - rosbag2_name = 'rosbag2' + rosbag2_dir = 'rosbag2' + pblog_dir = 'pblog' run_results_dir = f'results_run_{idx}_{start_time}' - # TODO(anyone) also path for pblog when ready os.makedirs(os.path.join(batch_results_dir, run_results_dir)) + # python workaround for 'ln -sf' + os.symlink(os.path.join(run_results_dir, rosbag2_dir), + os.path.join(batch_results_dir, f'latest_rosbag_{start_time}'), target_is_directory=True) + os.replace(os.path.join(batch_results_dir, f'latest_rosbag_{start_time}'), + os.path.join(batch_results_dir, f'latest_rosbag')) + def generate_launch_description(): mbari_wec = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -459,13 +465,15 @@ def generate_simulations(sim_params_yaml): launch_arguments={'extra_gz_args': '-r' if enable_gui else '-rs' + f' --iterations {iterations}', 'world_file': 'mbari_wec_batch.sdf', - 'regenerate_models': 'false'}.items(), + 'regenerate_models': 'false', + 'pbloghome': batch_results_dir, + 'pblogdir': os.path.join(run_results_dir, pblog_dir)}.items(), ) # record all topics with rosbag2 rosbag2 = ExecuteProcess( cmd=['ros2', 'bag', 'record', - '-o', os.path.join(batch_results_dir, run_results_dir, rosbag2_name), '-a'], + '-o', os.path.join(batch_results_dir, run_results_dir, rosbag2_dir), '-a'], output='screen' ) @@ -484,7 +492,8 @@ def generate_simulations(sim_params_yaml): # Write to log with open(os.path.join(batch_results_dir, 'batch_runs.log'), 'a') as fd: fd.write(f'{idx}, {result}, {start_time}' + - f', {os.path.join(run_results_dir, rosbag2_name)}' + + f', {os.path.join(run_results_dir, rosbag2_dir)}' + + f', {os.path.join(run_results_dir, pblog_dir)}' + f', {print_optional(ps)}, {print_optional(physics_rtf)}' + f', {print_optional(seed)}, {duration}, {ds}, {sf}' + f', {print_optional(bs)}' + diff --git a/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp b/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp index d1d9d612..f9811629 100644 --- a/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp +++ b/buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp @@ -81,6 +81,11 @@ void IncidentWaves::Configure( return; } + double seed = SdfParamDouble(_sdf, "IncWaveSeed", 0.0); + this->dataPtr->Inc->SetSeed(seed); + + auto linkName = _sdf->Get("LinkName"); + auto SpectrumType = _sdf->Get("IncWaveSpectrumType"); // double beta = SdfParamDouble(_sdf, "WaveDir", 180.0); // Not yet implemented diff --git a/buoy_gazebo/src/buoy_utils/Rate.hpp b/buoy_gazebo/src/buoy_utils/Rate.hpp new file mode 100644 index 00000000..292b5337 --- /dev/null +++ b/buoy_gazebo/src/buoy_utils/Rate.hpp @@ -0,0 +1,111 @@ +// 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 BUOY_UTILS__RATE_HPP_ +#define BUOY_UTILS__RATE_HPP_ + + +#include + +#include +#include + + +namespace buoy_utils +{ + +using std::chrono::duration; +using std::chrono::duration_cast; +using std::chrono::nanoseconds; + +class SimRate : public rclcpp::RateBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SimRate); + + explicit SimRate(const double & rate, rclcpp::Clock::SharedPtr _clock) + : SimRate( + duration_cast(duration(1.0 / rate)), + _clock + ) + {} + + explicit SimRate( + const std::chrono::nanoseconds & period, + rclcpp::Clock::SharedPtr _clock) + : clock(_clock), + period_(period), + last_interval_( + rclcpp::Time(_clock->now(), + _clock->get_clock_type()) + ) + {} + + virtual bool sleep() + { + // Time coming into sleep + auto now = clock->now(); + // Time of next interval + auto next_interval = last_interval_ + period_; + // Detect backwards time flow + if (now < last_interval_) { + // Best thing to do is to set the next_interval to now + period + next_interval = now + period_; + } + // Calculate the time to sleep + auto time_to_sleep = next_interval - now; + // Update the interval + last_interval_ += period_; + // If the time_to_sleep is negative or zero, don't sleep + if (time_to_sleep <= rclcpp::Duration(0, 0U)) { + // If an entire cycle was missed then reset next interval. + // This might happen if the loop took more than a cycle. + // Or if time jumps forward. + if (now > next_interval + period_) { + last_interval_ = now + period_; + } + // Either way do not sleep and return false + return false; + } + // Sleep (will get interrupted by ctrl-c, may not sleep full time) + clock->sleep_for(time_to_sleep); + return true; + } + + virtual bool is_steady() const + { + return false; + } + + virtual void reset() + { + last_interval_ = clock->now(); + } + + rclcpp::Duration period() const + { + return period_; + } + +private: + RCLCPP_DISABLE_COPY(SimRate) + + rclcpp::Clock::SharedPtr clock = nullptr; + rclcpp::Duration period_; + rclcpp::Time last_interval_; +}; + +} // namespace buoy_utils + +#endif // BUOY_UTILS__RATE_HPP_ diff --git a/buoy_gazebo/src/controllers/BatteryController/BatteryController.cpp b/buoy_gazebo/src/controllers/BatteryController/BatteryController.cpp index e5cdd63b..9808fa15 100644 --- a/buoy_gazebo/src/controllers/BatteryController/BatteryController.cpp +++ b/buoy_gazebo/src/controllers/BatteryController/BatteryController.cpp @@ -33,9 +33,11 @@ #include +#include "buoy_utils/Rate.hpp" #include "ElectroHydraulicPTO/BatteryState.hpp" #include "BatteryController.hpp" + using namespace std::chrono_literals; namespace buoy_gazebo @@ -48,7 +50,7 @@ struct BatteryControllerROS2 bool use_sim_time_{true}; rclcpp::Publisher::SharedPtr bc_pub_{nullptr}; - std::unique_ptr pub_rate_{nullptr}; + std::unique_ptr pub_rate_{nullptr}; buoy_interfaces::msg::BCRecord bc_record_; double pub_rate_hz_{10.0}; }; @@ -122,7 +124,9 @@ struct BatteryControllerPrivate std::unique_lock next(next_access_mutex_); std::unique_lock data(data_mutex_); next.unlock(); - ros_->pub_rate_ = std::make_unique(ros_->pub_rate_hz_); + ros_->pub_rate_ = std::make_unique( + ros_->pub_rate_hz_, + ros_->node_->get_clock()); data.unlock(); } } @@ -155,6 +159,10 @@ BatteryController::BatteryController() BatteryController::~BatteryController() { // Stop ros2 threads + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + this->dataPtr->stop_ = true; if (this->dataPtr->ros_->executor_) { this->dataPtr->ros_->executor_->cancel(); diff --git a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp index 83e88f9d..ba163865 100644 --- a/buoy_gazebo/src/controllers/PowerController/PowerController.cpp +++ b/buoy_gazebo/src/controllers/PowerController/PowerController.cpp @@ -34,12 +34,14 @@ #include #include +#include #include #include #include #include #include +#include "buoy_utils/Rate.hpp" #include "ElectroHydraulicPTO/ElectroHydraulicState.hpp" #include "PowerController.hpp" @@ -56,7 +58,7 @@ struct PowerControllerROS2 bool use_sim_time_{true}; rclcpp::Publisher::SharedPtr pc_pub_{nullptr}; - std::unique_ptr pub_rate_{nullptr}; + std::unique_ptr pub_rate_{nullptr}; static const rcl_interfaces::msg::FloatingPointRange valid_pub_rate_range_; buoy_interfaces::msg::PCRecord pc_record_; double pub_rate_hz_{10.0}; @@ -68,6 +70,12 @@ const rcl_interfaces::msg::FloatingPointRange PowerControllerROS2::valid_pub_rat struct PowerControllerServices { + // PCPackRateCommand + rclcpp::Service::SharedPtr packrate_command_service_{ + nullptr}; + std::function, + std::shared_ptr)> packrate_command_handler_; + // PCWindCurrCommand -- Winding Current (Torque) rclcpp::Service::SharedPtr torque_command_service_{ nullptr}; @@ -206,7 +214,9 @@ struct PowerControllerPrivate std::unique_lock next(next_access_mutex_); std::unique_lock data(data_mutex_); next.unlock(); - ros_->pub_rate_ = std::make_unique(ros_->pub_rate_hz_); + ros_->pub_rate_ = std::make_unique( + ros_->pub_rate_hz_, + ros_->node_->get_clock()); data.unlock(); } @@ -296,6 +306,22 @@ struct PowerControllerPrivate void setupServices() { + // Pack Rate + services_->packrate_command_handler_ = + [this](const std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_DEBUG_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Power Control] PCPackRateCommand Received [" << request->rate_hz << " Hz]"); + rclcpp::Parameter parameter("publish_rate", static_cast(request->rate_hz)); + handle_publish_rate(parameter); + }; + services_->packrate_command_service_ = + ros_->node_->create_service( + "pc_pack_rate_command", + services_->packrate_command_handler_); + // PCWindCurrCommand services_->torque_command_watch_.SetClock(ros_->node_->get_clock()); services_->torque_command_handler_ = @@ -586,6 +612,10 @@ PowerController::PowerController() PowerController::~PowerController() { // Stop ros2 threads + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + this->dataPtr->stop_ = true; if (this->dataPtr->ros_->executor_) { this->dataPtr->ros_->executor_->cancel(); diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp index 43e8da91..9b26bb89 100644 --- a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp +++ b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp @@ -36,10 +36,13 @@ #include #include #include +#include +#include "buoy_utils/Rate.hpp" #include "PolytropicPneumaticSpring/SpringState.hpp" #include "SpringController.hpp" + using namespace std::chrono_literals; namespace buoy_gazebo @@ -52,13 +55,23 @@ struct SpringControllerROS2 bool use_sim_time_{true}; rclcpp::Publisher::SharedPtr sc_pub_{nullptr}; - std::unique_ptr pub_rate_{nullptr}; + std::unique_ptr pub_rate_{nullptr}; + static const rcl_interfaces::msg::FloatingPointRange valid_pub_rate_range_; buoy_interfaces::msg::SCRecord sc_record_; double pub_rate_hz_{10.0}; }; +const rcl_interfaces::msg::FloatingPointRange SpringControllerROS2::valid_pub_rate_range_ = + rcl_interfaces::msg::FloatingPointRange() + .set__from_value(10.0F) + .set__to_value(50.0F); struct SpringControllerServices { + rclcpp::Service::SharedPtr packrate_command_service_{ + nullptr}; + std::function, + std::shared_ptr)> packrate_command_handler_; + rclcpp::Service::SharedPtr valve_command_service_{nullptr}; std::function, std::shared_ptr)> valve_command_handler_; @@ -100,6 +113,40 @@ struct SpringControllerPrivate return spring_data_valid_ && load_cell_data_valid_; } + void handle_publish_rate(const rclcpp::Parameter & parameter) + { + double rate_hz = parameter.as_double(); + RCLCPP_INFO_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Spring Control] setting publish_rate: " << rate_hz); + + if (SpringControllerROS2::valid_pub_rate_range_.from_value > rate_hz || + rate_hz > SpringControllerROS2::valid_pub_rate_range_.to_value) + { + RCLCPP_WARN_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Spring Control] publish_rate out of bounds -- clipped between [" << + SpringControllerROS2::valid_pub_rate_range_.from_value << ", " << + SpringControllerROS2::valid_pub_rate_range_.to_value << "] Hz"); + } + + ros_->pub_rate_hz_ = std::min( + std::max( + rate_hz, + SpringControllerROS2::valid_pub_rate_range_.from_value), + SpringControllerROS2::valid_pub_rate_range_.to_value); + + // low prio data access + std::unique_lock low(low_prio_mutex_); + std::unique_lock next(next_access_mutex_); + std::unique_lock data(data_mutex_); + next.unlock(); + ros_->pub_rate_ = std::make_unique( + ros_->pub_rate_hz_, + ros_->node_->get_clock()); + data.unlock(); + } + void ros2_setup(const std::string & node_name, const std::string & ns) { if (!rclcpp::ok()) { @@ -129,26 +176,7 @@ struct SpringControllerPrivate parameter.get_name() == "publish_rate" && parameter.get_type() == rclcpp::PARAMETER_DOUBLE) { - double rate_hz = parameter.as_double(); - RCLCPP_INFO_STREAM( - ros_->node_->get_logger(), - "[ROS 2 Spring Control] setting publish_rate: " << rate_hz); - - if (rate_hz < 10.0 || rate_hz > 50.0) { - RCLCPP_WARN_STREAM( - ros_->node_->get_logger(), - "[ROS 2 Spring Control] publish_rate out of bounds -- clipped between [10,50]"); - } - - ros_->pub_rate_hz_ = std::min(std::max(rate_hz, 10.0), 50.0); - - // low prio data access - std::unique_lock low(low_prio_mutex_); - std::unique_lock next(next_access_mutex_); - std::unique_lock data(data_mutex_); - next.unlock(); - ros_->pub_rate_ = std::make_unique(ros_->pub_rate_hz_); - data.unlock(); + handle_publish_rate(parameter); } } return result; @@ -171,6 +199,22 @@ struct SpringControllerPrivate void setup_services() { + // Pack Rate + services_->packrate_command_handler_ = + [this](const std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_DEBUG_STREAM( + ros_->node_->get_logger(), + "[ROS 2 Spring Control] SCPackRateCommand Received [" << request->rate_hz << " Hz]"); + rclcpp::Parameter parameter("publish_rate", static_cast(request->rate_hz)); + handle_publish_rate(parameter); + }; + services_->packrate_command_service_ = + ros_->node_->create_service( + "sc_pack_rate_command", + services_->packrate_command_handler_); + services_->command_watch_.SetClock(ros_->node_->get_clock()); // ValveCommand @@ -437,6 +481,10 @@ SpringController::SpringController() SpringController::~SpringController() { // Stop ros2 threads + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + this->dataPtr->stop_ = true; if (this->dataPtr->ros_->executor_) { this->dataPtr->ros_->executor_->cancel(); diff --git a/buoy_gazebo/src/controllers/TrefoilController/TrefoilController.cpp b/buoy_gazebo/src/controllers/TrefoilController/TrefoilController.cpp index 8a7d6d80..e9c15f1e 100644 --- a/buoy_gazebo/src/controllers/TrefoilController/TrefoilController.cpp +++ b/buoy_gazebo/src/controllers/TrefoilController/TrefoilController.cpp @@ -32,8 +32,10 @@ #include #include +#include "buoy_utils/Rate.hpp" #include "TrefoilController.hpp" + struct buoy_gazebo::TrefoilControllerPrivate { rclcpp::Node::SharedPtr rosnode_{nullptr}; @@ -51,7 +53,7 @@ struct buoy_gazebo::TrefoilControllerPrivate std::function imu_cb_; std::function mag_cb_; double pub_rate_hz_{10.0}; - std::unique_ptr pub_rate_{nullptr}; + std::unique_ptr pub_rate_{nullptr}; buoy_interfaces::msg::TFRecord tf_record_; std::mutex data_mutex_, next_access_mutex_, low_prio_mutex_; @@ -109,6 +111,10 @@ TrefoilController::TrefoilController() TrefoilController::~TrefoilController() { // Stop ros2 threads + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + this->dataPtr->stop_ = true; this->dataPtr->executor_->cancel(); this->dataPtr->thread_executor_spin_.join(); @@ -186,8 +192,8 @@ void TrefoilController::Configure( this->dataPtr->imu_data_valid_ = true; data.unlock(); }; - if (!this->dataPtr->node_.Subscribe("/Trefoil_link/trefoil_imu", this->dataPtr->imu_cb_)) { - gzerr << "Error subscribing to topic [" << "/Trefoil_link/trefoil_imu" << "]" << std::endl; + if (!this->dataPtr->node_.Subscribe("/HeaveCone_link/tf_imu", this->dataPtr->imu_cb_)) { + gzerr << "Error subscribing to topic [" << "/HeaveCone_link/tf_imu" << "]" << std::endl; return; } @@ -203,8 +209,8 @@ void TrefoilController::Configure( this->dataPtr->mag_data_valid_ = true; data.unlock(); }; - if (!this->dataPtr->node_.Subscribe("/Trefoil_link/trefoil_mag", this->dataPtr->mag_cb_)) { - gzerr << "Error subscribing to topic [" << "/Trefoil_link/trefoil_mag" << "]" << std::endl; + if (!this->dataPtr->node_.Subscribe("/HeaveCone_link/tf_mag", this->dataPtr->mag_cb_)) { + gzerr << "Error subscribing to topic [" << "/HeaveCone_link/tf_mag" << "]" << std::endl; return; } @@ -223,7 +229,9 @@ void TrefoilController::Configure( this->dataPtr->pub_rate_hz_ = \ _sdf->Get("publish_rate", this->dataPtr->pub_rate_hz_).first; - this->dataPtr->pub_rate_ = std::make_unique(this->dataPtr->pub_rate_hz_); + this->dataPtr->pub_rate_ = std::make_unique( + this->dataPtr->pub_rate_hz_, + this->dataPtr->rosnode_->get_clock()); auto publish = [this]() { @@ -275,23 +283,24 @@ void TrefoilController::PostUpdate( // Get Trefoil link's pose auto model = gz::sim::Model(this->dataPtr->entity_); - auto link = model.LinkByName(_ecm, "Trefoil"); + auto link = model.LinkByName(_ecm, "HeaveCone"); auto pose = gz::sim::worldPose(link, _ecm); double depth = pose.Pos().Z(); + this->dataPtr->current_time_ = _info.simTime; + auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_); + // low prio data access std::unique_lock low(this->dataPtr->low_prio_mutex_); std::unique_lock next(this->dataPtr->next_access_mutex_); std::unique_lock data(this->dataPtr->data_mutex_); next.unlock(); - this->dataPtr->current_time_ = _info.simTime; - auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_); - this->dataPtr->tf_record_.header.stamp.sec = sec_nsec.first; this->dataPtr->tf_record_.header.stamp.nanosec = sec_nsec.second; + // Sea pressure: depth*rho*g, Pascal to psi - this->dataPtr->tf_record_.pressure = (depth * 1025 * 9.8) / 6894.75729; + this->dataPtr->tf_record_.pressure = (depth * 1025.0 * 9.8) / 6894.75729; // Constants this->dataPtr->tf_record_.power_timeouts = 60; diff --git a/buoy_gazebo/src/controllers/XBowAHRS/CMakeLists.txt b/buoy_gazebo/src/controllers/XBowAHRS/CMakeLists.txt index 0cdb373c..01a2f606 100644 --- a/buoy_gazebo/src/controllers/XBowAHRS/CMakeLists.txt +++ b/buoy_gazebo/src/controllers/XBowAHRS/CMakeLists.txt @@ -1,5 +1,7 @@ gz_add_plugin(XBowAHRS SOURCES XBowAHRS.cpp + INCLUDE_DIRS + ../.. ROS ) diff --git a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp index 3f4757b1..e6c17d7e 100644 --- a/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp +++ b/buoy_gazebo/src/controllers/XBowAHRS/XBowAHRS.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -30,34 +31,39 @@ #include #include +#include +#include "buoy_utils/Rate.hpp" #include "XBowAHRS.hpp" + struct buoy_gazebo::XBowAHRSPrivate { gz::sim::Entity entity_; gz::sim::Entity linkEntity_; rclcpp::Node::SharedPtr rosnode_{nullptr}; + bool use_sim_time_{true}; gz::transport::Node node_; std::function imu_cb_; + std::function gps_cb_; rclcpp::Publisher::SharedPtr xb_pub_{nullptr}; rclcpp::Publisher::SharedPtr imu_pub_{nullptr}; + rclcpp::Publisher::SharedPtr gps_pub_{nullptr}; double pub_rate_hz_{10.0}; - std::unique_ptr pub_rate_{nullptr}; + std::unique_ptr pub_rate_{nullptr}; std::chrono::steady_clock::duration current_time_; buoy_interfaces::msg::XBRecord xb_record_; std::mutex data_mutex_, next_access_mutex_, low_prio_mutex_; - std::atomic imu_data_valid_{false}, velocity_data_valid_{false}; + std::atomic imu_data_valid_{false}, velocity_data_valid_{false}, gps_data_valid_{false}; std::thread thread_executor_spin_, thread_publish_; rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; std::atomic stop_{false}, paused_{true}; 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(); - xb_record_.header.frame_id = "Buoy"; - xb_record_.imu.header = xb_record_.header; + xb_record_.imu.header.stamp.sec = _imu.header().stamp().sec(); + xb_record_.imu.header.stamp.nanosec = _imu.header().stamp().nsec(); + xb_record_.imu.header.frame_id = xb_record_.header.frame_id; xb_record_.imu.orientation.x = _imu.orientation().x(); xb_record_.imu.orientation.y = _imu.orientation().y(); xb_record_.imu.orientation.z = _imu.orientation().z(); @@ -70,9 +76,19 @@ struct buoy_gazebo::XBowAHRSPrivate xb_record_.imu.linear_acceleration.z = _imu.linear_acceleration().z(); } + void set_xb_record_gps(const gz::msgs::NavSat & _gps) + { + xb_record_.gps.header.stamp.nanosec = _gps.header().stamp().nsec(); + xb_record_.gps.header.frame_id = xb_record_.header.frame_id; + xb_record_.gps.header = xb_record_.header; + xb_record_.gps.latitude = _gps.latitude_deg(); + xb_record_.gps.longitude = _gps.longitude_deg(); + xb_record_.gps.altitude = _gps.altitude(); + } + bool data_valid() const { - return velocity_data_valid_ && imu_data_valid_; + return velocity_data_valid_ && imu_data_valid_ && gps_data_valid_; } }; @@ -94,6 +110,10 @@ XBowAHRS::XBowAHRS() XBowAHRS::~XBowAHRS() { // Stop ros2 threads + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + this->dataPtr->stop_ = true; this->dataPtr->executor_->cancel(); this->dataPtr->thread_executor_spin_.join(); @@ -136,6 +156,11 @@ void XBowAHRS::Configure( std::string node_name = _sdf->Get("node_name", "xbow_ahrs").first; this->dataPtr->rosnode_ = rclcpp::Node::make_shared(node_name, ns); + this->dataPtr->rosnode_->set_parameter( + rclcpp::Parameter( + "use_sim_time", + this->dataPtr->use_sim_time_)); + this->dataPtr->executor_ = std::make_shared(); this->dataPtr->executor_->add_node(this->dataPtr->rosnode_); this->dataPtr->stop_ = false; @@ -171,6 +196,23 @@ void XBowAHRS::Configure( return; } + // GPS Sensor + this->dataPtr->gps_cb_ = [this](const gz::msgs::NavSat & _gps) + { + // low prio data access + std::unique_lock low(this->dataPtr->low_prio_mutex_); + std::unique_lock next(this->dataPtr->next_access_mutex_); + std::unique_lock data(this->dataPtr->data_mutex_); + next.unlock(); + this->dataPtr->set_xb_record_gps(_gps); + this->dataPtr->gps_data_valid_ = true; + data.unlock(); + }; + if (!this->dataPtr->node_.Subscribe("/Buoy_link/xbow_gps", this->dataPtr->gps_cb_)) { + gzerr << "Error subscribing to topic [" << "/Buoy_link/xbow_gps" << "]" << std::endl; + return; + } + // Publisher std::string xb_topic = _sdf->Get("xb_topic", "ahrs_data").first; this->dataPtr->xb_pub_ = \ @@ -180,15 +222,22 @@ void XBowAHRS::Configure( this->dataPtr->imu_pub_ = \ this->dataPtr->rosnode_->create_publisher(imu_topic, 10); + std::string gps_topic = _sdf->Get("gps_topic", "xb_gps").first; + this->dataPtr->gps_pub_ = \ + this->dataPtr->rosnode_->create_publisher(gps_topic, 10); + this->dataPtr->pub_rate_hz_ = \ _sdf->Get("publish_rate", this->dataPtr->pub_rate_hz_).first; - this->dataPtr->pub_rate_ = std::make_unique(this->dataPtr->pub_rate_hz_); + this->dataPtr->pub_rate_ = std::make_unique( + this->dataPtr->pub_rate_hz_, + this->dataPtr->rosnode_->get_clock()); auto publish = [this]() { while (rclcpp::ok() && !this->dataPtr->stop_) { if (this->dataPtr->xb_pub_->get_subscription_count() <= 0 && \ - this->dataPtr->imu_pub_->get_subscription_count() <= 0) {continue;} + this->dataPtr->imu_pub_->get_subscription_count() <= 0 && \ + this->dataPtr->gps_pub_->get_subscription_count() <= 0) {continue;} // Only update and publish if not paused. if (this->dataPtr->paused_) {continue;} @@ -207,6 +256,9 @@ void XBowAHRS::Configure( if (this->dataPtr->imu_pub_->get_subscription_count() > 0) { this->dataPtr->imu_pub_->publish(xb_record.imu); } + if (this->dataPtr->gps_pub_->get_subscription_count() > 0) { + this->dataPtr->gps_pub_->publish(xb_record.gps); + } } else { data.unlock(); } @@ -227,11 +279,19 @@ void XBowAHRS::PostUpdate( gz::sim::Link link(link_entity); auto v_world = link.WorldLinearVelocity(_ecm); // assume x,y,z == ENU + this->dataPtr->current_time_ = _info.simTime; + auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_); + // low prio data access std::unique_lock low(this->dataPtr->low_prio_mutex_); std::unique_lock next(this->dataPtr->next_access_mutex_); std::unique_lock data(this->dataPtr->data_mutex_); next.unlock(); + + this->dataPtr->xb_record_.header.frame_id = "Buoy"; + this->dataPtr->xb_record_.header.stamp.sec = sec_nsec.first; + this->dataPtr->xb_record_.header.stamp.nanosec = sec_nsec.second; + if (v_world) { this->dataPtr->xb_record_.ned_velocity.x = v_world->Y(); this->dataPtr->xb_record_.ned_velocity.y = v_world->X(); diff --git a/buoy_gazebo/worlds/mbari_wec.sdf.em b/buoy_gazebo/worlds/mbari_wec.sdf.em index 479f39a9..04497fef 100644 --- a/buoy_gazebo/worlds/mbari_wec.sdf.em +++ b/buoy_gazebo/worlds/mbari_wec.sdf.em @@ -29,6 +29,7 @@ except NameError: @(lat_lon[0]) @(lat_lon[1]) 0.0 + 0.0