From 4b3354e217a9c9d98585f17095df762ba4e0c144 Mon Sep 17 00:00:00 2001 From: haitomatic Date: Wed, 24 Apr 2024 14:54:06 +0000 Subject: [PATCH] gz sitl: add option to use GPS home position from gazebo --- src/modules/simulation/gz_bridge/GZBridge.cpp | 35 +++++++++++++++++-- src/modules/simulation/gz_bridge/GZBridge.hpp | 8 +++++ src/modules/simulation/gz_bridge/parameters.c | 9 +++++ 3 files changed, 49 insertions(+), 3 deletions(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 6bfbee43dcdc..409257f3fe2f 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -44,6 +44,7 @@ #include #include +#include GZBridge::GZBridge(const char *world, const char *name, const char *model, const char *pose_str) : ModuleParams(nullptr), @@ -545,7 +546,16 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) _angular_velocity_ground_truth_pub.publish(vehicle_angular_velocity_groundtruth); if (!_pos_ref.isInitialized()) { - _pos_ref.initReference((double)_param_sim_home_lat.get(), (double)_param_sim_home_lon.get(), hrt_absolute_time()); + if (!_param_use_sim_home.get() || !std::isnan(_gz_home_lat) && !std::isnan(_gz_home_lon)) { + _pos_ref.initReference(_param_use_sim_home.get() ? _gz_home_lat : (double)_param_sim_home_lat.get(), + _param_use_sim_home.get() ? _gz_home_lon : (double)_param_sim_home_lon.get(), + hrt_absolute_time()); + + } else { + // no reference set yet + pthread_mutex_unlock(&_node_mutex); + return; + } } vehicle_local_position_s local_position_groundtruth{}; @@ -576,7 +586,8 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) local_position_groundtruth.ref_lat = _pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees local_position_groundtruth.ref_lon = _pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees - local_position_groundtruth.ref_alt = _param_sim_home_alt.get(); + local_position_groundtruth.ref_alt = _param_use_sim_home.get() ? static_cast(_gz_home_alt) : + _param_sim_home_alt.get(); local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp(); local_position_groundtruth.timestamp = hrt_absolute_time(); @@ -594,7 +605,9 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) _pos_ref.reproject(local_position_groundtruth.x, local_position_groundtruth.y, global_position_groundtruth.lat, global_position_groundtruth.lon); - global_position_groundtruth.alt = _param_sim_home_alt.get() - static_cast(position(2)); + global_position_groundtruth.alt = _param_use_sim_home.get() ? static_cast(_gz_home_alt) : + _param_sim_home_alt.get() + - static_cast(position(2)); global_position_groundtruth.timestamp = hrt_absolute_time(); _gpos_ground_truth_pub.publish(global_position_groundtruth); } @@ -607,6 +620,22 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) pthread_mutex_unlock(&_node_mutex); } +void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat) +{ + if (hrt_absolute_time() == 0 || _pos_ref.isInitialized()) { + return; + } + + pthread_mutex_lock(&_node_mutex); + + // initialize gz gps position + _gz_home_lat = nav_sat.latitude_deg(); + _gz_home_lon = nav_sat.longitude_deg(); + _gz_home_alt = nav_sat.altitude(); + + pthread_mutex_unlock(&_node_mutex); +} + void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry) { if (hrt_absolute_time() == 0) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 0ed1ca3e4f9c..19f64dcaf88e 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -109,6 +109,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void barometerCallback(const gz::msgs::FluidPressure &air_pressure); void imuCallback(const gz::msgs::IMU &imu); void poseInfoCallback(const gz::msgs::Pose_V &pose); + void navSatCallback(const gz::msgs::NavSat &nav_sat); void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry); /** @@ -163,11 +164,18 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S float _temperature{288.15}; // 15 degrees + double _gz_home_lat{std::numeric_limits::quiet_NaN()}; + + double _gz_home_lon{std::numeric_limits::quiet_NaN()}; + + double _gz_home_alt{std::numeric_limits::quiet_NaN()}; + gz::transport::Node::Publisher _cmd_vel_pub; gz::transport::Node _node; DEFINE_PARAMETERS( + (ParamBool) _param_use_sim_home, (ParamFloat) _param_sim_home_lat, (ParamFloat) _param_sim_home_lon, (ParamFloat) _param_sim_home_alt diff --git a/src/modules/simulation/gz_bridge/parameters.c b/src/modules/simulation/gz_bridge/parameters.c index 8f4a81eec170..24fe796de300 100644 --- a/src/modules/simulation/gz_bridge/parameters.c +++ b/src/modules/simulation/gz_bridge/parameters.c @@ -49,6 +49,15 @@ PARAM_DEFINE_INT32(SIM_GZ_EN, 0); */ PARAM_DEFINE_INT32(SIM_GZ_RUN_GZSIM, 1); +/** + * use simulator origin (home) position + * + * @boolean + * @reboot_required true + * @group Simulator + */ +PARAM_DEFINE_FLOAT(SIM_GZ_USE_SIM_HOME, 0); + /** * simulator origin latitude *