mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-22 14:24:21 +08:00
gz small improvements (#24761)
* gz: print version number in init, remove gst plugin spam, rename function * fix 0 timestamp issue by waiting for clock callback before subscribing to other topics. Refactor to cleanup topic subscriptions * format * change gzerr to gzwarn
This commit is contained in:
Jacob Dahl
GitHub
@@ -1,8 +1,6 @@
|
||||
#!/bin/sh
|
||||
# shellcheck disable=SC2154
|
||||
|
||||
echo "INFO [init] Gazebo simulator"
|
||||
|
||||
# Enforce minimum gz version as Harmonic (gz-sim8)
|
||||
MIN_GZ_VERSION="8.0.0"
|
||||
GZ_SIM_VERSION=$(gz sim --versions 2>/dev/null | head -n 1 | tr -d ' ')
|
||||
@@ -17,6 +15,8 @@ if [ "$(printf '%s\n' "$GZ_SIM_VERSION" "$MIN_GZ_VERSION" | sort -V | head -n1)"
|
||||
gz_command="gz"
|
||||
gz_sub_command="sim"
|
||||
|
||||
echo "INFO [init] Gazebo simulator $GZ_SIM_VERSION"
|
||||
|
||||
# Specify render engine if `GZ_SIM_RENDER_ENGINE` is set
|
||||
# (for example, if you want to use Ogre 1.x instead of Ogre 2.x):
|
||||
if [ -n "${PX4_GZ_SIM_RENDER_ENGINE}" ]; then
|
||||
@@ -24,7 +24,7 @@ if [ "$(printf '%s\n' "$GZ_SIM_VERSION" "$MIN_GZ_VERSION" | sort -V | head -n1)"
|
||||
gz_sub_command="${gz_sub_command} --render-engine ${PX4_GZ_SIM_RENDER_ENGINE}"
|
||||
fi
|
||||
else
|
||||
echo "ERROR [init] Gazebo gz sim version is too old ($GZ_SIM_VERSION). Minimum required version is $MIN_GZ_VERSION"
|
||||
echo "ERROR [init] Gazebo version too hold ($GZ_SIM_VERSION). Minimum required version is $MIN_GZ_VERSION"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
|
||||
@@ -61,112 +61,82 @@ GZBridge::~GZBridge()
|
||||
|
||||
int GZBridge::init()
|
||||
{
|
||||
// clock
|
||||
std::string clock_topic = "/world/" + _world_name + "/clock";
|
||||
|
||||
if (!_node.Subscribe(clock_topic, &GZBridge::clockCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", clock_topic.c_str());
|
||||
// REQUIRED:
|
||||
if (!subscribeClock(true)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// pose: /world/$WORLD/pose/info
|
||||
std::string world_pose_topic = "/world/" + _world_name + "/pose/info";
|
||||
// We must wait for clock before subscribing to other topics. This is because
|
||||
// if we publish a 0 timestamp it screws up the EKF.
|
||||
while (1) {
|
||||
px4_usleep(25000);
|
||||
|
||||
if (!_node.Subscribe(world_pose_topic, &GZBridge::poseInfoCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", world_pose_topic.c_str());
|
||||
if (_realtime_clock_set) {
|
||||
px4_usleep(25000);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!subscribePoseInfo(true)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// IMU: /world/$WORLD/model/$MODEL/link/base_link/sensor/imu_sensor/imu
|
||||
std::string imu_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/imu_sensor/imu";
|
||||
|
||||
if (!_node.Subscribe(imu_topic, &GZBridge::imuCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", imu_topic.c_str());
|
||||
if (!subscribeImu(true)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// mag: /world/$WORLD/model/$MODEL/link/base_link/sensor/magnetometer_sensor/magnetometer
|
||||
std::string mag_topic = "/world/" + _world_name + "/model/" + _model_name +
|
||||
"/link/base_link/sensor/magnetometer_sensor/magnetometer";
|
||||
|
||||
if (!_node.Subscribe(mag_topic, &GZBridge::magnetometerCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", mag_topic.c_str());
|
||||
if (!subscribeMag(true)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// odom: /world/$WORLD/model/$MODEL/link/base_link/odometry_with_covariance
|
||||
std::string odometry_topic = "/model/" + _model_name + "/odometry_with_covariance";
|
||||
|
||||
if (!_node.Subscribe(odometry_topic, &GZBridge::odometryCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", odometry_topic.c_str());
|
||||
// OPTIONAL:
|
||||
if (!subscribeNavsat(false)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// Laser Scan: optional
|
||||
std::string laser_scan_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/link/sensor/lidar_2d_v2/scan";
|
||||
|
||||
if (!_node.Subscribe(laser_scan_topic, &GZBridge::laserScanCallback, this)) {
|
||||
PX4_WARN("failed to subscribe to %s", laser_scan_topic.c_str());
|
||||
}
|
||||
|
||||
// Distance Sensor(AFBRS50): optional
|
||||
std::string lidar_sensor = "/world/" + _world_name + "/model/" + _model_name +
|
||||
"/link/lidar_sensor_link/sensor/lidar/scan";
|
||||
|
||||
if (!_node.Subscribe(lidar_sensor, &GZBridge::laserScantoLidarSensorCallback, this)) {
|
||||
PX4_WARN("failed to subscribe to %s", lidar_sensor.c_str());
|
||||
}
|
||||
|
||||
// Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed
|
||||
std::string airspeed_topic = "/world/" + _world_name + "/model/" + _model_name +
|
||||
"/link/airspeed_link/sensor/air_speed/air_speed";
|
||||
|
||||
if (!_node.Subscribe(airspeed_topic, &GZBridge::airspeedCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", airspeed_topic.c_str());
|
||||
if (!subscribeAirPressure(false)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// Air pressure: /world/$WORLD/model/$MODEL/link/base_link/sensor/air_pressure_sensor/air_pressure
|
||||
std::string air_pressure_topic = "/world/" + _world_name + "/model/" + _model_name +
|
||||
"/link/base_link/sensor/air_pressure_sensor/air_pressure";
|
||||
|
||||
if (!_node.Subscribe(air_pressure_topic, &GZBridge::barometerCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", air_pressure_topic.c_str());
|
||||
if (!subscribeDistanceSensor(false)) {
|
||||
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());
|
||||
if (!subscribeAirspeed(false)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
std::string flow_topic = "/world/" + _world_name + "/model/" + _model_name +
|
||||
"/link/flow_link/sensor/optical_flow/optical_flow";
|
||||
|
||||
if (!_node.Subscribe(flow_topic, &GZBridge::opticalFlowCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", flow_topic.c_str());
|
||||
if (!subscribeOpticalFlow(false)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
if (!subscribeOdometry(false)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
if (!subscribeLaserScan(false)) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// ESC mixing interface
|
||||
if (!_mixing_interface_esc.init(_model_name)) {
|
||||
PX4_ERR("failed to init ESC output");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// Servo mixing interface
|
||||
if (!_mixing_interface_servo.init(_model_name)) {
|
||||
PX4_ERR("failed to init servo output");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// Wheel mixing interface
|
||||
if (!_mixing_interface_wheel.init(_model_name)) {
|
||||
PX4_ERR("failed to init motor output");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// Gimbal mixing interface
|
||||
if (!_gimbal.init(_world_name, _model_name)) {
|
||||
PX4_ERR("failed to init gimbal");
|
||||
return PX4_ERROR;
|
||||
@@ -176,6 +146,174 @@ int GZBridge::init()
|
||||
return OK;
|
||||
}
|
||||
|
||||
void GZBridge::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
|
||||
_mixing_interface_esc.stop();
|
||||
_mixing_interface_servo.stop();
|
||||
_mixing_interface_wheel.stop();
|
||||
_gimbal.stop();
|
||||
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
updateParams();
|
||||
|
||||
_mixing_interface_esc.updateParams();
|
||||
_mixing_interface_servo.updateParams();
|
||||
_mixing_interface_wheel.updateParams();
|
||||
_gimbal.updateParams();
|
||||
}
|
||||
|
||||
ScheduleDelayed(10_ms);
|
||||
}
|
||||
|
||||
bool GZBridge::subscribeClock(bool required)
|
||||
{
|
||||
std::string clock_topic = "/world/" + _world_name + "/clock";
|
||||
|
||||
if (!_node.Subscribe(clock_topic, &GZBridge::clockCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", clock_topic.c_str());
|
||||
return required ? false : true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GZBridge::subscribePoseInfo(bool required)
|
||||
{
|
||||
std::string world_pose_topic = "/world/" + _world_name + "/pose/info";
|
||||
|
||||
if (!_node.Subscribe(world_pose_topic, &GZBridge::poseInfoCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", world_pose_topic.c_str());
|
||||
return required ? false : true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GZBridge::subscribeImu(bool required)
|
||||
{
|
||||
std::string imu_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/imu_sensor/imu";
|
||||
|
||||
if (!_node.Subscribe(imu_topic, &GZBridge::imuCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", imu_topic.c_str());
|
||||
return required ? false : true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GZBridge::subscribeMag(bool required)
|
||||
{
|
||||
std::string mag_topic = "/world/" + _world_name + "/model/" + _model_name +
|
||||
"/link/base_link/sensor/magnetometer_sensor/magnetometer";
|
||||
|
||||
if (!_node.Subscribe(mag_topic, &GZBridge::magnetometerCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", mag_topic.c_str());
|
||||
return required ? false : true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GZBridge::subscribeOdometry(bool required)
|
||||
{
|
||||
// odom: /world/$WORLD/model/$MODEL/link/base_link/odometry_with_covariance
|
||||
std::string odometry_topic = "/model/" + _model_name + "/odometry_with_covariance";
|
||||
|
||||
if (!_node.Subscribe(odometry_topic, &GZBridge::odometryCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", odometry_topic.c_str());
|
||||
return required ? false : true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GZBridge::subscribeLaserScan(bool required)
|
||||
{
|
||||
std::string laser_scan_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/link/sensor/lidar_2d_v2/scan";
|
||||
|
||||
if (!_node.Subscribe(laser_scan_topic, &GZBridge::laserScanCallback, this)) {
|
||||
PX4_WARN("failed to subscribe to %s", laser_scan_topic.c_str());
|
||||
return required ? false : true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GZBridge::subscribeDistanceSensor(bool required)
|
||||
{
|
||||
std::string lidar_sensor = "/world/" + _world_name + "/model/" + _model_name +
|
||||
"/link/lidar_sensor_link/sensor/lidar/scan";
|
||||
|
||||
if (!_node.Subscribe(lidar_sensor, &GZBridge::laserScantoLidarSensorCallback, this)) {
|
||||
PX4_WARN("failed to subscribe to %s", lidar_sensor.c_str());
|
||||
return required ? false : true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GZBridge::subscribeAirspeed(bool required)
|
||||
{
|
||||
std::string airspeed_topic = "/world/" + _world_name + "/model/" + _model_name +
|
||||
"/link/airspeed_link/sensor/air_speed/air_speed";
|
||||
|
||||
if (!_node.Subscribe(airspeed_topic, &GZBridge::airspeedCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", airspeed_topic.c_str());
|
||||
return required ? false : true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GZBridge::subscribeAirPressure(bool required)
|
||||
{
|
||||
std::string air_pressure_topic = "/world/" + _world_name + "/model/" + _model_name +
|
||||
"/link/base_link/sensor/air_pressure_sensor/air_pressure";
|
||||
|
||||
if (!_node.Subscribe(air_pressure_topic, &GZBridge::airPressureCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", air_pressure_topic.c_str());
|
||||
return required ? false : true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GZBridge::subscribeNavsat(bool required)
|
||||
{
|
||||
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 required ? false : true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GZBridge::subscribeOpticalFlow(bool required)
|
||||
{
|
||||
std::string flow_topic = "/world/" + _world_name + "/model/" + _model_name +
|
||||
"/link/flow_link/sensor/optical_flow/optical_flow";
|
||||
|
||||
if (!_node.Subscribe(flow_topic, &GZBridge::opticalFlowCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", flow_topic.c_str());
|
||||
return required ? false : true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void GZBridge::clockCallback(const gz::msgs::Clock &msg)
|
||||
{
|
||||
// NOTE: PX4-SITL time needs to stay in sync with gz, so this clock-sync will happen on every callback.
|
||||
@@ -254,7 +392,7 @@ void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &msg)
|
||||
_sensor_mag_pub.publish(report);
|
||||
}
|
||||
|
||||
void GZBridge::barometerCallback(const gz::msgs::FluidPressure &msg)
|
||||
void GZBridge::airPressureCallback(const gz::msgs::FluidPressure &msg)
|
||||
{
|
||||
const uint64_t timestamp = hrt_absolute_time();
|
||||
|
||||
@@ -273,7 +411,6 @@ void GZBridge::barometerCallback(const gz::msgs::FluidPressure &msg)
|
||||
_sensor_baro_pub.publish(report);
|
||||
}
|
||||
|
||||
|
||||
void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &msg)
|
||||
{
|
||||
const uint64_t timestamp = hrt_absolute_time();
|
||||
@@ -495,7 +632,7 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &msg)
|
||||
_visual_odometry_pub.publish(report);
|
||||
}
|
||||
|
||||
static float generate_wgn()
|
||||
float GZBridge::generate_wgn()
|
||||
{
|
||||
// generate white Gaussian noise sample with std=1
|
||||
|
||||
@@ -798,35 +935,6 @@ void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::m
|
||||
q_FRD_to_NED = q_ENU_to_NED * q_FLU_to_ENU * q_FLU_to_FRD.Inverse();
|
||||
}
|
||||
|
||||
void GZBridge::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
|
||||
_mixing_interface_esc.stop();
|
||||
_mixing_interface_servo.stop();
|
||||
_mixing_interface_wheel.stop();
|
||||
_gimbal.stop();
|
||||
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
updateParams();
|
||||
|
||||
_mixing_interface_esc.updateParams();
|
||||
_mixing_interface_servo.updateParams();
|
||||
_mixing_interface_wheel.updateParams();
|
||||
_gimbal.updateParams();
|
||||
}
|
||||
|
||||
ScheduleDelayed(10_ms);
|
||||
}
|
||||
|
||||
int GZBridge::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
std::string world_name;
|
||||
|
||||
@@ -108,9 +108,21 @@ private:
|
||||
|
||||
void Run() override;
|
||||
|
||||
bool subscribeClock(bool required);
|
||||
bool subscribePoseInfo(bool required);
|
||||
bool subscribeImu(bool required);
|
||||
bool subscribeMag(bool required);
|
||||
bool subscribeOdometry(bool required);
|
||||
bool subscribeLaserScan(bool required);
|
||||
bool subscribeDistanceSensor(bool required);
|
||||
bool subscribeAirspeed(bool required);
|
||||
bool subscribeAirPressure(bool required);
|
||||
bool subscribeNavsat(bool required);
|
||||
bool subscribeOpticalFlow(bool required);
|
||||
|
||||
void clockCallback(const gz::msgs::Clock &msg);
|
||||
void airspeedCallback(const gz::msgs::AirSpeed &msg);
|
||||
void barometerCallback(const gz::msgs::FluidPressure &msg);
|
||||
void airPressureCallback(const gz::msgs::FluidPressure &msg);
|
||||
void imuCallback(const gz::msgs::IMU &msg);
|
||||
void poseInfoCallback(const gz::msgs::Pose_V &msg);
|
||||
void odometryCallback(const gz::msgs::OdometryWithCovariance &msg);
|
||||
@@ -122,6 +134,8 @@ private:
|
||||
|
||||
static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU);
|
||||
|
||||
static float generate_wgn();
|
||||
|
||||
void addGpsNoise(double &latitude, double &longitude, double &altitude,
|
||||
float &vel_north, float &vel_east, float &vel_down);
|
||||
|
||||
|
||||
@@ -175,10 +175,6 @@ void GstCameraSystem::findCameraTopic()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!_initialized) {
|
||||
gzdbg << "No camera topics found yet, will check again next update" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
@@ -282,7 +278,7 @@ void GstCameraSystem::gstThreadFunc()
|
||||
NULL);
|
||||
|
||||
} else {
|
||||
gzerr << "NVIDIA H.264 encoder not available, falling back to software encoder" << std::endl;
|
||||
gzwarn << "NVIDIA H.264 encoder not available, falling back to software encoder" << std::endl;
|
||||
encoder = gst_element_factory_make("x264enc", nullptr);
|
||||
g_object_set(G_OBJECT(encoder),
|
||||
"bitrate", 4000,
|
||||
|
||||
Reference in New Issue
Block a user