mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 03:53:34 +08:00
gz: Refactor GZBridge and px4-rc.simulator (#24421)
* disable SENS_EN_GPSSIM for all gz airframes * add GPS + noise to GZBridge * remove mutex from gz callbacks. Callbacks run synchronously after sim update step and run() loop does not share resources. * remove hrt check in callbacks * format * remove param set-default for already default params * update submodule * remove unnecessary comments * overhaul of the GZBridge and px4-rc.simulator script * remove arg * shellcheck disable * add bus/address * start gz_bridge before adjusting sim speed or camera follow
This commit is contained in:
Jacob Dahl
File diff suppressed because it is too large
Load Diff
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -44,24 +44,26 @@
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
#include <uORB/topics/sensor_accel.h>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/obstacle_distance.h>
|
||||
#include <uORB/topics/wheel_encoders.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/vehicle_odometry.h>
|
||||
#include <uORB/topics/wheel_encoders.h>
|
||||
#include <uORB/topics/obstacle_distance.h>
|
||||
|
||||
#include <gz/math.hh>
|
||||
#include <gz/msgs.hh>
|
||||
@@ -81,7 +83,7 @@ using namespace time_literals;
|
||||
class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
GZBridge(const char *world, const char *name, const char *model, const char *pose_str);
|
||||
GZBridge(const std::string &world, const std::string &model_name);
|
||||
~GZBridge() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
@@ -98,78 +100,26 @@ public:
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
|
||||
uint64_t world_time_us() const { return _world_time_us.load(); }
|
||||
|
||||
private:
|
||||
|
||||
void Run() override;
|
||||
|
||||
bool updateClock(const uint64_t tv_sec, const uint64_t tv_nsec);
|
||||
void clockCallback(const gz::msgs::Clock &msg);
|
||||
void airspeedCallback(const gz::msgs::AirSpeed &msg);
|
||||
void barometerCallback(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);
|
||||
void navSatCallback(const gz::msgs::NavSat &msg);
|
||||
void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg);
|
||||
void laserScanCallback(const gz::msgs::LaserScan &msg);
|
||||
|
||||
void clockCallback(const gz::msgs::Clock &clock);
|
||||
|
||||
void airspeedCallback(const gz::msgs::AirSpeed &air_speed);
|
||||
void barometerCallback(const gz::msgs::FluidPressure &air_pressure);
|
||||
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);
|
||||
void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan);
|
||||
void laserScanCallback(const gz::msgs::LaserScan &scan);
|
||||
|
||||
/**
|
||||
* @brief Call Entityfactory service
|
||||
*
|
||||
* @param req
|
||||
* @return true
|
||||
* @return false
|
||||
*/
|
||||
bool callEntityFactoryService(const std::string &service, const gz::msgs::EntityFactory &req);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Call scene info service
|
||||
*
|
||||
* @param service
|
||||
* @param req
|
||||
* @return true
|
||||
* @return false
|
||||
*/
|
||||
bool callSceneInfoMsgService(const std::string &service);
|
||||
|
||||
/**
|
||||
* @brief Call String service
|
||||
*
|
||||
* @param service
|
||||
* @param req
|
||||
* @return true
|
||||
* @return false
|
||||
*/
|
||||
bool callStringMsgService(const std::string &service, const gz::msgs::StringMsg &req);
|
||||
|
||||
/**
|
||||
* @brief Call Vector3d Service
|
||||
*
|
||||
* @param service
|
||||
* @param req
|
||||
* @return true
|
||||
* @return false
|
||||
*/
|
||||
bool callVector3dService(const std::string &service, const gz::msgs::Vector3d &req);
|
||||
/**
|
||||
*
|
||||
* Convert a quaterion from FLU_to_ENU frames (ROS convention)
|
||||
* to FRD_to_NED frames (PX4 convention)
|
||||
*
|
||||
* @param q_FRD_to_NED output quaterion in PX4 conventions
|
||||
* @param q_FLU_to_ENU input quaterion in ROS conventions
|
||||
*/
|
||||
static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU);
|
||||
|
||||
bool callPhysicsMsgService(const std::string &service, const gz::msgs::Physics &req);
|
||||
void addRealisticGpsNoise(double &latitude, double &longitude, double &altitude,
|
||||
float &vel_north, float &vel_east, float &vel_down);
|
||||
|
||||
// Subscriptions
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Publication<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
|
||||
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
|
||||
@@ -178,23 +128,21 @@ private:
|
||||
uORB::Publication<vehicle_attitude_s> _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
|
||||
uORB::Publication<vehicle_global_position_s> _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
|
||||
uORB::Publication<vehicle_local_position_s> _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
|
||||
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
|
||||
|
||||
uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
|
||||
uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
|
||||
uORB::PublicationMulti<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
|
||||
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
|
||||
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
|
||||
uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
|
||||
uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
|
||||
uORB::PublicationMulti<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
|
||||
|
||||
GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex};
|
||||
GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex};
|
||||
GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex};
|
||||
GZGimbal _gimbal{_node, _node_mutex};
|
||||
GZMixingInterfaceESC _mixing_interface_esc{_node};
|
||||
GZMixingInterfaceServo _mixing_interface_servo{_node};
|
||||
GZMixingInterfaceWheel _mixing_interface_wheel{_node};
|
||||
|
||||
px4::atomic<uint64_t> _world_time_us{0};
|
||||
|
||||
pthread_mutex_t _node_mutex;
|
||||
GZGimbal _gimbal{_node};
|
||||
|
||||
MapProjection _pos_ref{};
|
||||
double _alt_ref{}; // starting altitude reference
|
||||
double _alt_ref{};
|
||||
|
||||
matrix::Vector3d _position_prev{};
|
||||
matrix::Vector3d _velocity_prev{};
|
||||
@@ -203,10 +151,26 @@ private:
|
||||
|
||||
const std::string _world_name;
|
||||
const std::string _model_name;
|
||||
const std::string _model_sim;
|
||||
const std::string _model_pose;
|
||||
|
||||
float _temperature{288.15}; // 15 degrees
|
||||
|
||||
gz::transport::Node _node;
|
||||
|
||||
// GPS noise model
|
||||
float _gps_pos_noise_n = 0.0f;
|
||||
float _gps_pos_noise_e = 0.0f;
|
||||
float _gps_pos_noise_d = 0.0f;
|
||||
float _gps_vel_noise_n = 0.0f;
|
||||
float _gps_vel_noise_e = 0.0f;
|
||||
float _gps_vel_noise_d = 0.0f;
|
||||
const float _pos_noise_amplitude = 0.8f; // Position noise amplitude [m]
|
||||
const float _pos_random_walk = 0.01f; // Position random walk coefficient
|
||||
const float _pos_markov_time = 0.95f; // Position Markov process coefficient
|
||||
const float _vel_noise_amplitude = 0.05f; // Velocity noise amplitude [m/s]
|
||||
const float _vel_noise_density = 0.2f; // Velocity noise process density
|
||||
const float _vel_markov_time = 0.85f; // Velocity Markov process coefficient
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::SIM_GPS_USED>) _sim_gps_used
|
||||
)
|
||||
};
|
||||
|
||||
@@ -49,6 +49,8 @@ bool GZGimbal::init(const std::string &world_name, const std::string &model_name
|
||||
return false;
|
||||
}
|
||||
|
||||
pthread_mutex_init(&_node_mutex, nullptr);
|
||||
|
||||
updateParameters();
|
||||
|
||||
ScheduleOnInterval(200_ms); // @5Hz
|
||||
|
||||
@@ -27,18 +27,17 @@ using namespace time_literals;
|
||||
class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams
|
||||
{
|
||||
public:
|
||||
GZGimbal(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
|
||||
GZGimbal(gz::transport::Node &node) :
|
||||
px4::ScheduledWorkItem(MODULE_NAME "-gimbal", px4::wq_configurations::rate_ctrl),
|
||||
ModuleParams(nullptr),
|
||||
_node(node),
|
||||
_node_mutex(node_mutex)
|
||||
_node(node)
|
||||
{}
|
||||
|
||||
private:
|
||||
friend class GZBridge;
|
||||
|
||||
gz::transport::Node &_node;
|
||||
pthread_mutex_t &_node_mutex;
|
||||
pthread_mutex_t _node_mutex;
|
||||
|
||||
uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)};
|
||||
uORB::Subscription _gimbal_controls_sub{ORB_ID(gimbal_controls)};
|
||||
|
||||
@@ -55,6 +55,8 @@ bool GZMixingInterfaceESC::init(const std::string &model_name)
|
||||
|
||||
_esc_status_pub.advertise();
|
||||
|
||||
pthread_mutex_init(&_node_mutex, nullptr);
|
||||
|
||||
ScheduleNow();
|
||||
|
||||
return true;
|
||||
|
||||
@@ -50,10 +50,9 @@ class GZMixingInterfaceESC : public OutputModuleInterface
|
||||
public:
|
||||
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
|
||||
|
||||
GZMixingInterfaceESC(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
|
||||
GZMixingInterfaceESC(gz::transport::Node &node) :
|
||||
OutputModuleInterface(MODULE_NAME "-actuators-esc", px4::wq_configurations::rate_ctrl),
|
||||
_node(node),
|
||||
_node_mutex(node_mutex)
|
||||
_node(node)
|
||||
{}
|
||||
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
@@ -77,7 +76,7 @@ private:
|
||||
void motorSpeedCallback(const gz::msgs::Actuators &actuators);
|
||||
|
||||
gz::transport::Node &_node;
|
||||
pthread_mutex_t &_node_mutex;
|
||||
pthread_mutex_t _node_mutex;
|
||||
|
||||
MixingOutput _mixing_output{"SIM_GZ_EC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
|
||||
|
||||
@@ -121,6 +121,8 @@ bool GZMixingInterfaceServo::init(const std::string &model_name)
|
||||
_angular_range_rad.push_back(max_val - min_val);
|
||||
}
|
||||
|
||||
pthread_mutex_init(&_node_mutex, nullptr);
|
||||
|
||||
ScheduleNow();
|
||||
|
||||
return true;
|
||||
|
||||
@@ -44,10 +44,9 @@
|
||||
class GZMixingInterfaceServo : public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
GZMixingInterfaceServo(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
|
||||
GZMixingInterfaceServo(gz::transport::Node &node) :
|
||||
OutputModuleInterface(MODULE_NAME "-actuators-servo", px4::wq_configurations::rate_ctrl),
|
||||
_node(node),
|
||||
_node_mutex(node_mutex)
|
||||
_node(node)
|
||||
{}
|
||||
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
@@ -83,7 +82,7 @@ private:
|
||||
float get_servo_angle_min(const size_t index);
|
||||
|
||||
gz::transport::Node &_node;
|
||||
pthread_mutex_t &_node_mutex;
|
||||
pthread_mutex_t _node_mutex;
|
||||
|
||||
MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
|
||||
|
||||
@@ -51,6 +51,8 @@ bool GZMixingInterfaceWheel::init(const std::string &model_name)
|
||||
return false;
|
||||
}
|
||||
|
||||
pthread_mutex_init(&_node_mutex, nullptr);
|
||||
|
||||
_wheel_encoders_pub.advertise();
|
||||
|
||||
ScheduleNow();
|
||||
|
||||
@@ -51,10 +51,9 @@ class GZMixingInterfaceWheel : public OutputModuleInterface
|
||||
public:
|
||||
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
|
||||
|
||||
GZMixingInterfaceWheel(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
|
||||
GZMixingInterfaceWheel(gz::transport::Node &node) :
|
||||
OutputModuleInterface(MODULE_NAME "-actuators-wheel", px4::wq_configurations::rate_ctrl),
|
||||
_node(node),
|
||||
_node_mutex(node_mutex)
|
||||
_node(node)
|
||||
{}
|
||||
|
||||
bool updateOutputs(bool stop_wheels, uint16_t outputs[MAX_ACTUATORS],
|
||||
@@ -78,7 +77,7 @@ private:
|
||||
void wheelSpeedCallback(const gz::msgs::Actuators &actuators);
|
||||
|
||||
gz::transport::Node &_node;
|
||||
pthread_mutex_t &_node_mutex;
|
||||
pthread_mutex_t _node_mutex;
|
||||
|
||||
MixingOutput _mixing_output{"SIM_GZ_WH", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user