dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <frederik.anilmarkus@gmail.com>
This commit is contained in:
Frederik Markus 2024-02-13 17:09:35 +01:00 committed by GitHub
parent c9ad60e3cc
commit bb53781b8f
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
4 changed files with 66 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
View File

@ -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

83
src/modules/simulation/gz_bridge/GZBridge.cpp
View File

@ -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();
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 = _param_sim_home_alt.get();
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<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);
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<float>(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

8
src/modules/simulation/gz_bridge/GZBridge.hpp
View File

@ -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<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
src/modules/simulation/gz_bridge/parameters.c
View File

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