diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 7c7beea2d7f9..d1a7addf7e7b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -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 @@ -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 diff --git a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp index 60aa1de546fb..74ef0f57ad79 100644 --- a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp +++ b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp @@ -42,11 +42,13 @@ #include #include -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); @@ -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); } @@ -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()) { @@ -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()); @@ -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(actuator_topic); if (!_actuators_pub.Valid()) { @@ -169,6 +168,7 @@ 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; @@ -176,7 +176,7 @@ 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:n:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'w': // world @@ -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; @@ -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); @@ -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); @@ -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; diff --git a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp index 70efe2c392a9..c2c59778c7aa 100644 --- a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp +++ b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp @@ -62,7 +62,7 @@ using namespace time_literals; class SimulatorIgnitionBridge : public ModuleBase, 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 */ @@ -117,6 +117,7 @@ class SimulatorIgnitionBridge : public ModuleBase, 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};