From bb53781b8f94fdc3b3d94c23dda429f0932c28fc Mon Sep 17 00:00:00 2001 From: Frederik Markus <80588263+frede791@users.noreply.github.com> Date: Tue, 13 Feb 2024 17:09:35 +0100 Subject: [PATCH] simulation/gz_bridge: enable navsat plugin for accurate positioning of real life maps in Gazebo (#22638) * publish the global groundtruth from the navsat callback and rearrange the local groundtruth as the altitude reference now has a dependency on the global groundtruth being initialized --------- Signed-off-by: frederik --- .../init.d-posix/px4-rc.simulator | 13 --- src/modules/simulation/gz_bridge/GZBridge.cpp | 89 +++++++++++++------ src/modules/simulation/gz_bridge/GZBridge.hpp | 8 +- src/modules/simulation/gz_bridge/parameters.c | 23 ----- 4 files changed, 66 insertions(+), 67 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index ec619bbcda..0a0a21cbf5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -41,19 +41,6 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then echo "INFO [init] Gazebo simulator" - # set local coordinate frame reference - if [ -n "${PX4_HOME_LAT}" ]; then - param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT} - fi - - if [ -n "${PX4_HOME_LON}" ]; then - param set SIM_GZ_HOME_LON ${PX4_HOME_LON} - fi - - if [ -n "${PX4_HOME_ALT}" ]; then - param set SIM_GZ_HOME_ALT ${PX4_HOME_ALT} - fi - # Only start up Gazebo if PX4_GZ_STANDALONE is not set. if [ -z "${PX4_GZ_STANDALONE}" ]; then diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index eda32df25c..2821213272 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -216,6 +216,15 @@ int GZBridge::init() return PX4_ERROR; } + // GPS: /world/$WORLD/model/$MODEL/link/base_link/sensor/navsat_sensor/navsat + std::string nav_sat_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/base_link/sensor/navsat_sensor/navsat"; + + if (!_node.Subscribe(nav_sat_topic, &GZBridge::navSatCallback, this)) { + PX4_ERR("failed to subscribe to %s", nav_sat_topic.c_str()); + return PX4_ERROR; + } + if (!_mixing_interface_esc.init(_model_name)) { PX4_ERR("failed to init ESC output"); return PX4_ERROR; @@ -565,10 +574,6 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) vehicle_angular_velocity_groundtruth.timestamp = hrt_absolute_time(); _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()); - } - vehicle_local_position_s local_position_groundtruth{}; #if defined(ENABLE_LOCKSTEP_SCHEDULER) local_position_groundtruth.timestamp_sample = time_us; @@ -595,31 +600,27 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) local_position_groundtruth.heading = euler.psi(); - 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_timestamp = _pos_ref.getProjectionReferenceTimestamp(); + if (_pos_ref.isInitialized()) { + + 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 = _alt_ref; + local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp(); + local_position_groundtruth.xy_global = true; + local_position_groundtruth.z_global = true; + + } else { + local_position_groundtruth.ref_lat = static_cast(NAN); + local_position_groundtruth.ref_lon = static_cast(NAN); + local_position_groundtruth.ref_alt = NAN; + local_position_groundtruth.ref_timestamp = 0; + local_position_groundtruth.xy_global = false; + local_position_groundtruth.z_global = false; + } local_position_groundtruth.timestamp = hrt_absolute_time(); _lpos_ground_truth_pub.publish(local_position_groundtruth); - if (_pos_ref.isInitialized()) { - // publish position groundtruth - vehicle_global_position_s global_position_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - global_position_groundtruth.timestamp_sample = time_us; -#else - global_position_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif - - _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.timestamp = hrt_absolute_time(); - _gpos_ground_truth_pub.publish(global_position_groundtruth); - } - pthread_mutex_unlock(&_node_mutex); return; } @@ -704,6 +705,44 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry pthread_mutex_unlock(&_node_mutex); } +void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat) +{ + if (hrt_absolute_time() == 0) { + return; + } + + pthread_mutex_lock(&_node_mutex); + + const uint64_t time_us = (nav_sat.header().stamp().sec() * 1000000) + (nav_sat.header().stamp().nsec() / 1000); + + if (time_us > _world_time_us.load()) { + updateClock(nav_sat.header().stamp().sec(), nav_sat.header().stamp().nsec()); + } + + _timestamp_prev = time_us; + + // initialize gps position + if (!_pos_ref.isInitialized()) { + _pos_ref.initReference(nav_sat.latitude_deg(), nav_sat.longitude_deg(), hrt_absolute_time()); + _alt_ref = nav_sat.altitude(); + + } else { + // publish GPS groundtruth + vehicle_global_position_s global_position_groundtruth{}; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + global_position_groundtruth.timestamp_sample = time_us; +#else + global_position_groundtruth.timestamp_sample = hrt_absolute_time(); +#endif + global_position_groundtruth.lat = nav_sat.latitude_deg(); + global_position_groundtruth.lon = nav_sat.longitude_deg(); + global_position_groundtruth.alt = nav_sat.altitude(); + _gpos_ground_truth_pub.publish(global_position_groundtruth); + } + + pthread_mutex_unlock(&_node_mutex); +} + void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU) { // FLU (ROS) to FRD (PX4) static rotation diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 8bf7ab06c0..6d2e2670a1 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -105,6 +105,7 @@ private: void imuCallback(const gz::msgs::IMU &imu); void poseInfoCallback(const gz::msgs::Pose_V &pose); void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry); + void navSatCallback(const gz::msgs::NavSat &nav_sat); /** * @@ -139,6 +140,7 @@ private: pthread_mutex_t _node_mutex; MapProjection _pos_ref{}; + double _alt_ref{}; // starting altitude reference matrix::Vector3d _position_prev{}; matrix::Vector3d _velocity_prev{}; @@ -153,10 +155,4 @@ private: float _temperature{288.15}; // 15 degrees gz::transport::Node _node; - - DEFINE_PARAMETERS( - (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 513c67436c..8dd6022ff6 100644 --- a/src/modules/simulation/gz_bridge/parameters.c +++ b/src/modules/simulation/gz_bridge/parameters.c @@ -40,26 +40,3 @@ */ PARAM_DEFINE_INT32(SIM_GZ_EN, 0); -/** - * simulator origin latitude - * - * @unit deg - * @group Simulator - */ -PARAM_DEFINE_FLOAT(SIM_GZ_HOME_LAT, 47.397742f); - -/** - * simulator origin longitude - * - * @unit deg - * @group Simulator - */ -PARAM_DEFINE_FLOAT(SIM_GZ_HOME_LON, 8.545594); - -/** - * simulator origin altitude - * - * @unit m - * @group Simulator - */ -PARAM_DEFINE_FLOAT(SIM_GZ_HOME_ALT, 488.0);