Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Multi drone in ignition #20221

Closed
wants to merge 6 commits into from
Closed

Multi drone in ignition #20221

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
8048123
take instance number in model/topic name to allow multiple wpans in g…
dsix-ls2n Sep 9, 2022
c3f0fc7
Merge branch 'main'
dsix-ls2n Sep 9, 2022
2e064ff
Merge branch 'main'
dsix-ls2n Sep 12, 2022
49990bc
rename instance option into postfix (more general use)
dsix-ls2n Sep 12, 2022
34b6df9
Merge branch 'main'
dsix-ls2n Sep 15, 2022
2c2c471
make format
dsix-ls2n Sep 15, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ elif [ "$PX4_SIMULATOR" = "ignition" ]; then
if [ -z $PX4_IGN_MODEL_POSE ]; then
# start ignition bridge without pose arg.
echo "WARN [init] PX4_IGN_MODEL_POSE not set, spawning at origin."
if simulator_ignition_bridge start -m "${PX4_SIM_MODEL}" -w "${PX4_SIM_WORLD}"; then
if simulator_ignition_bridge start -m "${PX4_SIM_MODEL}" -w "${PX4_SIM_WORLD}" -n "${px4_instance}"; then
sensor_baro_sim start
sensor_gps_sim start
sensor_mag_sim start
Expand All @@ -62,7 +62,7 @@ elif [ "$PX4_SIMULATOR" = "ignition" ]; then
echo "INFO [init] PX4_IGN_MODEL_POSE set, spawning at: ${model_pose}"

# start ignition bridge with pose arg.
if simulator_ignition_bridge start -p "${model_pose}" -m "${PX4_SIM_MODEL}" -w "${PX4_SIM_WORLD}"; then
if simulator_ignition_bridge start -p "${model_pose}" -m "${PX4_SIM_MODEL}" -w "${PX4_SIM_WORLD}" -n "${px4_instance}"; then
sensor_baro_sim start
sensor_gps_sim start
sensor_mag_sim start
Expand Down
30 changes: 18 additions & 12 deletions src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,11 +42,13 @@
#include <iostream>
#include <string>

SimulatorIgnitionBridge::SimulatorIgnitionBridge(const char *world, const char *model, const char *pose_str) :
SimulatorIgnitionBridge::SimulatorIgnitionBridge(const char *world, const char *model, const char *pose_str,
const char *postfix) :
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
_world_name(world),
_model_name(model),
_model_pose(pose_str)
_model_pose(pose_str),
_model_instance_name(std::string(model) + "_" + std::string(postfix))
{
pthread_mutex_init(&_mutex, nullptr);

Expand All @@ -55,8 +57,6 @@ SimulatorIgnitionBridge::SimulatorIgnitionBridge(const char *world, const char *

SimulatorIgnitionBridge::~SimulatorIgnitionBridge()
{
// TODO: unsubscribe

for (auto &sub_topic : _node.SubscribedTopics()) {
_node.Unsubscribe(sub_topic);
}
Expand All @@ -68,9 +68,7 @@ int SimulatorIgnitionBridge::init()
// 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");

// TODO: support model instances?
// req.set_name("model_instance_name"); // New name for the entity, overrides the name on the SDF.
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

if (!_model_pose.empty()) {
Expand Down Expand Up @@ -140,7 +138,8 @@ 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";
std::string imu_topic = "/world/" + _world_name + "/model/" + _model_instance_name +
"/link/base_link/sensor/imu_sensor/imu";

if (!_node.Subscribe(imu_topic, &SimulatorIgnitionBridge::imuCallback, this)) {
PX4_ERR("failed to subscribe to %s", imu_topic.c_str());
Expand All @@ -152,7 +151,7 @@ int SimulatorIgnitionBridge::init()
}

// output eg /X3/command/motor_speed
std::string actuator_topic = "model/" + _model_name + "/command/motor_speed";
std::string actuator_topic = "model/" + _model_instance_name + "/command/motor_speed";
_actuators_pub = _node.Advertise<ignition::msgs::Actuators>(actuator_topic);

if (!_actuators_pub.Valid()) {
Expand All @@ -169,14 +168,15 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
const char *world_name = "default";
const char *model_name = nullptr;
const char *model_pose = nullptr;
const char *postfix = nullptr;


bool error_flag = false;
int myoptind = 1;
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:n:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'w':
// world
Expand All @@ -193,6 +193,11 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
model_pose = myoptarg;
break;

case 'n':
// model name postfix (allows multiple drone spawn)
postfix = myoptarg;
break;

case '?':
error_flag = true;
break;
Expand All @@ -214,7 +219,7 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
model_pose = "";
}

SimulatorIgnitionBridge *instance = new SimulatorIgnitionBridge(world_name, model_name, model_pose);
SimulatorIgnitionBridge *instance = new SimulatorIgnitionBridge(world_name, model_name, model_pose, postfix);

if (instance) {
_object.store(instance);
Expand Down Expand Up @@ -329,7 +334,7 @@ void SimulatorIgnitionBridge::poseInfoCallback(const ignition::msgs::Pose_V &pos
pthread_mutex_lock(&_mutex);

for (int p = 0; p < pose.pose_size(); p++) {
if (pose.pose(p).name() == _model_name) {
if (pose.pose(p).name() == _model_instance_name) {

const uint64_t time_us = (pose.header().stamp().sec() * 1000000) + (pose.header().stamp().nsec() / 1000);

Expand Down Expand Up @@ -516,6 +521,7 @@ int SimulatorIgnitionBridge::print_usage(const char *reason)
PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, nullptr, "Model name", false);
PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, nullptr, "Model Pose", false);
PRINT_MODULE_USAGE_PARAM_STRING('w', nullptr, nullptr, "World name", true);
PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Postfix for the model name in Gazebo", false);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

return 0;
Expand Down
3 changes: 2 additions & 1 deletion src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ using namespace time_literals;
class SimulatorIgnitionBridge : public ModuleBase<SimulatorIgnitionBridge>, public OutputModuleInterface
{
public:
SimulatorIgnitionBridge(const char *world, const char *model, const char *pose_str);
SimulatorIgnitionBridge(const char *world, const char *model, const char *pose_str, const char *postfix);
~SimulatorIgnitionBridge() override;

/** @see ModuleBase */
Expand Down Expand Up @@ -117,6 +117,7 @@ class SimulatorIgnitionBridge : public ModuleBase<SimulatorIgnitionBridge>, publ
const std::string _world_name;
const std::string _model_name;
const std::string _model_pose;
const std::string _model_instance_name;

MixingOutput _mixing_output{"SIM_IGN", 8, *this, MixingOutput::SchedulingPolicy::Auto, false, false};

Expand Down