fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 10:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

simulation/gz_bridge: rc_cessna plane model working, Gazebo Garden updates, and prepare for proper airspeed (#20989)

Browse Source 
Co-authored-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
This commit is contained in:
Benjamin Perseghetti
2023-01-24 19:01:45 -05:00
committed by GitHub
parent fbd2e111d0
commit 684b4a4b8a
26 changed files with 348 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/gz_bridge/CMakeLists.txt
+1 -3
View File
@@ -35,9 +35,7 @@
find_package(gz-transport
#REQUIRED COMPONENTS core
NAMES
ignition-transport8
ignition-transport10
ignition-transport11
#ignition-transport11 # IGN (Fortress and earlier) no longer supported
gz-transport12
#QUIET
)
src/modules/simulation/gz_bridge/GZBridge.cpp
+68 -30
View File
@@ -35,6 +35,7 @@
#include <uORB/Subscription.hpp>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <px4_platform_common/getopt.h>
@@ -70,8 +71,7 @@ int GZBridge::init()
if (!_model_sim.empty()) {
// service call to create model
// ign service -s /world/${PX4_GZ_WORLD}/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 1000 --req "sdf_filename: \"${PX4_GZ_MODEL}/model.sdf\""
ignition::msgs::EntityFactory req{};
gz::msgs::EntityFactory req{};
req.set_sdf_filename(_model_sim + "/model.sdf");
req.set_name(_model_name); // New name for the entity, overrides the name on the SDF.
@@ -95,16 +95,16 @@ int GZBridge::init()
model_pose_v.push_back(0.0);
}
ignition::msgs::Pose *p = req.mutable_pose();
ignition::msgs::Vector3d *position = p->mutable_position();
gz::msgs::Pose *p = req.mutable_pose();
gz::msgs::Vector3d *position = p->mutable_position();
position->set_x(model_pose_v[0]);
position->set_y(model_pose_v[1]);
position->set_z(model_pose_v[2]);
ignition::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]);
gz::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]);
q.Normalize();
ignition::msgs::Quaternion *orientation = p->mutable_orientation();
gz::msgs::Quaternion *orientation = p->mutable_orientation();
orientation->set_x(q.X());
orientation->set_y(q.Y());
orientation->set_z(q.Z());
@@ -112,7 +112,7 @@ int GZBridge::init()
}
//world/$WORLD/create service.
ignition::msgs::Boolean rep;
gz::msgs::Boolean rep;
bool result;
std::string create_service = "/world/" + _world_name + "/create";
@@ -152,6 +152,18 @@ int GZBridge::init()
return PX4_ERROR;
}
#if 0
// Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed
std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name +
"/link/airspeed_link/sensor/air_speed/air_speed";
if (!_node.Subscribe(airpressure_topic, &GZBridge::airpressureCallback, this)) {
PX4_ERR("failed to subscribe to %s", airpressure_topic.c_str());
return PX4_ERROR;
}
#endif
if (!_mixing_interface_esc.init(_model_name)) {
PX4_ERR("failed to init ESC output");
return PX4_ERROR;
@@ -304,7 +316,7 @@ bool GZBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec)
return false;
}
void GZBridge::clockCallback(const ignition::msgs::Clock &clock)
void GZBridge::clockCallback(const gz::msgs::Clock &clock)
{
pthread_mutex_lock(&_node_mutex);
@@ -317,7 +329,33 @@ void GZBridge::clockCallback(const ignition::msgs::Clock &clock)
pthread_mutex_unlock(&_node_mutex);
}
void GZBridge::imuCallback(const ignition::msgs::IMU &imu)
#if 0
void GZBridge::airpressureCallback(const gz::msgs::FluidPressure &air_pressure)
{
if (hrt_absolute_time() == 0) {
return;
}
pthread_mutex_lock(&_mutex);
const uint64_t time_us = (air_pressure.header().stamp().sec() * 1000000)
+ (air_pressure.header().stamp().nsec() / 1000);
double air_pressure_value = air_pressure.pressure();
differential_pressure_s report{};
report.timestamp_sample = time_us;
report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
report.differential_pressure_pa = static_cast<float>(air_pressure_value); // hPa to Pa;
report.temperature = static_cast<float>(air_pressure.variance()) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // K to C
report.timestamp = hrt_absolute_time();;
_differential_pressure_pub.publish(report);
pthread_mutex_unlock(&_node_mutex);
}
#endif
void GZBridge::imuCallback(const gz::msgs::IMU &imu)
{
if (hrt_absolute_time() == 0) {
return;
@@ -332,12 +370,12 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu)
}
// FLU -> FRD
static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0);
ignition::math::Vector3d accel_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d(
imu.linear_acceleration().x(),
imu.linear_acceleration().y(),
imu.linear_acceleration().z()));
gz::math::Vector3d accel_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d(
imu.linear_acceleration().x(),
imu.linear_acceleration().y(),
imu.linear_acceleration().z()));
// publish accel
sensor_accel_s sensor_accel{};
@@ -357,10 +395,10 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu)
_sensor_accel_pub.publish(sensor_accel);
ignition::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d(
imu.angular_velocity().x(),
imu.angular_velocity().y(),
imu.angular_velocity().z()));
gz::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d(
imu.angular_velocity().x(),
imu.angular_velocity().y(),
imu.angular_velocity().z()));
// publish gyro
sensor_gyro_s sensor_gyro{};
@@ -382,7 +420,7 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu)
pthread_mutex_unlock(&_node_mutex);
}
void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
{
if (hrt_absolute_time() == 0) {
return;
@@ -402,10 +440,10 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
const double dt = math::constrain((time_us - _timestamp_prev) * 1e-6, 0.001, 0.1);
_timestamp_prev = time_us;
ignition::msgs::Vector3d pose_position = pose.pose(p).position();
ignition::msgs::Quaternion pose_orientation = pose.pose(p).orientation();
gz::msgs::Vector3d pose_position = pose.pose(p).position();
gz::msgs::Quaternion pose_orientation = pose.pose(p).orientation();
static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0);
/**
* @brief Quaternion for rotation between ENU and NED frames
@@ -414,17 +452,17 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
* ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North)
* This rotation is symmetric, so q_ENU_to_NED == q_NED_to_ENU.
*/
static const auto q_ENU_to_NED = ignition::math::Quaterniond(0, 0.70711, 0.70711, 0);
static const auto q_ENU_to_NED = gz::math::Quaterniond(0, 0.70711, 0.70711, 0);
// ground truth
ignition::math::Quaterniond q_gr = ignition::math::Quaterniond(
pose_orientation.w(),
pose_orientation.x(),
pose_orientation.y(),
pose_orientation.z());
gz::math::Quaterniond q_gr = gz::math::Quaterniond(
pose_orientation.w(),
pose_orientation.x(),
pose_orientation.y(),
pose_orientation.z());
ignition::math::Quaterniond q_gb = q_gr * q_FLU_to_FRD.Inverse();
ignition::math::Quaterniond q_nb = q_ENU_to_NED * q_gb;
gz::math::Quaterniond q_gb = q_gr * q_FLU_to_FRD.Inverse();
gz::math::Quaterniond q_nb = q_ENU_to_NED * q_gb;
// publish attitude groundtruth
vehicle_attitude_s vehicle_attitude_groundtruth{};
src/modules/simulation/gz_bridge/GZBridge.hpp
+14 -8
View File
@@ -47,6 +47,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_angular_velocity.h>
@@ -54,10 +55,12 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <ignition/msgs.hh>
#include <ignition/transport.hh>
#include <ignition/math.hh>
#include <ignition/msgs/imu.pb.h>
#include <gz/math.hh>
#include <gz/msgs.hh>
#include <gz/transport.hh>
// #include <gz/msgs/fluid_pressure.pb.h>
#include <gz/msgs/imu.pb.h>
using namespace time_literals;
@@ -89,13 +92,16 @@ private:
bool updateClock(const uint64_t tv_sec, const uint64_t tv_nsec);
void clockCallback(const ignition::msgs::Clock &clock);
void imuCallback(const ignition::msgs::IMU &imu);
void poseInfoCallback(const ignition::msgs::Pose_V &pose);
void clockCallback(const gz::msgs::Clock &clock);
//void airpressureCallback(const gz::msgs::FluidPressure &air_pressure);
void imuCallback(const gz::msgs::IMU &imu);
void poseInfoCallback(const gz::msgs::Pose_V &pose);
// Subscriptions
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
//uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
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)};
@@ -123,7 +129,7 @@ private:
const std::string _model_sim;
const std::string _model_pose;
ignition::transport::Node _node;
gz::transport::Node _node;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SIM_GZ_HOME_LAT>) _param_sim_home_lat,
src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp
+3 -3
View File
@@ -46,7 +46,7 @@ bool GZMixingInterfaceESC::init(const std::string &model_name)
// output eg /X500/command/motor_speed
std::string actuator_topic = "/" + model_name + "/command/motor_speed";
_actuators_pub = _node.Advertise<ignition::msgs::Actuators>(actuator_topic);
_actuators_pub = _node.Advertise<gz::msgs::Actuators>(actuator_topic);
if (!_actuators_pub.Valid()) {
PX4_ERR("failed to advertise %s", actuator_topic.c_str());
@@ -73,7 +73,7 @@ bool GZMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_
}
if (active_output_count > 0) {
ignition::msgs::Actuators rotor_velocity_message;
gz::msgs::Actuators rotor_velocity_message;
rotor_velocity_message.mutable_velocity()->Resize(active_output_count, 0);
for (unsigned i = 0; i < active_output_count; i++) {
@@ -96,7 +96,7 @@ void GZMixingInterfaceESC::Run()
pthread_mutex_unlock(&_node_mutex);
}
void GZMixingInterfaceESC::motorSpeedCallback(const ignition::msgs::Actuators &actuators)
void GZMixingInterfaceESC::motorSpeedCallback(const gz::msgs::Actuators &actuators)
{
if (hrt_absolute_time() == 0) {
return;
src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp
+5 -5
View File
@@ -35,7 +35,7 @@
#include <lib/mixer_module/mixer_module.hpp>
#include <ignition/transport.hh>
#include <gz/transport.hh>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/esc_status.h>
@@ -49,7 +49,7 @@ class GZMixingInterfaceESC : public OutputModuleInterface
public:
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
GZMixingInterfaceESC(ignition::transport::Node &node, pthread_mutex_t &node_mutex) :
GZMixingInterfaceESC(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
OutputModuleInterface(MODULE_NAME "-actuators-esc", px4::wq_configurations::rate_ctrl),
_node(node),
_node_mutex(node_mutex)
@@ -73,14 +73,14 @@ private:
void Run() override;
void motorSpeedCallback(const ignition::msgs::Actuators &actuators);
void motorSpeedCallback(const gz::msgs::Actuators &actuators);
ignition::transport::Node &_node;
gz::transport::Node &_node;
pthread_mutex_t &_node_mutex;
MixingOutput _mixing_output{"SIM_GZ_EC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
ignition::transport::Node::Publisher _actuators_pub;
gz::transport::Node::Publisher _actuators_pub;
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp
+4 -4
View File
@@ -35,12 +35,12 @@
bool GZMixingInterfaceServo::init(const std::string &model_name)
{
// /model/plane_0/joint/left_elevon_joint/0/cmd_pos
// /model/rascal_110_0/px4_servo_2
for (int i = 0; i < 8; i++) {
std::string joint_name = "px4_servo_" + std::to_string(i);
std::string servo_topic = "/model/" + model_name + "/joint/" + joint_name + "/0/cmd_pos";
std::string servo_topic = "/model/" + model_name + "/" + joint_name;
//std::cout << "Servo topic: " << servo_topic << std::endl;
_servos_pub.push_back(_node.Advertise<ignition::msgs::Double>(servo_topic));
_servos_pub.push_back(_node.Advertise<gz::msgs::Double>(servo_topic));
if (!_servos_pub.back().Valid()) {
PX4_ERR("failed to advertise %s", servo_topic.c_str());
@@ -63,7 +63,7 @@ bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MA
for (auto &servo_pub : _servos_pub) {
if (_mixing_output.isFunctionSet(i)) {
ignition::msgs::Double servo_output;
gz::msgs::Double servo_output;
///TODO: Normalize output data
double output = (outputs[i] - 500) / 500.0;
// std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl;
src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp
+4 -4
View File
@@ -35,7 +35,7 @@
#include <lib/mixer_module/mixer_module.hpp>
#include <ignition/transport.hh>
#include <gz/transport.hh>
// GZBridge mixing class for Servos.
// It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling
@@ -43,7 +43,7 @@
class GZMixingInterfaceServo : public OutputModuleInterface
{
public:
GZMixingInterfaceServo(ignition::transport::Node &node, pthread_mutex_t &node_mutex) :
GZMixingInterfaceServo(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
OutputModuleInterface(MODULE_NAME "-actuators-servo", px4::wq_configurations::rate_ctrl),
_node(node),
_node_mutex(node_mutex)
@@ -67,10 +67,10 @@ private:
void Run() override;
ignition::transport::Node &_node;
gz::transport::Node &_node;
pthread_mutex_t &_node_mutex;
MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
std::vector<ignition::transport::Node::Publisher> _servos_pub;
std::vector<gz::transport::Node::Publisher> _servos_pub;
};
src/modules/simulation/gz_bridge/gz_env.sh.in
-3
View File
@@ -3,7 +3,4 @@
export PX4_GZ_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/gz/models
export PX4_GZ_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/gz/worlds
export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS
# IGN -> GZ as of Garden
export GZ_SIM_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS
src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake
+3 -1
View File
@@ -1,5 +1,7 @@
find_package(gazebo)
find_package(gazebo
QUIET
)
if(gazebo_FOUND)