Skip to content

Commit

Permalink
simulation/gz_bridge: enable navsat plugin for accurate positioning o…
Browse files Browse the repository at this point in the history 
…f real life maps in Gazebo (PX4#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 <[email protected]>
  • Loading branch information
@fredmarkus @jwwaite
fredmarkus authored and jwwaite committed Feb 24, 2024
1 parent cde1474 commit 443c36e
Show file tree
Hide file tree
Showing 4 changed files with 65 additions and 66 deletions.
13 changes: 0 additions & 13 deletions ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
87 changes: 63 additions & 24 deletions src/modules/simulation/gz_bridge/GZBridge.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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();

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);
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;

global_position_groundtruth.alt = _param_sim_home_alt.get() - static_cast<float>(position(2));
global_position_groundtruth.timestamp = hrt_absolute_time();
_gpos_ground_truth_pub.publish(global_position_groundtruth);
} else {
local_position_groundtruth.ref_lat = static_cast<double>(NAN);
local_position_groundtruth.ref_lon = static_cast<double>(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);

pthread_mutex_unlock(&_node_mutex);
return;
}
Expand Down Expand Up @@ -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
Expand Down
8 changes: 2 additions & 6 deletions src/modules/simulation/gz_bridge/GZBridge.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
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);

/**
*
Expand Down Expand Up @@ -139,6 +140,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
pthread_mutex_t _node_mutex;

MapProjection _pos_ref{};
double _alt_ref{}; // starting altitude reference

matrix::Vector3d _position_prev{};
matrix::Vector3d _velocity_prev{};
Expand All @@ -153,10 +155,4 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
float _temperature{288.15}; // 15 degrees

gz::transport::Node _node;

DEFINE_PARAMETERS(
(ParamFloat<px4::params::SIM_GZ_HOME_LAT>) _param_sim_home_lat,
(ParamFloat<px4::params::SIM_GZ_HOME_LON>) _param_sim_home_lon,
(ParamFloat<px4::params::SIM_GZ_HOME_ALT>) _param_sim_home_alt
)
};
23 changes: 0 additions & 23 deletions src/modules/simulation/gz_bridge/parameters.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

0 comments on commit 443c36e

Please sign in to comment.