fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 03:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Gazebo Simulation Enablement (#20319)

Browse Source
This commit is contained in:
Benjamin Perseghetti
2022-09-29 13:49:31 -04:00
committed by GitHub
parent f89df9d986
commit f9522e831c
48 changed files with 269 additions and 2269 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/simulator_ignition_bridge/CMakeLists.txt → src/modules/simulation/gz_bridge/CMakeLists.txt
+16 -16
View File
@@ -48,13 +48,13 @@ if(ignition-transport_FOUND)
set(IGN_TRANSPORT_VER ${ignition-transport_VERSION_MAJOR})
px4_add_module(
MODULE modules__simulation__ignition_bridge
MAIN simulator_ignition_bridge
MODULE modules__simulation__gz_bridge
MAIN gz_bridge
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS
SimulatorIgnitionBridge.cpp
SimulatorIgnitionBridge.hpp
GZBridge.cpp
GZBridge.hpp
DEPENDS
mixer_module
px4_work_queue
@@ -63,33 +63,33 @@ if(ignition-transport_FOUND)
module.yaml
)
file(GLOB ign_models
file(GLOB gz_models
LIST_DIRECTORIES true
RELATIVE ${PX4_SOURCE_DIR}/Tools/simulation/ignition/models
RELATIVE ${PX4_SOURCE_DIR}/Tools/simulation/gz/models
CONFIGURE_DEPENDS
${PX4_SOURCE_DIR}/Tools/simulation/ignition/models/*
${PX4_SOURCE_DIR}/Tools/simulation/gz/models/*
)
file(GLOB ign_worlds
file(GLOB gz_worlds
CONFIGURE_DEPENDS
${PX4_SOURCE_DIR}/Tools/simulation/ignition/worlds/*.sdf
${PX4_SOURCE_DIR}/Tools/simulation/gz/worlds/*.sdf
)
foreach(model ${ign_models})
foreach(world ${ign_worlds})
foreach(model ${gz_models})
foreach(world ${gz_worlds})
get_filename_component("world_name" ${world} NAME_WE)
if(world_name MATCHES "default")
add_custom_target(ign_${model}
add_custom_target(gz_${model}
COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=${model} $<TARGET_FILE:px4>
WORKING_DIRECTORY ${SITL_WORKING_DIR}
USES_TERMINAL
DEPENDS px4
)
else()
add_custom_target(ign_${model}_${world_name}
COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=${model} PX4_SIM_WORLD=${world_name} $<TARGET_FILE:px4>
add_custom_target(gz_${model}_${world_name}
COMMAND ${CMAKE_COMMAND} -E env PX4_GZ_MODEL=${model} PX4_GZ_WORLD=${world_name} $<TARGET_FILE:px4>
WORKING_DIRECTORY ${SITL_WORKING_DIR}
USES_TERMINAL
DEPENDS px4
@@ -99,8 +99,8 @@ if(ignition-transport_FOUND)
endforeach()
# TODO: PX4_IGN_MODELS_PATH
# PX4_IGN_WORLDS_PATH
# TODO: PX4_GZ_MODELS_PATH
# PX4_GZ_WORLDS_PATH
configure_file(gazebo_env.sh.in ${PX4_BINARY_DIR}/rootfs/gazebo_env.sh)
endif()
src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp → src/modules/simulation/gz_bridge/GZBridge.cpp
+112 -81
View File
@@ -31,7 +31,7 @@
*
****************************************************************************/
#include "SimulatorIgnitionBridge.hpp"
#include "GZBridge.hpp"
#include <uORB/Subscription.hpp>
@@ -42,10 +42,12 @@
#include <iostream>
#include <string>
SimulatorIgnitionBridge::SimulatorIgnitionBridge(const char *world, const char *model, const char *pose_str) :
GZBridge::GZBridge(const char *world, const char *name, const char *model,
const char *pose_str) :
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
_world_name(world),
_model_name(model),
_model_name(name),
_model_sim(model),
_model_pose(pose_str)
{
pthread_mutex_init(&_mutex, nullptr);
@@ -53,7 +55,7 @@ SimulatorIgnitionBridge::SimulatorIgnitionBridge(const char *world, const char *
updateParams();
}
SimulatorIgnitionBridge::~SimulatorIgnitionBridge()
GZBridge::~GZBridge()
{
// TODO: unsubscribe
@@ -62,71 +64,73 @@ SimulatorIgnitionBridge::~SimulatorIgnitionBridge()
}
}
int SimulatorIgnitionBridge::init()
int GZBridge::init()
{
// service call to create model
// ign service -s /world/${PX4_SIM_WORLD}/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 1000 --req "sdf_filename: \"${PX4_SIM_MODEL}/model.sdf\""
ignition::msgs::EntityFactory req{};
req.set_sdf_filename(_model_name + "/model.sdf");
if (!_model_sim.empty()) {
// TODO: support model instances?
// req.set_name("model_instance_name"); // New name for the entity, overrides the name on the SDF.
req.set_allow_renaming(false); // allowed to rename the entity in case of overlap with existing entities
// 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{};
req.set_sdf_filename(_model_sim + "/model.sdf");
if (!_model_pose.empty()) {
PX4_INFO("Requested Model Position: %s", _model_pose.c_str());
req.set_name(_model_name); // New name for the entity, overrides the name on the SDF.
std::vector<double> model_pose_v;
req.set_allow_renaming(false); // allowed to rename the entity in case of overlap with existing entities
std::stringstream ss(_model_pose);
if (!_model_pose.empty()) {
PX4_INFO("Requested Model Position: %s", _model_pose.c_str());
while (ss.good()) {
std::string substr;
std::getline(ss, substr, ',');
model_pose_v.push_back(std::stod(substr));
std::vector<float> model_pose_v;
std::stringstream ss(_model_pose);
while (ss.good()) {
std::string substr;
std::getline(ss, substr, ',');
model_pose_v.push_back(std::stof(substr));
}
while (model_pose_v.size() < 6) {
model_pose_v.push_back(0.0);
}
ignition::msgs::Pose *p = req.mutable_pose();
ignition::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]);
q.Normalize();
ignition::msgs::Quaternion *orientation = p->mutable_orientation();
orientation->set_x(q.X());
orientation->set_y(q.Y());
orientation->set_z(q.Z());
orientation->set_w(q.W());
}
while (model_pose_v.size() < 6) {
model_pose_v.push_back(0.0);
}
//world/$WORLD/create service.
ignition::msgs::Boolean rep;
bool result;
std::string create_service = "/world/" + _world_name + "/create";
ignition::msgs::Pose *p = req.mutable_pose();
ignition::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]);
if (_node.Request(create_service, req, 1000, rep, result)) {
if (!rep.data() || !result) {
PX4_ERR("EntityFactory service call failed");
return PX4_ERROR;
}
ignition::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]);
q.Normalize();
ignition::msgs::Quaternion *orientation = p->mutable_orientation();
orientation->set_x(q.X());
orientation->set_y(q.Y());
orientation->set_z(q.Z());
orientation->set_w(q.W());
}
//world/$WORLD/create service.
ignition::msgs::Boolean rep;
bool result;
std::string create_service = "/world/" + _world_name + "/create";
if (_node.Request(create_service, req, 1000, rep, result)) {
if (!rep.data() || !result) {
PX4_ERR("EntityFactory service call failed");
} else {
PX4_ERR("Service call timed out");
return PX4_ERROR;
}
} else {
PX4_ERR("Service call timed out");
return PX4_ERROR;
}
// clock
std::string clock_topic = "/world/" + _world_name + "/clock";
if (!_node.Subscribe(clock_topic, &SimulatorIgnitionBridge::clockCallback, this)) {
if (!_node.Subscribe(clock_topic, &GZBridge::clockCallback, this)) {
PX4_ERR("failed to subscribe to %s", clock_topic.c_str());
return PX4_ERROR;
}
@@ -134,7 +138,7 @@ int SimulatorIgnitionBridge::init()
// pose: /world/$WORLD/pose/info
std::string world_pose_topic = "/world/" + _world_name + "/pose/info";
if (!_node.Subscribe(world_pose_topic, &SimulatorIgnitionBridge::poseInfoCallback, this)) {
if (!_node.Subscribe(world_pose_topic, &GZBridge::poseInfoCallback, this)) {
PX4_ERR("failed to subscribe to %s", world_pose_topic.c_str());
return PX4_ERROR;
}
@@ -142,15 +146,15 @@ int SimulatorIgnitionBridge::init()
// 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, &SimulatorIgnitionBridge::imuCallback, this)) {
if (!_node.Subscribe(imu_topic, &GZBridge::imuCallback, this)) {
PX4_ERR("failed to subscribe to %s", imu_topic.c_str());
return PX4_ERROR;
}
// ESC feedback: /model/X3/command/motor_speed
std::string motor_speed_topic = "/model/" + _model_name + "/command/motor_speed";
// ESC feedback: /x500/command/motor_speed
std::string motor_speed_topic = "/" + _model_name + "/command/motor_speed";
if (!_node.Subscribe(motor_speed_topic, &SimulatorIgnitionBridge::motorSpeedCallback, this)) {
if (!_node.Subscribe(motor_speed_topic, &GZBridge::motorSpeedCallback, this)) {
PX4_ERR("failed to subscribe to %s", motor_speed_topic.c_str());
return PX4_ERROR;
}
@@ -160,9 +164,8 @@ int SimulatorIgnitionBridge::init()
PX4_INFO("subscribed: %s", sub_topic.c_str());
}
// output eg /model/X3/command/motor_speed
std::string actuator_topic = "/model/" + _model_name + "/command/motor_speed";
// output eg /X500/command/motor_speed
std::string actuator_topic = "/" + _model_name + "/command/motor_speed";
_actuators_pub = _node.Advertise<ignition::msgs::Actuators>(actuator_topic);
if (!_actuators_pub.Valid()) {
@@ -174,11 +177,13 @@ int SimulatorIgnitionBridge::init()
return OK;
}
int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
int GZBridge::task_spawn(int argc, char *argv[])
{
const char *world_name = "default";
const char *model_name = nullptr;
const char *model_pose = nullptr;
const char *model_sim = nullptr;
const char *px4_instance = nullptr;
bool error_flag = false;
@@ -186,14 +191,14 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
int ch;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "w:m:p:", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "w:m:p:i:n:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'w':
// world
world_name = myoptarg;
break;
case 'm':
case 'n':
// model
model_name = myoptarg;
break;
@@ -203,6 +208,16 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
model_pose = myoptarg;
break;
case 'm':
// pose
model_sim = myoptarg;
break;
case 'i':
// pose
px4_instance = myoptarg;
break;
case '?':
error_flag = true;
break;
@@ -218,13 +233,27 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
return PX4_ERROR;
}
PX4_INFO("world: %s, model: %s", world_name, model_name);
if (!model_pose) {
model_pose = "";
}
SimulatorIgnitionBridge *instance = new SimulatorIgnitionBridge(world_name, model_name, model_pose);
if (!model_sim) {
model_sim = "";
}
if (!px4_instance) {
if (!model_name) {
model_name = model_sim;
}
} else if (!model_name) {
std::string model_name_std = std::string(model_sim) + "_" + std::string(px4_instance);
model_name = model_name_std.c_str();
}
PX4_INFO("world: %s, model name: %s, simulation model: %s", world_name, model_name, model_sim);
GZBridge *instance = new GZBridge(world_name, model_name, model_sim, model_pose);
if (instance) {
_object.store(instance);
@@ -270,7 +299,7 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
return PX4_ERROR;
}
bool SimulatorIgnitionBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec)
bool GZBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec)
{
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
struct timespec ts;
@@ -287,7 +316,7 @@ bool SimulatorIgnitionBridge::updateClock(const uint64_t tv_sec, const uint64_t
return false;
}
void SimulatorIgnitionBridge::clockCallback(const ignition::msgs::Clock &clock)
void GZBridge::clockCallback(const ignition::msgs::Clock &clock)
{
pthread_mutex_lock(&_mutex);
@@ -300,7 +329,7 @@ void SimulatorIgnitionBridge::clockCallback(const ignition::msgs::Clock &clock)
pthread_mutex_unlock(&_mutex);
}
void SimulatorIgnitionBridge::imuCallback(const ignition::msgs::IMU &imu)
void GZBridge::imuCallback(const ignition::msgs::IMU &imu)
{
if (hrt_absolute_time() == 0) {
return;
@@ -355,7 +384,7 @@ void SimulatorIgnitionBridge::imuCallback(const ignition::msgs::IMU &imu)
pthread_mutex_unlock(&_mutex);
}
void SimulatorIgnitionBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
{
if (hrt_absolute_time() == 0) {
return;
@@ -475,7 +504,7 @@ void SimulatorIgnitionBridge::poseInfoCallback(const ignition::msgs::Pose_V &pos
pthread_mutex_unlock(&_mutex);
}
void SimulatorIgnitionBridge::motorSpeedCallback(const ignition::msgs::Actuators &actuators)
void GZBridge::motorSpeedCallback(const ignition::msgs::Actuators &actuators)
{
if (hrt_absolute_time() == 0) {
return;
@@ -504,8 +533,8 @@ void SimulatorIgnitionBridge::motorSpeedCallback(const ignition::msgs::Actuators
pthread_mutex_unlock(&_mutex);
}
bool SimulatorIgnitionBridge::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
bool GZBridge::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
unsigned active_output_count = 0;
@@ -534,7 +563,7 @@ bool SimulatorIgnitionBridge::updateOutputs(bool stop_motors, uint16_t outputs[M
return false;
}
void SimulatorIgnitionBridge::Run()
void GZBridge::Run()
{
if (should_exit()) {
ScheduleClear();
@@ -563,7 +592,7 @@ void SimulatorIgnitionBridge::Run()
pthread_mutex_unlock(&_mutex);
}
int SimulatorIgnitionBridge::print_status()
int GZBridge::print_status()
{
//perf_print_counter(_cycle_perf);
_mixing_output.printStatus();
@@ -571,12 +600,12 @@ int SimulatorIgnitionBridge::print_status()
return 0;
}
int SimulatorIgnitionBridge::custom_command(int argc, char *argv[])
int GZBridge::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int SimulatorIgnitionBridge::print_usage(const char *reason)
int GZBridge::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
@@ -588,17 +617,19 @@ int SimulatorIgnitionBridge::print_usage(const char *reason)
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("simulator_ignition_bridge", "driver");
PRINT_MODULE_USAGE_NAME("gz_bridge", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, nullptr, "Model name", false);
PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, nullptr, "Fuel model name", false);
PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, nullptr, "Model Pose", false);
PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Model name", false);
PRINT_MODULE_USAGE_PARAM_STRING('i', nullptr, nullptr, "PX4 instance", false);
PRINT_MODULE_USAGE_PARAM_STRING('w', nullptr, nullptr, "World name", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int simulator_ignition_bridge_main(int argc, char *argv[])
extern "C" __EXPORT int gz_bridge_main(int argc, char *argv[])
{
return SimulatorIgnitionBridge::main(argc, argv);
return GZBridge::main(argc, argv);
}
src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp → src/modules/simulation/gz_bridge/GZBridge.hpp
+8 -7
View File
@@ -60,11 +60,11 @@
using namespace time_literals;
class SimulatorIgnitionBridge : public ModuleBase<SimulatorIgnitionBridge>, public OutputModuleInterface
class GZBridge : public ModuleBase<GZBridge>, public OutputModuleInterface
{
public:
SimulatorIgnitionBridge(const char *world, const char *model, const char *pose_str);
~SimulatorIgnitionBridge() override;
GZBridge(const char *world, const char *name, const char *model, const char *pose_str);
~GZBridge() override;
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
@@ -121,16 +121,17 @@ private:
const std::string _world_name;
const std::string _model_name;
const std::string _model_sim;
const std::string _model_pose;
MixingOutput _mixing_output{"SIM_IGN", 8, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
MixingOutput _mixing_output{"SIM_GZ", 8, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
ignition::transport::Node _node;
ignition::transport::Node::Publisher _actuators_pub;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SIM_IGN_HOME_LAT>) _param_sim_home_lat,
(ParamFloat<px4::params::SIM_IGN_HOME_LON>) _param_sim_home_lon,
(ParamFloat<px4::params::SIM_IGN_HOME_ALT>) _param_sim_home_alt
(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
)
};
src/modules/simulation/gz_bridge/Kconfig
+5
View File
@@ -0,0 +1,5 @@
menuconfig MODULES_SIMULATION_GZ_BRIDGE
bool "gz_bridge"
default n
---help---
Enable support for gz_bridge
src/modules/simulation/gz_bridge/gazebo_env.sh.in
+6
View File
@@ -0,0 +1,6 @@
#!/usr/bin/env bash
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
src/modules/simulation/simulator_ignition_bridge/module.yaml → src/modules/simulation/gz_bridge/module.yaml
+2 -2
View File
@@ -1,7 +1,7 @@
module_name: SIM_IGN
module_name: SIM_GZ
actuator_output:
output_groups:
- param_prefix: SIM_IGN
- param_prefix: SIM_GZ
channel_label: Channel
num_channels: 8
standard_params:
src/modules/simulation/simulator_ignition_bridge/parameters.c → src/modules/simulation/gz_bridge/parameters.c
+3 -3
View File
@@ -37,7 +37,7 @@
* @unit deg
* @group Simulator
*/
PARAM_DEFINE_FLOAT(SIM_IGN_HOME_LAT, 47.397742f);
PARAM_DEFINE_FLOAT(SIM_GZ_HOME_LAT, 47.397742f);
/**
* simulator origin longitude
@@ -45,7 +45,7 @@ PARAM_DEFINE_FLOAT(SIM_IGN_HOME_LAT, 47.397742f);
* @unit deg
* @group Simulator
*/
PARAM_DEFINE_FLOAT(SIM_IGN_HOME_LON, 8.545594);
PARAM_DEFINE_FLOAT(SIM_GZ_HOME_LON, 8.545594);
/**
* simulator origin altitude
@@ -53,4 +53,4 @@ PARAM_DEFINE_FLOAT(SIM_IGN_HOME_LON, 8.545594);
* @unit m
* @group Simulator
*/
PARAM_DEFINE_FLOAT(SIM_IGN_HOME_ALT, 488.0);
PARAM_DEFINE_FLOAT(SIM_GZ_HOME_ALT, 488.0);
src/modules/simulation/simulator_ignition_bridge/Kconfig
-5
View File
@@ -1,5 +0,0 @@
menuconfig MODULES_SIMULATION_SIMULATOR_IGNITION_BRIDGE
bool "simulator_ignition_bridge"
default n
---help---
Enable support for simulator_ignition_bridge
src/modules/simulation/simulator_ignition_bridge/gazebo_env.sh.in
-6
View File
@@ -1,6 +0,0 @@
#!/usr/bin/env bash
export PX4_IGN_GAZEBO_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/ignition/models
export PX4_IGN_GAZEBO_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/ignition/worlds
export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:$PX4_IGN_GAZEBO_MODELS