Skip to content

Robot spawner services #6

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

Open
cychitivav wants to merge 6 commits into
base: ros2
Choose a base branch
from
Open

Robot spawner services #6

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
00537c8
Add dynamic spawning service and parameters for UAVs
cychitivav Mar 5, 2026
acaceae
fix: update spawn service path and add UAV check in collision handling
cychitivav Mar 5, 2026
5eb39e7
feat: add Kill service for UAV removal functionality
cychitivav Mar 9, 2026
81ff504
refactor: move spawn and kill services to mrs_msgs
cychitivav Mar 14, 2026
c0be436
feat: add callback group to spawn and kill services
cychitivav Mar 14, 2026
37b0f06
fix: improve UAV parameter validation and error handling in UavSystemRos
cychitivav Mar 14, 2026
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
14 changes: 14 additions & 0 deletions include/mrs_multirotor_simulator/uav_system_ros.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,12 +34,24 @@
namespace mrs_multirotor_simulator
{

struct SpawnParams_t
{
std::string type;
double x;
double y;
double z;
double heading;
};

struct UavSystemRos_CommonHandlers_t
{

rclcpp::Node::SharedPtr node;
std::string uav_name;
std::optional<std::shared_ptr<mrs_lib::TransformBroadcaster>> transform_broadcaster;

// Optional spawn parameters for dynamic spawning
std::optional<SpawnParams_t> spawn_params;
};

class UavSystemRos {
Expand All @@ -60,6 +72,8 @@ class UavSystemRos {
MultirotorModel::ModelParams getParams();
MultirotorModel::State getState();

std::string getUavName(void) const;

private:
rclcpp::Node::SharedPtr node_;

Expand Down
130 changes: 130 additions & 0 deletions src/multirotor_simulator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,9 @@
#include <rosgraph_msgs/msg/clock.hpp>
#include <geometry_msgs/msg/pose_array.hpp>

#include <mrs_msgs/srv/spawn.hpp>
#include <mrs_msgs/srv/kill.hpp>

#include <mrs_lib/param_loader.h>
#include <mrs_lib/publisher_handler.h>
#include <mrs_lib/timer_handler.h>
Expand All @@ -15,6 +18,7 @@
#include <KDTreeVectorOfVectorsAdaptor.h>
#include <Eigen/Dense>
#include <vector>
#include <algorithm>

#include <mrs_multirotor_simulator/uav_system_ros.h>
#include <mrs_multirotor_simulator/rate_counter.h>
Expand Down Expand Up @@ -64,6 +68,16 @@ class MultirotorSimulator : public mrs_lib::Node {
rclcpp::TimerBase::SharedPtr timer_status_;
void timerStatus();

// | ----------------------- services ----------------------- |

rclcpp::Service<mrs_msgs::srv::Spawn>::SharedPtr service_spawn_;

void callbackSpawn(const std::shared_ptr<mrs_msgs::srv::Spawn::Request> request, const std::shared_ptr<mrs_msgs::srv::Spawn::Response> response);

rclcpp::Service<mrs_msgs::srv::Kill>::SharedPtr service_kill_;

void callbackKill(const std::shared_ptr<mrs_msgs::srv::Kill::Request> request, const std::shared_ptr<mrs_msgs::srv::Kill::Response> response);

// | ------------------------ rtf check ----------------------- |

double actual_rtf_ = 1.0;
Expand Down Expand Up @@ -239,6 +253,23 @@ void MultirotorSimulator::initialize() {

ph_poses_ = mrs_lib::PublisherHandler<geometry_msgs::msg::PoseArray>(node_, "~/uav_poses_out");

// | ----------------------- services ----------------------- |

Comment thread
cychitivav marked this conversation as resolved.
service_spawn_ = node_->create_service<mrs_msgs::srv::Spawn>(
"~/spawn",
[this](const std::shared_ptr<mrs_msgs::srv::Spawn::Request> request, const std::shared_ptr<mrs_msgs::srv::Spawn::Response> response) {
callbackSpawn(request, response);
},
rclcpp::ServicesQoS(), cbgrp_main_);

service_kill_ = node_->create_service<mrs_msgs::srv::Kill>(
"~/kill",
[this](const std::shared_ptr<mrs_msgs::srv::Kill::Request> request, const std::shared_ptr<mrs_msgs::srv::Kill::Response> response) {
callbackKill(request, response);
},
rclcpp::ServicesQoS(), cbgrp_main_);


// | ------------------------- timers ------------------------- |

timer_main_ = node_->create_wall_timer(std::chrono::duration<double>(1.0 / (_clock_rate_ * drs_params_.realtime_factor)),
Expand Down Expand Up @@ -381,6 +412,9 @@ void MultirotorSimulator::handleCollisions(void) {
if (!(drs_params.collisions_crash || drs_params.collisions_enabled)) {
return;
}
if (uavs_.empty()) {
return;
}

std::vector<Eigen::VectorXd> poses;

Expand Down Expand Up @@ -478,6 +512,102 @@ void MultirotorSimulator::publishPoses(void) {

//}

/* callbackSpawn() //{ */

void MultirotorSimulator::callbackSpawn(const std::shared_ptr<mrs_msgs::srv::Spawn::Request> request,
const std::shared_ptr<mrs_msgs::srv::Spawn::Response> response) {

RCLCPP_INFO(node_->get_logger(), "callbackSpawn(): spawning '%s' of type '%s' at [%.2f, %.2f, %.2f], heading: %.2f", request->name.c_str(),
request->type.c_str(), request->x, request->y, request->z, request->heading);

response->success = false;
response->message = "";

// Validate spawn parameters
if (request->name.empty()) {
response->message = "UAV name cannot be empty";
RCLCPP_ERROR(node_->get_logger(), "callbackSpawn(): %s", response->message.c_str());
return;
}

if (request->type.empty()) {
response->message = "UAV type cannot be empty";
RCLCPP_ERROR(node_->get_logger(), "callbackSpawn(): %s", response->message.c_str());
return;
}

// Check if UAV with this name already exists
for (const auto &uav : uavs_) {
if (uav->getUavName() == request->name) {
response->message = "UAV with name '" + request->name + "' already exists";
RCLCPP_ERROR(node_->get_logger(), "callbackSpawn(): %s", response->message.c_str());
return;
}
}

try {
UavSystemRos_CommonHandlers_t common_handlers;

common_handlers.node = node_;
common_handlers.uav_name = request->name;
common_handlers.transform_broadcaster = tf_broadcaster_;

// Set spawn parameters from request
SpawnParams_t spawn_params;
spawn_params.type = request->type;
spawn_params.x = static_cast<double>(request->x);
spawn_params.y = static_cast<double>(request->y);
spawn_params.z = static_cast<double>(request->z);
spawn_params.heading = static_cast<double>(request->heading);

common_handlers.spawn_params = spawn_params;

uavs_.push_back(std::make_unique<UavSystemRos>(common_handlers));

Comment thread
cychitivav marked this conversation as resolved.
response->success = true;
response->message = "Successfully spawned UAV '" + request->name + "'";
RCLCPP_INFO(node_->get_logger(), "callbackSpawn(): %s", response->message.c_str());
}
catch (const std::exception &e) {
response->message = "Failed to spawn UAV: " + std::string(e.what());
}
}

//}

/* callbackKill() //{ */

void MultirotorSimulator::callbackKill(const std::shared_ptr<mrs_msgs::srv::Kill::Request> request,
const std::shared_ptr<mrs_msgs::srv::Kill::Response> response) {

RCLCPP_INFO(node_->get_logger(), "callbackKill(): removing UAV '%s'", request->name.c_str());

response->success = false;
response->message = "";

if (request->name.empty()) {
response->message = "UAV name cannot be empty";
RCLCPP_ERROR(node_->get_logger(), "callbackKill(): %s", response->message.c_str());
return;
}

auto it = std::find_if(uavs_.begin(), uavs_.end(), [&](const std::unique_ptr<UavSystemRos> &uav) { return uav->getUavName() == request->name; });

Comment thread
cychitivav marked this conversation as resolved.
if (it == uavs_.end()) {
response->message = "UAV '" + request->name + "' not found";
RCLCPP_ERROR(node_->get_logger(), "callbackKill(): %s", response->message.c_str());
return;
}

uavs_.erase(it);

response->success = true;
response->message = "Successfully removed UAV '" + request->name + "'";
RCLCPP_INFO(node_->get_logger(), "callbackKill(): %s", response->message.c_str());
}

//}

} // namespace mrs_multirotor_simulator

#include <rclcpp_components/register_node_macro.hpp>
Expand Down
43 changes: 36 additions & 7 deletions src/uav_system_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,12 @@ UavSystemRos::UavSystemRos(const UavSystemRos_CommonHandlers_t common_handlers)
}

std::string type;
param_loader.loadParam(_uav_name_ + "/type", type);
if (common_handlers.spawn_params.has_value()) {
type = common_handlers.spawn_params.value().type;
RCLCPP_INFO(node_->get_logger(), "[%s]: using dynamic spawn type: %s", _uav_name_.c_str(), type.c_str());
} else {
param_loader.loadParam(_uav_name_ + "/type", type);
}

// | --------------------- general params --------------------- |

Expand Down Expand Up @@ -69,7 +74,11 @@ UavSystemRos::UavSystemRos(const UavSystemRos_CommonHandlers_t common_handlers)

// | ------------------ model-specific params ----------------- |

param_loader.loadParam(type + "/n_motors", model_params_.n_motors);
// Validate that UAV type is specified and first type-specific parameter (n_motors) can be loaded successfully
if (type.empty() || !param_loader.loadParam(type + "/n_motors", model_params_.n_motors)) {
RCLCPP_ERROR(node_->get_logger(), "UAV type is not specified or invalid.");
throw std::runtime_error("UAV type '" + type + "' is not specified or invalid.");
}
param_loader.loadParam(type + "/mass", model_params_.mass);
param_loader.loadParam(type + "/arm_length", model_params_.arm_length);
param_loader.loadParam(type + "/body_height", model_params_.body_height);
Expand All @@ -88,10 +97,21 @@ UavSystemRos::UavSystemRos(const UavSystemRos_CommonHandlers_t common_handlers)
double spawn_z;
double spawn_heading;

param_loader.loadParam(_uav_name_ + "/spawn/x", spawn_x);
param_loader.loadParam(_uav_name_ + "/spawn/y", spawn_y);
param_loader.loadParam(_uav_name_ + "/spawn/z", spawn_z);
param_loader.loadParam(_uav_name_ + "/spawn/heading", spawn_heading);
// Use provided spawn parameters if available, otherwise load from config
if (common_handlers.spawn_params.has_value()) {
const auto &params = common_handlers.spawn_params.value();
spawn_x = params.x;
spawn_y = params.y;
spawn_z = params.z;
spawn_heading = params.heading;
RCLCPP_INFO(node_->get_logger(), "[%s]: using dynamic spawn position: [%.2f, %.2f, %.2f], heading: %.2f", _uav_name_.c_str(), spawn_x, spawn_y, spawn_z,
spawn_heading);
} else {
param_loader.loadParam(_uav_name_ + "/spawn/x", spawn_x);
param_loader.loadParam(_uav_name_ + "/spawn/y", spawn_y);
param_loader.loadParam(_uav_name_ + "/spawn/z", spawn_z);
param_loader.loadParam(_uav_name_ + "/spawn/heading", spawn_heading);
}

param_loader.loadParam("randomization/enabled", _randomization_enabled_);
param_loader.loadParam("randomization/bounds/x", _randomization_bounds_x_);
Expand Down Expand Up @@ -170,7 +190,7 @@ UavSystemRos::UavSystemRos(const UavSystemRos_CommonHandlers_t common_handlers)

if (!param_loader.loadedSuccessfully()) {
RCLCPP_ERROR(node_->get_logger(), "failed to load all parameters");
rclcpp::shutdown();
throw std::runtime_error("UavSystemRos: failed to load all parameters");
}

// | ----------------------- publishers ----------------------- |
Expand Down Expand Up @@ -395,6 +415,15 @@ MultirotorModel::State UavSystemRos::getState() {

//}

/* getUavName() //{ */

std::string UavSystemRos::getUavName(void) const {

return _uav_name_;
}

//}

/* crash() //{ */

void UavSystemRos::crash(void) {
Expand Down