From 1c69761d3bb20097985698f4a85020931cc7f989 Mon Sep 17 00:00:00 2001 From: marlonriv Date: Thu, 31 Jul 2025 17:42:50 +0200 Subject: [PATCH 01/33] Removing specific mission callbacks Replaced mission callbacks with new fleet manager action --- launch/iroc_bridge.launch | 4 +- src/iroc_bridge.cpp | 301 ++++---------------------------------- 2 files changed, 26 insertions(+), 279 deletions(-) diff --git a/launch/iroc_bridge.launch b/launch/iroc_bridge.launch index 47537c9..adb9e1d 100644 --- a/launch/iroc_bridge.launch +++ b/launch/iroc_bridge.launch @@ -67,9 +67,7 @@ - - - + diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index c0218d0..cd859a8 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -54,6 +54,7 @@ #include #include +#include #include #include #include @@ -74,11 +75,13 @@ using vec4_t = Eigen::Vector4d; using namespace actionlib; +using FleetManagerActionClient = SimpleActionClient; + typedef SimpleActionClient WaypointFleetManagerClient; typedef SimpleActionClient CoveragePlannerClient; typedef SimpleActionClient AutonomyTestClient; //Waypoint mission goal -typedef iroc_fleet_manager::WaypointFleetManagerGoal FleetManagerActionServerGoal; +typedef iroc_fleet_manager::FleetManagerGoal FleetManagerGoal; // Autonomy test goal typedef iroc_fleet_manager::AutonomyTestGoal AutonomyTestActionServerGoal; // Coverage mission goal @@ -239,8 +242,8 @@ class IROCBridge : public nodelet::Nodelet { crow::response setOriginCallback(const crow::request& req); crow::response setSafetyBorderCallback(const crow::request& req); crow::response setObstacleCallback(const crow::request& req); - crow::response waypointMissionCallback(const crow::request& req); - crow::response coverageMissionCallback(const crow::request& req); + crow::response missionCallback(const crow::request& req, const std::string& type); + crow::response coveragemissionCallback(const crow::request& req); crow::response autonomyTestCallback(const crow::request& req); crow::response changeFleetMissionStateCallback(const crow::request& req, const std::string& type); @@ -267,9 +270,7 @@ class IROCBridge : public nodelet::Nodelet { crow::websocket::connection* active_telemetry_connection_ = nullptr; std::mutex mtx_telemetry_connections_; - std::unique_ptr action_client_ptr_; - std::unique_ptr coverage_action_client_ptr_; - std::unique_ptr autonomy_test_client_ptr_; + std::unique_ptr fleet_manager_action_client_; // Latlon origin mrs_msgs::Point2D world_origin_; @@ -353,9 +354,7 @@ void IROCBridge::onInit() { CROW_ROUTE(http_srv_, "/safety-area/origin").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return setOriginCallback(req); }); CROW_ROUTE(http_srv_, "/safety-area/borders").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return setSafetyBorderCallback(req); }); CROW_ROUTE(http_srv_, "/safety-area/obstacles").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return setObstacleCallback(req); }); - CROW_ROUTE(http_srv_, "/mission/waypoints").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return waypointMissionCallback(req); }); - CROW_ROUTE(http_srv_, "/mission/coverage").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return coverageMissionCallback(req); }); - CROW_ROUTE(http_srv_, "/mission/autonomy-test").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return autonomyTestCallback(req); }); + CROW_ROUTE(http_srv_, "/mission/create/").methods(crow::HTTPMethod::Post)([this](const crow::request& req, const std::string& type){ return missionCallback(req, type); }); // Missions //TODO: CROW_REGEX_ROUTE(http_srv_, R"(/fleet/mission/(start|stop|pause))") @@ -508,22 +507,11 @@ void IROCBridge::onInit() { /* // | --------------------- action clients --------------------- | */ - //Waypoint Mission - const std::string waypoint_action_client_topic = nh_.resolveName("ac/waypoint_mission"); - action_client_ptr_ = std::make_unique(waypoint_action_client_topic, false); - ROS_INFO("[IROCBridge]: Created action client on topic \'ac/waypoint_mission\' -> \'%s\'", waypoint_action_client_topic.c_str()); - - //Coverage Mission - const std::string coverage_action_client_topic = nh_.resolveName("ac/coverage_mission"); - coverage_action_client_ptr_ = std::make_unique(coverage_action_client_topic, false); - ROS_INFO("[IROCBridge]: Created action client on topic \'ac/waypoint_mission\' -> \'%s\'", coverage_action_client_topic.c_str()); + //Mission action client + const std::string action_client_topic = nh_.resolveName("ac/mission"); + fleet_manager_action_client_ = std::make_unique(action_client_topic, false); + ROS_INFO("[IROCBridge]: Created action client on topic \'ac/mission\' -> \'%s\'", action_client_topic.c_str()); - - //Autonomy test - const std::string autonomy_test_client_topic = nh_.resolveName("ac/autonomy_test"); - autonomy_test_client_ptr_ = std::make_unique(autonomy_test_client_topic, false); - ROS_INFO("[IROCBridge]: Created action client on topic \'ac/autonomy_test\' -> \'%s\'", autonomy_test_client_topic.c_str()); - // | ------------------------- timers ------------------------- | timer_main_ = nh_.createTimer(ros::Rate(main_timer_rate), &IROCBridge::timerMain, this); @@ -1426,7 +1414,7 @@ crow::response IROCBridge::setObstacleCallback(const crow::request& req) } //} -/* waypointMissionCallback() method //{ */ +/* missionCallback() method //{ */ /** * \brief Callback for the waypoint mission request. It receives a list of missions for each robot and sends them to the fleet manager. @@ -1434,9 +1422,9 @@ crow::response IROCBridge::setObstacleCallback(const crow::request& req) * \param req Crow request * \return res Crow response */ -crow::response IROCBridge::waypointMissionCallback(const crow::request& req) +crow::response IROCBridge::missionCallback(const crow::request& req, const std::string& type) { - ROS_INFO_STREAM("[IROCBridge]: Parsing a waypointMissionCallback message JSON -> ROS."); + ROS_INFO_STREAM("[IROCBridge]: Parsing a missionCallback message JSON -> ROS."); latest_mission_type_ = "waypoint"; try { @@ -1524,39 +1512,40 @@ crow::response IROCBridge::waypointMissionCallback(const crow::request& req) } // Validate if the action client is connected and if the action is already running - if (!action_client_ptr_->isServerConnected()) { + if (!fleet_manager_action_client_->isServerConnected()) { ROS_WARN_STREAM("[IROCBridge]: Action server is not connected. Check the iroc_fleet_manager node."); std::string msg = "Action server is not connected. Check iroc_fleet_manager node.\n"; return crow::response(crow::status::CONFLICT, "{\"message\": \"" + msg + "\"}"); } - else if (!action_client_ptr_->getState().isDone()) { + else if (!fleet_manager_action_client_->getState().isDone()) { ROS_WARN_STREAM("[IROCBridge]: Mission is already running. Terminate the previous one, or wait until it is finished."); std::string msg = "Mission is already running. Terminate the previous one, or wait until it is finished"; return crow::response(crow::status::CONFLICT, "{\"message\": \"" + msg + "\"}"); } // Send the action goal to the fleet manager - FleetManagerActionServerGoal action_goal; - action_goal.robots = mission_robots; + FleetManagerGoal action_goal; + // TODO update with fleet manager new action + // action_goal.robots = mission_robots; - action_client_ptr_->sendGoal( + fleet_manager_action_client_->sendGoal( action_goal, [this](const auto& state, const auto& result) { - missionDoneCallback(state, result); + missionDoneCallback(state, result); }, [this]() { missionActiveCallback(); }, [this](const auto& feedback) { - missionFeedbackCallback(feedback); + missionFeedbackCallback(feedback); } ); // Waiting in the case the trajectories are rejected. We can better wait will the state is pending ros::Duration(mission_robots.size() * 1.0).sleep(); - if (action_client_ptr_->getState().isDone()) { // If the action is done, the action finished instantly - auto result = action_client_ptr_->getResult(); + if (fleet_manager_action_client_->getState().isDone()) { // If the action is done, the action finished instantly + auto result = fleet_manager_action_client_->getResult(); auto json = resultToJson(result); const auto message = result->message; ROS_WARN("[IROCBridge]: %s", message.c_str()); @@ -1575,246 +1564,6 @@ crow::response IROCBridge::waypointMissionCallback(const crow::request& req) } //} -/* coverageMissionCallback() method //{ */ - -/** - * \brief Callback for the coverage mission request. It receives a list of missions for each robot and sends them to the fleet manager. - * - * \param req Crow request - * \return res Crow response - */ -crow::response IROCBridge::coverageMissionCallback(const crow::request& req) -{ - ROS_INFO_STREAM("[IROCBridge]: Parsing a coverageMissionCallback message JSON -> ROS."); - latest_mission_type_ = "coverage"; - - try { - crow::json::rvalue json_msg = crow::json::load(req.body); - if (!json_msg || !json_msg.has("robots") || json_msg["robots"].t() != crow::json::type::List - || !json_msg.has("search_area") || json_msg["search_area"].t() != crow::json::type::List ) - return crow::response(crow::status::BAD_REQUEST, "{\"message\": \"Bad request: Failed to parse JSON or missing 'mission' key\"}"); - - // Get message properties currently we are setting the same parameters for all robots - std::vector robots = json_msg["robots"].lo(); - std::vector search_area = json_msg["search_area"].lo(); - int height = json_msg["height"].i(); - int height_id = json_msg["height_id"].i(); - int terminal_action = json_msg["terminal_action"].i(); - - // To store the robots - std::vector mission_robots; - - //Get the robot names - std::vector robot_names; - robot_names.reserve(robots.size()); - for (const auto& robot : robots) { - robot_names.push_back(robot.s()); - } - - for (const auto& robot : robot_names) { - std::scoped_lock lck(robot_handlers_.mtx); - iroc_fleet_manager::CoverageMissionRobot mission_robot; - auto* rh_ptr = findRobotHandler(robot, robot_handlers_); - if (!rh_ptr) { - ROS_WARN_STREAM("[IROCBridge]: Robot \"" << robot << "\" not found. Ignoring."); - return crow::response(crow::status::NOT_FOUND, "{\"message\": \"Robot '" + robot + "' not found\"}"); - } - // Get the current robot position, to use it as the starting point of the coverage mission - auto robot_global_pose = rh_ptr->sh_state_estimation_info.getMsg()->global_pose; - auto robot_local_pose = rh_ptr->sh_state_estimation_info.getMsg()->local_pose; - // Fill the mission robot struct - mission_robot.name = robot; - mission_robot.global_position.x = robot_global_pose.position.x; - mission_robot.global_position.y = robot_global_pose.position.y; - mission_robot.global_position.z = robot_global_pose.position.z; - mission_robot.local_position.x = robot_local_pose.position.x; - mission_robot.local_position.y = robot_local_pose.position.y; - mission_robot.local_position.z = robot_local_pose.position.z; - mission_robot.frame_id = iroc_mission_handler::MissionGoal::FRAME_ID_LATLON; - mission_robot.height_id = height_id; - mission_robot.height = height; - mission_robot.terminal_action = terminal_action; - mission_robots.push_back(mission_robot); - } - - //Get the search area points - std::vector polygon_points; - polygon_points.reserve(search_area.size()); - - for (const auto& el : search_area) { - mrs_msgs::Point2D pt; - pt.x = el["x"].d(); - pt.y = el["y"].d(); - - polygon_points.push_back(pt); - } - - ROS_INFO("[IROCBridge]: Polygon points size %zu ", polygon_points.size()); - - // Validate if the action client is connected and if the action is already running - if (!coverage_action_client_ptr_->isServerConnected()) { - ROS_WARN_STREAM("[IROCBridge]: Action server is not connected. Check the iroc_fleet_manager node."); - std::string msg = "Action server is not connected. Check iroc_fleet_manager node.\n"; - return crow::response(crow::status::CONFLICT, "{\"message\": \"" + msg + "\"}"); - } - else if (!coverage_action_client_ptr_->getState().isDone()) { - ROS_WARN_STREAM("[IROCBridge]: Mission is already running. Terminate the previous one, or wait until it is finished."); - std::string msg = "Mission is already running. Terminate the previous one, or wait until it is finished"; - return crow::response(crow::status::CONFLICT, "{\"message\": \"" + msg + "\"}"); - } - - // Send the action goal to the fleet manager - coverageMissionActionServerGoal action_goal; - - if (world_origin_.x == 0.0 && world_origin_.y == 0.0) { - ROS_WARN_STREAM("[IROCBridge]: World origin is not set."); - std::string msg = "World origin is not set."; - return crow::response(crow::status::CONFLICT, "{\"message\": \"" + msg + "\"}"); - } - - action_goal.mission.latlon_origin = world_origin_; - action_goal.mission.robots = mission_robots; - action_goal.mission.search_area = polygon_points; - - coverage_action_client_ptr_->sendGoal( - action_goal, - [this](const auto& state, const auto& result) { - missionDoneCallback(state, result); - }, - [this]() { - missionActiveCallback(); - }, - [this](const auto& feedback) { - missionFeedbackCallback(feedback); - } - ); - - // Waiting in the case the trajectories are rejected. We can better wait will the state is pending - // bool finished_before_timeout = coverage_action_client_ptr_->waitForResult(ros::Duration(3.0)); - // ROS_INFO("[IROCBridge]: Finished before timeout: %d", finished_before_timeout); - ros::Duration(mission_robots.size() * 1.0).sleep(); - - if (coverage_action_client_ptr_->getState().isDone()) { - auto result = coverage_action_client_ptr_->getResult(); - const auto message = result->message; - auto json = resultToJson(result); - ROS_WARN("[IROCBridge]: %s", message.c_str()); - return crow::response(crow::status::BAD_REQUEST, json); - } - else { - ROS_INFO("[IROCBridge]: Mission received successfully"); - auto json = successMissionJson(mission_robots); - return crow::response(crow::status::CREATED, json); - } - } - catch (const std::exception& e) { - ROS_WARN_STREAM("[IROCBridge]: Failed to parse JSON from message: " << e.what()); - return crow::response(crow::status::BAD_REQUEST, "{\"message\": \"Failed to parse JSON from message: " + std::string(e.what()) + "\"}"); - } -} -//} - -/* autonomyTestCallback() method //{ */ - -/** - * \brief Callback for the autonomy test request. It receives the request for a specific robot the and sends them to the fleet manager autonomy test component. - * - * \param req Crow request - * \return res Crow response - */ -crow::response IROCBridge::autonomyTestCallback(const crow::request& req) -{ - ROS_INFO_STREAM("[IROCBridge]: Parsing a autonomyTestCallback message JSON -> ROS."); - latest_mission_type_ = "autonomy_test"; - - crow::json::rvalue json_msg = crow::json::load(req.body); - if (!json_msg || !json_msg.has("robot_name")) - return crow::response(crow::status::BAD_REQUEST, "{\"message\": \"Bad request: Failed to parse JSON or missing 'robot_name' key\"}"); - - // Get message properties - std::string robot_name = json_msg["robot_name"].s(); - int segment_length = json_msg["segment_length"].i(); - - std::scoped_lock lck(robot_handlers_.mtx); - - auto* rh_ptr = findRobotHandler(robot_name, robot_handlers_); - - if (!rh_ptr) { - ROS_WARN_STREAM("[IROCBridge]: Robot \"" << robot_name << "\" not found. Ignoring."); - return crow::response(crow::status::BAD_REQUEST, "{\"message\": \"Robot \"" + robot_name + "\" not found, ignoring\"}"); - } - - auto robot_type = rh_ptr->sh_general_robot_info.getMsg()->robot_type; - - switch (robot_type) { - case static_cast(robot_type_t::MULTIROTOR): { - ROS_INFO("[IROCBridge]: MULTIROTOR TYPE: "); - bool use_heading = false; - break; - }; - - case static_cast(robot_type_t::BOAT): { - ROS_INFO("[IROCBridge]: BOAT TYPE: "); - bool use_heading = false; - break; - }; - - default: - break; - } - - //We are using an array of robots as input, as we are using the waypoint mission structure that expects an array of robots. - //Kept for simplicity to avoid changing the previous structure that supports multiple robots. - //As we decide that autonomy test will be for one drone at a time, we are only adding the specified robot for the autonomy test. - std::vector mission_robots; - iroc_fleet_manager::AutonomyTestRobot mission_robot; - mission_robot.name = robot_name; - mission_robot.segment_length = segment_length; - mission_robots.push_back(mission_robot); - - AutonomyTestActionServerGoal action_goal; - action_goal.robots = mission_robots; - - if (!autonomy_test_client_ptr_->isServerConnected()) { - std::string msg = "Action server is not connected. Check iroc_fleet_manager node.\n"; - ROS_WARN_STREAM("[IROCBridge]: Action server is not connected. Check the iroc_fleet_manager node."); - return crow::response(crow::status::CONFLICT, "{\"message\": \"" + msg + "\"}"); - } - - if (!autonomy_test_client_ptr_->getState().isDone()) { - ROS_WARN_STREAM("[IROCBridge]: Mission is already running. Terminate the previous one, or wait until it is finished."); - std::string msg = "Mission is already running. Terminate the previous one, or wait until it is finished"; - return crow::response(crow::status::CONFLICT, "{\"message\": \"" + msg + "\"}"); - } - - autonomy_test_client_ptr_->sendGoal( - action_goal, - [this](const auto& state, const auto& result) { - missionDoneCallback(state, result); - }, - [this]() { - missionActiveCallback(); - }, - [this](const auto& feedback) { - missionFeedbackCallback(feedback); - } - ); - - ros::Duration(mission_robots.size() * 1.0).sleep(); - - if (autonomy_test_client_ptr_->getState().isDone()) { // If the action is done, the action finished instantly - auto result = autonomy_test_client_ptr_->getResult(); - const auto message = result->message; - ROS_WARN("[IROCBridge]: %s", message.c_str()); - return crow::response(crow::status::SERVICE_UNAVAILABLE, "{\"message\": \"" + message + "\"}"); - } - else { - ROS_INFO("[IROCBridge]: Mission received successfully"); - return crow::response(crow::status::CREATED, "{\"message\": \"Mission received successfully\"}"); - } -} -//} - /* changeFleetMissionStateCallback() method //{ */ /** From cf12ee8484dddbd55ceeafbd46b89bdfd9e4c54d Mon Sep 17 00:00:00 2001 From: marlonriv Date: Fri, 1 Aug 2025 16:06:12 +0200 Subject: [PATCH 02/33] Removed coverage and autonomy stuff, update with new mission structure --- json_examples/autonomy_test.json | 7 +- json_examples/coverage_mission.json | 38 +++--- json_examples/missions/new_mission.json | 18 +++ json_examples/missions/two_drones.json | 88 ++++++++++---- launch/iroc_bridge.launch | 2 +- src/iroc_bridge.cpp | 147 +++--------------------- 6 files changed, 130 insertions(+), 170 deletions(-) create mode 100644 json_examples/missions/new_mission.json diff --git a/json_examples/autonomy_test.json b/json_examples/autonomy_test.json index 46b9502..fd92bcd 100644 --- a/json_examples/autonomy_test.json +++ b/json_examples/autonomy_test.json @@ -1,4 +1,7 @@ { - "robot_name": "uav1", - "segment_length": 5 + "type": "autonomy-test", + "details": { + "robot_name": "uav1", + "segment_length": 5 + } } diff --git a/json_examples/coverage_mission.json b/json_examples/coverage_mission.json index 96c5bbe..807b2cb 100644 --- a/json_examples/coverage_mission.json +++ b/json_examples/coverage_mission.json @@ -1,15 +1,27 @@ { - "robots": [ - "uav1", - "uav2" - ], - "search_area": [ - {"x": 47.397978, "y": 8.545299}, - {"x": 47.397848, "y": 8.545872}, - {"x": 47.397551, "y": 8.545720}, - {"x": 47.397699, "y": 8.545129} - ], - "height_id": 0, - "height": 10, - "terminal_action": 0 + "type": "coverage", + "details": { + "robots": ["uav1", "uav2"], + "search_area": [ + { + "x": 47.397978, + "y": 8.545299 + }, + { + "x": 47.397848, + "y": 8.545872 + }, + { + "x": 47.397551, + "y": 8.54572 + }, + { + "x": 47.397699, + "y": 8.545129 + } + ], + "height_id": 0, + "height": 10, + "terminal_action": 0 + } } diff --git a/json_examples/missions/new_mission.json b/json_examples/missions/new_mission.json new file mode 100644 index 0000000..b2a267f --- /dev/null +++ b/json_examples/missions/new_mission.json @@ -0,0 +1,18 @@ +{ + "mission": [ + { + "robot_name": "uav1", + "frame_id": 0, + "height_id": 0, + "points": [ + { "x": 20, "y": 10, "z": 3, "heading": 1 }, + { "x": 20, "y": 10, "z": 3, "heading": 3 }, + { "x": -20, "y": -20, "z": 4, "heading": 3, "subtasks": [{ "type": 1, "parameters": "[0.5,0.5,0.5]" }] }, + { "x": -10, "y": 10, "z": 5, "heading": 3 }, + { "x": 10, "y": -10, "z": 4, "heading": 3, "subtasks": [{ "type": 0, "parameters": "5.6" }] }, + { "x": 20, "y": 10, "z": 3, "heading": 1 } + ], + "terminal_action": 0 + } + ] +} diff --git a/json_examples/missions/two_drones.json b/json_examples/missions/two_drones.json index 871bcda..ff71627 100644 --- a/json_examples/missions/two_drones.json +++ b/json_examples/missions/two_drones.json @@ -1,27 +1,65 @@ { - "mission": [ - { - "robot_name": "uav1", - "frame_id": 0, - "height_id": 0, - "points": [ - { "x": 20, "y": 10, "z": 3, "heading": 1 }, - { "x": -20, "y": 10, "z": 4, "heading": 3 }, - { "x": 10, "y": -10, "z": 2, "heading": 2 }, - { "x": -20, "y": -20, "z": 4, "heading": 3 }, - { "x": 10, "y": 20, "z": 2, "heading": 2 } - ], - "terminal_action": 0 - }, - { - "robot_name": "uav2", - "frame_id": 0, - "height_id": 0, - "points": [ - { "x": 30, "y": 5, "z": 3, "heading": 0 }, - { "x": 40, "y": 5, "z": 3, "heading": 0 } - ], - "terminal_action": 0 - } - ] + "type": "WaypointPlanner", + "details": { + "robots": [ + { + "name": "uav1", + "frame_id": 0, + "height_id": 0, + "points": [ + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1 + }, + { + "x": -20, + "y": 10, + "z": 4, + "heading": 3 + }, + { + "x": 10, + "y": -10, + "z": 2, + "heading": 2 + }, + { + "x": -20, + "y": -20, + "z": 4, + "heading": 3 + }, + { + "x": 10, + "y": 20, + "z": 2, + "heading": 2 + } + ], + "terminal_action": 0 + }, + { + "name": "uav2", + "frame_id": 0, + "height_id": 0, + "points": [ + { + "x": 30, + "y": 5, + "z": 3, + "heading": 0 + }, + { + "x": 40, + "y": 5, + "z": 3, + "heading": 0 + } + ], + "terminal_action": 0 + } + ] + } } diff --git a/launch/iroc_bridge.launch b/launch/iroc_bridge.launch index adb9e1d..731b8c7 100644 --- a/launch/iroc_bridge.launch +++ b/launch/iroc_bridge.launch @@ -67,7 +67,7 @@ - + diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index cd859a8..6631004 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -76,17 +76,8 @@ using vec4_t = Eigen::Vector4d; using namespace actionlib; using FleetManagerActionClient = SimpleActionClient; - -typedef SimpleActionClient WaypointFleetManagerClient; -typedef SimpleActionClient CoveragePlannerClient; -typedef SimpleActionClient AutonomyTestClient; //Waypoint mission goal typedef iroc_fleet_manager::FleetManagerGoal FleetManagerGoal; -// Autonomy test goal -typedef iroc_fleet_manager::AutonomyTestGoal AutonomyTestActionServerGoal; -// Coverage mission goal -typedef iroc_fleet_manager::CoverageMissionGoal coverageMissionActionServerGoal; - typedef mrs_robot_diagnostics::robot_type_t robot_type_t; /* class IROCBridge //{ */ @@ -201,9 +192,6 @@ class IROCBridge : public nodelet::Nodelet { // | ----------------------- ROS Clients ----------------------- | ros::ServiceClient sc_change_fleet_mission_state; ros::ServiceClient sc_change_robot_mission_state; - ros::ServiceClient sc_change_coverage_mission_state; - ros::ServiceClient sc_change_coverage_mission_robot_state; - ros::ServiceClient sc_change_autonomy_test_state; std::string latest_mission_type_; // | ----------------- action client callbacks ---------------- | @@ -242,9 +230,7 @@ class IROCBridge : public nodelet::Nodelet { crow::response setOriginCallback(const crow::request& req); crow::response setSafetyBorderCallback(const crow::request& req); crow::response setObstacleCallback(const crow::request& req); - crow::response missionCallback(const crow::request& req, const std::string& type); - crow::response coveragemissionCallback(const crow::request& req); - crow::response autonomyTestCallback(const crow::request& req); + crow::response missionCallback(const crow::request& req); crow::response changeFleetMissionStateCallback(const crow::request& req, const std::string& type); crow::response changeRobotMissionStateCallback(const crow::request& req, const std::string& robot_name, const std::string& type); @@ -354,7 +340,7 @@ void IROCBridge::onInit() { CROW_ROUTE(http_srv_, "/safety-area/origin").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return setOriginCallback(req); }); CROW_ROUTE(http_srv_, "/safety-area/borders").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return setSafetyBorderCallback(req); }); CROW_ROUTE(http_srv_, "/safety-area/obstacles").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return setObstacleCallback(req); }); - CROW_ROUTE(http_srv_, "/mission/create/").methods(crow::HTTPMethod::Post)([this](const crow::request& req, const std::string& type){ return missionCallback(req, type); }); + CROW_ROUTE(http_srv_, "/mission").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return missionCallback(req); }); // Missions //TODO: CROW_REGEX_ROUTE(http_srv_, R"(/fleet/mission/(start|stop|pause))") @@ -493,18 +479,6 @@ void IROCBridge::onInit() { ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/change_robot_mission_state\' -> \'%s\'", sc_change_robot_mission_state.getService().c_str()); - sc_change_coverage_mission_state = nh_.serviceClient(nh_.resolveName("svc/change_coverage_mission_state")); - ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/change_coverage_mission_state\' -> \'%s\'", - sc_change_coverage_mission_state.getService().c_str()); - - sc_change_coverage_mission_robot_state = nh_.serviceClient(nh_.resolveName("svc/change_coverage_mission_robot_state")); - ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/change_robot_mission_state\' -> \'%s\'", - sc_change_coverage_mission_state.getService().c_str()); - - sc_change_autonomy_test_state = nh_.serviceClient(nh_.resolveName("svc/change_autonomy_test_state")); - ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/change_autonomy_test_state\' -> \'%s\'", - sc_change_autonomy_test_state.getService().c_str()); - /* // | --------------------- action clients --------------------- | */ //Mission action client @@ -968,9 +942,6 @@ ros::ServiceClient IROCBridge::getServiceClient(const IROCBridge::Change_SvC_T s switch (service_type) { case Change_SvC_T::FleetWaypoint: return sc_change_fleet_mission_state; case Change_SvC_T::RobotWaypoint: return sc_change_robot_mission_state; - case Change_SvC_T::FleetCoverage: return sc_change_coverage_mission_state; - case Change_SvC_T::RobotCoverage: return sc_change_coverage_mission_robot_state; - case Change_SvC_T::RobotAutonomyTest: return sc_change_autonomy_test_state; default: return ros::ServiceClient(); } } @@ -1108,23 +1079,13 @@ json resultToJson(const boost::shared_ptr& result) { /* successMissionJson() method //{ */ -template -json successMissionJson(std::vector mission_robots) { - json robot_results = json::list(); - - for (size_t i = 0; i < mission_robots.size(); i++) { - robot_results[i] = { - {"robot_name", mission_robots[i].name}, - {"success", true }, - {"message", "Robot received the mission successfully"} - }; - } +json successMissionJson() { // Create the main JSON object json json_msg = { {"success", true}, {"message", "Mission uploaded successfully"}, - {"robot_results", robot_results} + {"robot_results", "All robots received their mission"} }; return json_msg; @@ -1422,95 +1383,16 @@ crow::response IROCBridge::setObstacleCallback(const crow::request& req) * \param req Crow request * \return res Crow response */ -crow::response IROCBridge::missionCallback(const crow::request& req, const std::string& type) +crow::response IROCBridge::missionCallback(const crow::request& req) { ROS_INFO_STREAM("[IROCBridge]: Parsing a missionCallback message JSON -> ROS."); latest_mission_type_ = "waypoint"; try { crow::json::rvalue json_msg = crow::json::load(req.body); - if (!json_msg || !json_msg.has("mission") || json_msg["mission"].t() != crow::json::type::List) - return crow::response(crow::status::BAD_REQUEST, "{\"message\": \"Bad request: Failed to parse JSON or missing 'mission' key\"}"); - - // Iterate over each robot in the mission - std::vector mission_robots; - for (const auto& mission_json : json_msg["mission"].lo()) { - std::scoped_lock lck(robot_handlers_.mtx); - - std::string robot_name = mission_json["robot_name"].s(); - auto* rh_ptr = findRobotHandler(robot_name, robot_handlers_); - if (!rh_ptr) { - ROS_WARN_STREAM("[IROCBridge]: Robot \"" << robot_name << "\" not found. Ignoring."); - return crow::response(crow::status::NOT_FOUND, "{\"message\": \"Robot '" + robot_name + "' not found\"}"); - } - - // Process the action request for each robot - iroc_mission_handler::MissionGoal mission_robot; - mission_robot.name = robot_name; - mission_robot.frame_id = mission_json["frame_id"].i(); - mission_robot.height_id = mission_json["height_id"].i(); - mission_robot.terminal_action = mission_json["terminal_action"].i(); - - // Check robot type - bool use_z; - auto robot_type = rh_ptr->sh_general_robot_info.getMsg()->robot_type; - switch (robot_type) { - case static_cast(robot_type_t::MULTIROTOR): { - ROS_INFO("[IROCBridge]: MULTIROTOR TYPE: "); - use_z = true; - break; - }; - case static_cast(robot_type_t::BOAT): { - ROS_INFO("[IROCBridge]: BOAT TYPE: "); - use_z = false; - break; - }; - default: - ROS_WARN_STREAM("[IROCBridge]: Unknown robot type for robot \"" << robot_name << "\". Ignoring."); - return crow::response(crow::status::BAD_REQUEST, "{\"message\": \"Unknown robot type for robot '" + robot_name + "'\"}"); - break; - } + if (!json_msg || !json_msg.has("type") || !json_msg.has("details")) + return crow::response(crow::status::BAD_REQUEST, "{\"message\": \"Bad request: Failed to parse JSON or missing 'type' and 'details' keys\"}"); - // Process the points in the mission - std::vector points = mission_json["points"].lo(); - - std::vector ref_points; - ref_points.reserve(points.size()); - for (const auto& point : points) { - mrs_msgs::Reference ref; - ref.position.x = point["x"].d(); - ref.position.y = point["y"].d(); - if (use_z) - ref.position.z = point["z"].d(); - ref.heading = point["heading"].d(); - - iroc_mission_handler::Waypoint waypoint; - waypoint.reference_point = ref; - if (point.has("subtasks")) { - std::vector subtasks; - for (const auto& subtask : point["subtasks"].lo()) { - iroc_mission_handler::Subtask subtask_obj; - subtask_obj.type = subtask["type"].i(); - if (subtask.has("parameters")) { - subtask_obj.parameters = subtask["parameters"].s(); - } - subtasks.push_back(subtask_obj); - } - waypoint.subtasks = subtasks; - } - ref_points.push_back(waypoint); - } - mission_robot.points = ref_points; - - // Debugging/logging - for (const auto& point : ref_points) { - ROS_INFO_STREAM("[IROCBridge]: Robot " << robot_name << " point: " << point.reference_point.position.x << ", " << point.reference_point.position.y << ", " << point.reference_point.position.z); - } - - // Add the mission to the list of missions - mission_robots.push_back(mission_robot); - } - // Validate if the action client is connected and if the action is already running if (!fleet_manager_action_client_->isServerConnected()) { ROS_WARN_STREAM("[IROCBridge]: Action server is not connected. Check the iroc_fleet_manager node."); @@ -1525,8 +1407,14 @@ crow::response IROCBridge::missionCallback(const crow::request& req, const std:: // Send the action goal to the fleet manager FleetManagerGoal action_goal; - // TODO update with fleet manager new action - // action_goal.robots = mission_robots; + std::string type = json_msg["type"].s(); + + // Convert rvalue to wvalue, then dump to string + crow::json::wvalue details_wvalue(json_msg["details"]); + std::string details = details_wvalue.dump(); + + action_goal.type = type; + action_goal.details = details; fleet_manager_action_client_->sendGoal( action_goal, @@ -1542,7 +1430,7 @@ crow::response IROCBridge::missionCallback(const crow::request& req, const std:: ); // Waiting in the case the trajectories are rejected. We can better wait will the state is pending - ros::Duration(mission_robots.size() * 1.0).sleep(); + ros::Duration(5).sleep(); if (fleet_manager_action_client_->getState().isDone()) { // If the action is done, the action finished instantly auto result = fleet_manager_action_client_->getResult(); @@ -1553,7 +1441,8 @@ crow::response IROCBridge::missionCallback(const crow::request& req, const std:: } else { ROS_INFO("[IROCBridge]: Mission received successfully"); - auto json = successMissionJson(mission_robots); + // TODO to replace with proper response, in ROS2 + auto json = successMissionJson(); return crow::response(crow::status::CREATED, json); } } From 04b01fcd71d0ac56691d9022e040ac11d1304367 Mon Sep 17 00:00:00 2001 From: marlonriv Date: Mon, 4 Aug 2025 17:14:12 +0200 Subject: [PATCH 03/33] Removed unnecessary routines As we are only having a centralized fleet manager, we don't need specific mission callbacks anymore. --- launch/iroc_bridge.launch | 3 --- src/iroc_bridge.cpp | 57 ++++++--------------------------------- 2 files changed, 8 insertions(+), 52 deletions(-) diff --git a/launch/iroc_bridge.launch b/launch/iroc_bridge.launch index 731b8c7..b6b2ed0 100644 --- a/launch/iroc_bridge.launch +++ b/launch/iroc_bridge.launch @@ -70,9 +70,6 @@ - - - diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index 6631004..c6e75ba 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -138,18 +138,6 @@ class IROCBridge : public nodelet::Nodelet { {"set_obstacle", CommandType::Set_Obstacle}, }; - std::map change_type_map_ = { - {"waypoint", Change_SvC_T::FleetWaypoint}, - {"coverage", Change_SvC_T::FleetCoverage}, - }; - - std::map change_robot_type_map_ = { - {"waypoint", Change_SvC_T::RobotWaypoint}, - {"coverage", Change_SvC_T::RobotCoverage}, - {"autonomy_test", Change_SvC_T::RobotAutonomyTest}, - }; - - // | ---------------------- ROS parameters ------------------ | double max_linear_speed_; double max_heading_rate_; @@ -190,9 +178,8 @@ class IROCBridge : public nodelet::Nodelet { void timerMain(const ros::TimerEvent& event); // | ----------------------- ROS Clients ----------------------- | - ros::ServiceClient sc_change_fleet_mission_state; - ros::ServiceClient sc_change_robot_mission_state; - std::string latest_mission_type_; + ros::ServiceClient sc_change_fleet_mission_state_; + ros::ServiceClient sc_change_robot_mission_state_; // | ----------------- action client callbacks ---------------- | @@ -471,13 +458,13 @@ void IROCBridge::onInit() { } } - sc_change_fleet_mission_state = nh_.serviceClient(nh_.resolveName("svc/change_fleet_mission_state")); + sc_change_fleet_mission_state_ = nh_.serviceClient(nh_.resolveName("svc/change_fleet_mission_state")); ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/change_fleet_mission_state\' -> \'%s\'", - sc_change_fleet_mission_state.getService().c_str()); + sc_change_fleet_mission_state_.getService().c_str()); - sc_change_robot_mission_state = nh_.serviceClient(nh_.resolveName("svc/change_robot_mission_state")); + sc_change_robot_mission_state_ = nh_.serviceClient(nh_.resolveName("svc/change_robot_mission_state")); ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/change_robot_mission_state\' -> \'%s\'", - sc_change_robot_mission_state.getService().c_str()); + sc_change_robot_mission_state_.getService().c_str()); /* // | --------------------- action clients --------------------- | */ @@ -938,13 +925,6 @@ ros::ServiceClient* IROCBridge::getServiceClient(IROCBridge::robot_handler_t* rh } } -ros::ServiceClient IROCBridge::getServiceClient(const IROCBridge::Change_SvC_T service_type) { - switch (service_type) { - case Change_SvC_T::FleetWaypoint: return sc_change_fleet_mission_state; - case Change_SvC_T::RobotWaypoint: return sc_change_robot_mission_state; - default: return ros::ServiceClient(); - } -} //} /* commandAction() method //{ */ @@ -1386,7 +1366,6 @@ crow::response IROCBridge::setObstacleCallback(const crow::request& req) crow::response IROCBridge::missionCallback(const crow::request& req) { ROS_INFO_STREAM("[IROCBridge]: Parsing a missionCallback message JSON -> ROS."); - latest_mission_type_ = "waypoint"; try { crow::json::rvalue json_msg = crow::json::load(req.body); @@ -1472,17 +1451,7 @@ crow::response IROCBridge::changeFleetMissionStateCallback(const crow::request& mrs_msgs::String ros_srv; ros_srv.request.value = type; - ros::ServiceClient service_client; - auto it = change_type_map_.find(latest_mission_type_); - if (it != change_type_map_.end()) { - service_client = getServiceClient(it->second); - } else { - std::string msg = "No active mission."; - return crow::response(crow::status::BAD_REQUEST, - "{\"message\": \"" + msg + "\"}"); - } - - const auto resp = callService(service_client, ros_srv.request); + const auto resp = callService(sc_change_fleet_mission_state_, ros_srv.request); if (!resp.success) return crow::response(crow::status::INTERNAL_SERVER_ERROR, "{\"message\": \"" + resp.message + "\"}"); else @@ -1518,17 +1487,7 @@ crow::response IROCBridge::changeRobotMissionStateCallback(const crow::request& ros_srv.request.robot_name = robot_name; ros_srv.request.type = type; - ros::ServiceClient service_client; - auto it = change_robot_type_map_.find(latest_mission_type_); - if (it != change_robot_type_map_.end()) { - service_client = getServiceClient(it->second); - } else { - std::string msg = "No active mission."; - return crow::response(crow::status::BAD_REQUEST, - "{\"message\": \"" + msg + "\"}"); - } - - const auto resp = callService(service_client, ros_srv.request); + const auto resp = callService(sc_change_robot_mission_state_, ros_srv.request); if (!resp.success) { json_msg["message"] = "Call was not successful with message: " + resp.message; ROS_WARN_STREAM("[IROCBridge]: " << json_msg["message"].dump()); From 8630e7f37d98648f0235b4b2b259aed89896d843 Mon Sep 17 00:00:00 2001 From: marlonriv Date: Tue, 5 Aug 2025 17:57:35 +0200 Subject: [PATCH 04/33] Added common handlers Handlers to share with fleet manager and planners --- include/iroc_bridge/common_handlers.h | 47 +++++++++++++++++ src/iroc_bridge.cpp | 75 ++++++++++++++++++++++----- 2 files changed, 110 insertions(+), 12 deletions(-) create mode 100644 include/iroc_bridge/common_handlers.h diff --git a/include/iroc_bridge/common_handlers.h b/include/iroc_bridge/common_handlers.h new file mode 100644 index 0000000..ce4f21e --- /dev/null +++ b/include/iroc_bridge/common_handlers.h @@ -0,0 +1,47 @@ +#ifndef IROC_BRIDGE_COMMON_HANDLERS_H +#define IROC_BRIDGE_COMMON_HANDLERS_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace iroc_bridge { + +// Note: ConstPtr type is a typedef for a shared pointer, +// specifically boost::shared_ptr in ROS 1 + +struct CommonRobotHandler_t { + std::string robot_name; + mrs_robot_diagnostics::GeneralRobotInfo::ConstPtr + general_robot_info; + mrs_robot_diagnostics::StateEstimationInfo::ConstPtr + state_estimation_info; + mrs_robot_diagnostics::ControlInfo::ConstPtr control_info; + mrs_robot_diagnostics::CollisionAvoidanceInfo::ConstPtr + collision_avoidance_info; + mrs_robot_diagnostics::UavInfo::ConstPtr uav_info; + mrs_robot_diagnostics::SystemHealthInfo::ConstPtr + system_health_info; +}; + +struct CommonRobotHandlers_t { + std::unordered_map handlers_map; + +}; + +struct CommonHandlers_t { + CommonRobotHandlers_t robot_handlers; + ros::NodeHandle parent_nh; +}; + +} // namespace iroc_bridge + +#endif // IROC_BRIDGE_COMMON_HANDLERS_H diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index c6e75ba..63edfff 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -63,6 +63,8 @@ #include #include + +#include //} namespace iroc_bridge @@ -172,6 +174,8 @@ class IROCBridge : public nodelet::Nodelet { std::vector handlers; } robot_handlers_; + CommonRobotHandlers_t common_robot_handlers_; + // | ----------------------- main timer ----------------------- | ros::Timer timer_main_; @@ -247,6 +251,10 @@ class IROCBridge : public nodelet::Nodelet { // Latlon origin mrs_msgs::Point2D world_origin_; + + + // contains handlers that are shared with fleet manager and planners + std::shared_ptr common_handlers_; }; //} @@ -266,6 +274,13 @@ void IROCBridge::onInit() { /* waits for the ROS to publish clock */ ros::Time::waitForValid(); + + // -------------------------------------------------------------- + // | common handler for fleet manager and planners | + // -------------------------------------------------------------- + + common_handlers_ = std::make_shared(); + /* load parameters */ mrs_lib::ParamLoader param_loader(nh_, "IROCBridge"); @@ -402,9 +417,14 @@ void IROCBridge::onInit() { std::scoped_lock lck(robot_handlers_.mtx); robot_handlers_.handlers.reserve(filtered_robot_names.size()); + common_robot_handlers_.handlers_map.reserve(filtered_robot_names.size()); for (const auto& robot_name : filtered_robot_names) { robot_handler_t robot_handler; + // To share with fleet manager and planners + CommonRobotHandler_t common_robot_handler; robot_handler.robot_name = robot_name; + // Saving only the robot names to fill up the rest during the parsing of the messages + common_robot_handler.robot_name = robot_name; const std::string general_robot_info_topic_name = "/" + robot_name + nh_.resolveName("in/general_robot_info"); robot_handler.sh_general_robot_info = mrs_lib::SubscribeHandler(shopts, general_robot_info_topic_name); @@ -455,6 +475,7 @@ void IROCBridge::onInit() { // move is necessary because copy construction of the subscribe handlers is deleted due to mutexes robot_handlers_.handlers.emplace_back(std::move(robot_handler)); + common_robot_handlers_.handlers_map[robot_name] = std::move(common_robot_handler); } } @@ -494,27 +515,56 @@ void IROCBridge::onInit() { void IROCBridge::timerMain([[maybe_unused]] const ros::TimerEvent& event) { std::scoped_lock lck(robot_handlers_.mtx); + // Parsing the messages to send in telemetry and populating + // the common handler with the messages for (auto& rh : robot_handlers_.handlers) { const auto& robot_name = rh.robot_name; - if (rh.sh_general_robot_info.newMsg()) - parseGeneralRobotInfo(rh.sh_general_robot_info.getMsg(), robot_name); + if (rh.sh_general_robot_info.newMsg()) { + const auto general_robot_info_msg = rh.sh_general_robot_info.getMsg(); + parseGeneralRobotInfo(general_robot_info_msg, robot_name); + common_robot_handlers_.handlers_map[robot_name].general_robot_info = + std::move(general_robot_info_msg); + } - if (rh.sh_state_estimation_info.newMsg()) - parseStateEstimationInfo(rh.sh_state_estimation_info.getMsg(), robot_name); + if (rh.sh_state_estimation_info.newMsg()) { + const auto state_estimation_info_msg = + rh.sh_state_estimation_info.getMsg(); + parseStateEstimationInfo(state_estimation_info_msg, robot_name); + common_robot_handlers_.handlers_map[robot_name].state_estimation_info = + std::move(state_estimation_info_msg); + } - if (rh.sh_control_info.newMsg()) - parseControlInfo(rh.sh_control_info.getMsg(), robot_name); + if (rh.sh_control_info.newMsg()) { + const auto control_info_msg = rh.sh_control_info.getMsg(); + parseControlInfo(control_info_msg,robot_name); + common_robot_handlers_.handlers_map[robot_name].control_info = + std::move(control_info_msg); + } - if (rh.sh_collision_avoidance_info.newMsg()) - parseCollisionAvoidanceInfo(rh.sh_collision_avoidance_info.getMsg(), robot_name); + if (rh.sh_collision_avoidance_info.newMsg()) { + const auto collision_avoidance_info_msg = + rh.sh_collision_avoidance_info.getMsg(); + parseCollisionAvoidanceInfo(collision_avoidance_info_msg, robot_name); + common_robot_handlers_.handlers_map[robot_name].collision_avoidance_info = + std::move(collision_avoidance_info_msg); + } - if (rh.sh_uav_info.newMsg()) - parseUavInfo(rh.sh_uav_info.getMsg(), robot_name); + if (rh.sh_uav_info.newMsg()) { + const auto uav_info_msg = rh.sh_uav_info.getMsg(); + parseUavInfo(uav_info_msg, robot_name); + common_robot_handlers_.handlers_map[robot_name].uav_info = + std::move(uav_info_msg); + } - if (rh.sh_system_health_info.newMsg()) - parseSystemHealthInfo(rh.sh_system_health_info.getMsg(), robot_name); + if (rh.sh_system_health_info.newMsg()) { + const auto system_health_info = rh.sh_system_health_info.getMsg(); + parseSystemHealthInfo(system_health_info, robot_name); + common_robot_handlers_.handlers_map[robot_name].system_health_info = + std::move(system_health_info); + } } + } //} @@ -630,6 +680,7 @@ template /* parseGeneralRobotInfo() //{ */ void IROCBridge::parseGeneralRobotInfo(mrs_robot_diagnostics::GeneralRobotInfo::ConstPtr general_robot_info, const std::string& robot_name) { + json json_msg = { {"robot_name", general_robot_info->robot_name}, {"robot_type", general_robot_info->robot_type}, From d1fa46da8d293ba948ed2f49b00eccb2dff3c38f Mon Sep 17 00:00:00 2001 From: marlonriv Date: Thu, 7 Aug 2025 10:14:04 +0200 Subject: [PATCH 05/33] Removed common handlers Will be added instead into fleet manager --- include/iroc_bridge/common_handlers.h | 47 -------------------- src/iroc_bridge.cpp | 62 +++++---------------------- 2 files changed, 11 insertions(+), 98 deletions(-) delete mode 100644 include/iroc_bridge/common_handlers.h diff --git a/include/iroc_bridge/common_handlers.h b/include/iroc_bridge/common_handlers.h deleted file mode 100644 index ce4f21e..0000000 --- a/include/iroc_bridge/common_handlers.h +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef IROC_BRIDGE_COMMON_HANDLERS_H -#define IROC_BRIDGE_COMMON_HANDLERS_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace iroc_bridge { - -// Note: ConstPtr type is a typedef for a shared pointer, -// specifically boost::shared_ptr in ROS 1 - -struct CommonRobotHandler_t { - std::string robot_name; - mrs_robot_diagnostics::GeneralRobotInfo::ConstPtr - general_robot_info; - mrs_robot_diagnostics::StateEstimationInfo::ConstPtr - state_estimation_info; - mrs_robot_diagnostics::ControlInfo::ConstPtr control_info; - mrs_robot_diagnostics::CollisionAvoidanceInfo::ConstPtr - collision_avoidance_info; - mrs_robot_diagnostics::UavInfo::ConstPtr uav_info; - mrs_robot_diagnostics::SystemHealthInfo::ConstPtr - system_health_info; -}; - -struct CommonRobotHandlers_t { - std::unordered_map handlers_map; - -}; - -struct CommonHandlers_t { - CommonRobotHandlers_t robot_handlers; - ros::NodeHandle parent_nh; -}; - -} // namespace iroc_bridge - -#endif // IROC_BRIDGE_COMMON_HANDLERS_H diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index 63edfff..20627ea 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -64,7 +64,6 @@ #include #include -#include //} namespace iroc_bridge @@ -174,8 +173,6 @@ class IROCBridge : public nodelet::Nodelet { std::vector handlers; } robot_handlers_; - CommonRobotHandlers_t common_robot_handlers_; - // | ----------------------- main timer ----------------------- | ros::Timer timer_main_; @@ -252,10 +249,7 @@ class IROCBridge : public nodelet::Nodelet { // Latlon origin mrs_msgs::Point2D world_origin_; - - // contains handlers that are shared with fleet manager and planners - std::shared_ptr common_handlers_; -}; + }; //} /* onInit() //{ */ @@ -274,13 +268,6 @@ void IROCBridge::onInit() { /* waits for the ROS to publish clock */ ros::Time::waitForValid(); - - // -------------------------------------------------------------- - // | common handler for fleet manager and planners | - // -------------------------------------------------------------- - - common_handlers_ = std::make_shared(); - /* load parameters */ mrs_lib::ParamLoader param_loader(nh_, "IROCBridge"); @@ -417,15 +404,11 @@ void IROCBridge::onInit() { std::scoped_lock lck(robot_handlers_.mtx); robot_handlers_.handlers.reserve(filtered_robot_names.size()); - common_robot_handlers_.handlers_map.reserve(filtered_robot_names.size()); for (const auto& robot_name : filtered_robot_names) { robot_handler_t robot_handler; // To share with fleet manager and planners - CommonRobotHandler_t common_robot_handler; robot_handler.robot_name = robot_name; // Saving only the robot names to fill up the rest during the parsing of the messages - common_robot_handler.robot_name = robot_name; - const std::string general_robot_info_topic_name = "/" + robot_name + nh_.resolveName("in/general_robot_info"); robot_handler.sh_general_robot_info = mrs_lib::SubscribeHandler(shopts, general_robot_info_topic_name); @@ -475,7 +458,6 @@ void IROCBridge::onInit() { // move is necessary because copy construction of the subscribe handlers is deleted due to mutexes robot_handlers_.handlers.emplace_back(std::move(robot_handler)); - common_robot_handlers_.handlers_map[robot_name] = std::move(common_robot_handler); } } @@ -512,59 +494,37 @@ void IROCBridge::onInit() { // -------------------------------------------------------------- /* timerMain() //{ */ -void IROCBridge::timerMain([[maybe_unused]] const ros::TimerEvent& event) { +void IROCBridge::timerMain([[maybe_unused]] const ros::TimerEvent &event) { std::scoped_lock lck(robot_handlers_.mtx); - // Parsing the messages to send in telemetry and populating - // the common handler with the messages - for (auto& rh : robot_handlers_.handlers) { - const auto& robot_name = rh.robot_name; + // Parsing the messages to send in telemetry + for (auto &rh : robot_handlers_.handlers) { + const auto &robot_name = rh.robot_name; if (rh.sh_general_robot_info.newMsg()) { - const auto general_robot_info_msg = rh.sh_general_robot_info.getMsg(); - parseGeneralRobotInfo(general_robot_info_msg, robot_name); - common_robot_handlers_.handlers_map[robot_name].general_robot_info = - std::move(general_robot_info_msg); + parseGeneralRobotInfo(rh.sh_general_robot_info.getMsg(), robot_name); } if (rh.sh_state_estimation_info.newMsg()) { - const auto state_estimation_info_msg = - rh.sh_state_estimation_info.getMsg(); - parseStateEstimationInfo(state_estimation_info_msg, robot_name); - common_robot_handlers_.handlers_map[robot_name].state_estimation_info = - std::move(state_estimation_info_msg); + parseStateEstimationInfo(rh.sh_state_estimation_info.getMsg(), robot_name); } if (rh.sh_control_info.newMsg()) { - const auto control_info_msg = rh.sh_control_info.getMsg(); - parseControlInfo(control_info_msg,robot_name); - common_robot_handlers_.handlers_map[robot_name].control_info = - std::move(control_info_msg); + parseControlInfo(rh.sh_control_info.getMsg(),robot_name); } if (rh.sh_collision_avoidance_info.newMsg()) { - const auto collision_avoidance_info_msg = - rh.sh_collision_avoidance_info.getMsg(); - parseCollisionAvoidanceInfo(collision_avoidance_info_msg, robot_name); - common_robot_handlers_.handlers_map[robot_name].collision_avoidance_info = - std::move(collision_avoidance_info_msg); + parseCollisionAvoidanceInfo(rh.sh_collision_avoidance_info.getMsg(), robot_name); } if (rh.sh_uav_info.newMsg()) { - const auto uav_info_msg = rh.sh_uav_info.getMsg(); - parseUavInfo(uav_info_msg, robot_name); - common_robot_handlers_.handlers_map[robot_name].uav_info = - std::move(uav_info_msg); + parseUavInfo(rh.sh_uav_info.getMsg(), robot_name); } if (rh.sh_system_health_info.newMsg()) { - const auto system_health_info = rh.sh_system_health_info.getMsg(); - parseSystemHealthInfo(system_health_info, robot_name); - common_robot_handlers_.handlers_map[robot_name].system_health_info = - std::move(system_health_info); + parseSystemHealthInfo(rh.sh_system_health_info.getMsg(), robot_name); } } - } //} From 565dd652d9994f50cd0efd18deafb1b68ecb7d8b Mon Sep 17 00:00:00 2001 From: marlonriv Date: Tue, 12 Aug 2025 16:35:01 +0200 Subject: [PATCH 06/33] Formatting --- src/iroc_bridge.cpp | 721 +++++++++++++++++++------------------------- 1 file changed, 302 insertions(+), 419 deletions(-) diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index 20627ea..ce6c8cc 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -66,10 +66,9 @@ //} -namespace iroc_bridge -{ +namespace iroc_bridge { -using json = crow::json::wvalue; +using json = crow::json::wvalue; using vec3_t = Eigen::Vector3d; using vec4_t = Eigen::Vector4d; @@ -77,57 +76,39 @@ using vec4_t = Eigen::Vector4d; using namespace actionlib; using FleetManagerActionClient = SimpleActionClient; -//Waypoint mission goal +// Waypoint mission goal typedef iroc_fleet_manager::FleetManagerGoal FleetManagerGoal; typedef mrs_robot_diagnostics::robot_type_t robot_type_t; /* class IROCBridge //{ */ class IROCBridge : public nodelet::Nodelet { -public: + public: virtual void onInit(); -private: + private: ros::NodeHandle nh_; // | ---------------------- HTTP REST API --------------------- | - std::thread th_http_srv_; + std::thread th_http_srv_; crow::App http_srv_; std::unique_ptr http_client_; - - struct result_t - { - bool success; + struct result_t { + bool success; std::string message; }; - struct action_result_t - { - bool success; - std::string message; - crow::status status_code; + struct action_result_t { + bool success; + std::string message; + crow::status status_code; }; // | ---------------------- Command types --------------------- | - enum class CommandType { - Takeoff, - Land, - Hover, - Home, - Set_Origin, - Set_SafetyBorder, - Set_Obstacle, - Unknown - }; + enum class CommandType { Takeoff, Land, Hover, Home, Set_Origin, Set_SafetyBorder, Set_Obstacle, Unknown }; - enum class Change_SvC_T { - FleetWaypoint, - RobotWaypoint, - FleetCoverage, - RobotCoverage, - RobotAutonomyTest - }; + enum class Change_SvC_T { FleetWaypoint, RobotWaypoint, FleetCoverage, RobotCoverage, RobotAutonomyTest }; std::map command_type_map_ = { {"takeoff", CommandType::Takeoff}, @@ -145,15 +126,14 @@ class IROCBridge : public nodelet::Nodelet { // | ---------------------- ROS subscribers --------------------- | - struct robot_handler_t - { - std::string robot_name; - mrs_lib::SubscribeHandler sh_general_robot_info; - mrs_lib::SubscribeHandler sh_state_estimation_info; - mrs_lib::SubscribeHandler sh_control_info; + struct robot_handler_t { + std::string robot_name; + mrs_lib::SubscribeHandler sh_general_robot_info; + mrs_lib::SubscribeHandler sh_state_estimation_info; + mrs_lib::SubscribeHandler sh_control_info; mrs_lib::SubscribeHandler sh_collision_avoidance_info; - mrs_lib::SubscribeHandler sh_uav_info; - mrs_lib::SubscribeHandler sh_system_health_info; + mrs_lib::SubscribeHandler sh_uav_info; + mrs_lib::SubscribeHandler sh_system_health_info; ros::ServiceClient sc_takeoff; ros::ServiceClient sc_land; @@ -167,16 +147,15 @@ class IROCBridge : public nodelet::Nodelet { ros::Publisher pub_path; }; - struct robot_handlers_t - { - std::recursive_mutex mtx; + struct robot_handlers_t { + std::recursive_mutex mtx; std::vector handlers; } robot_handlers_; // | ----------------------- main timer ----------------------- | ros::Timer timer_main_; - void timerMain(const ros::TimerEvent& event); + void timerMain(const ros::TimerEvent& event); // | ----------------------- ROS Clients ----------------------- | ros::ServiceClient sc_change_fleet_mission_state_; @@ -188,8 +167,8 @@ class IROCBridge : public nodelet::Nodelet { void missionActiveCallback(); template void missionDoneCallback(const SimpleClientGoalState& state, const boost::shared_ptr& result); - template - void missionFeedbackCallback(const boost::shared_ptr& feedback); + template + void missionFeedbackCallback(const boost::shared_ptr& feedback); // | ------------------ Additional functions ------------------ | @@ -200,17 +179,16 @@ class IROCBridge : public nodelet::Nodelet { void parseUavInfo(mrs_robot_diagnostics::UavInfo::ConstPtr uav_info, const std::string& robot_name); void parseSystemHealthInfo(mrs_robot_diagnostics::SystemHealthInfo::ConstPtr uav_info, const std::string& robot_name); - void sendJsonMessage(const std::string& msg_type,json& json_msg); - void sendTelemetryJsonMessage(const std::string& type, json& json_msg); - robot_handler_t* findRobotHandler(const std::string& robot_name, robot_handlers_t& robot_handlers); + void sendJsonMessage(const std::string& msg_type, json& json_msg); + void sendTelemetryJsonMessage(const std::string& type, json& json_msg); + robot_handler_t* findRobotHandler(const std::string& robot_name, robot_handlers_t& robot_handlers); ros::ServiceClient* getServiceClient(IROCBridge::robot_handler_t* rh_ptr, const IROCBridge::CommandType command_type); - ros::ServiceClient getServiceClient(const IROCBridge::Change_SvC_T service_type); - + ros::ServiceClient getServiceClient(const IROCBridge::Change_SvC_T service_type); action_result_t commandAction(const std::vector& robot_names, const std::string& command_type); - template + template action_result_t commandAction(const std::vector& robot_names, const std::string& command_type, typename Svc_T::Request req); // REST API callbacks @@ -224,7 +202,7 @@ class IROCBridge : public nodelet::Nodelet { crow::response changeRobotMissionStateCallback(const crow::request& req, const std::string& robot_name, const std::string& type); crow::response availableRobotsCallback(const crow::request& req); - crow::response commandCallback(const crow::request& req, const std::string & command_type , std::optional robot_name); + crow::response commandCallback(const crow::request& req, const std::string& command_type, std::optional robot_name); // Websocket callbacks void remoteControlCallback(crow::websocket::connection& conn, const std::string& data, bool is_binary); @@ -240,16 +218,15 @@ class IROCBridge : public nodelet::Nodelet { std::thread th_death_check_; std::thread th_telemetry_check_; - void routine_death_check(); - crow::websocket::connection* active_telemetry_connection_ = nullptr; + void routine_death_check(); + crow::websocket::connection* active_telemetry_connection_ = nullptr; std::mutex mtx_telemetry_connections_; - + std::unique_ptr fleet_manager_action_client_; // Latlon origin - mrs_msgs::Point2D world_origin_; - - }; + mrs_msgs::Point2D world_origin_; +}; //} /* onInit() //{ */ @@ -288,21 +265,21 @@ void IROCBridge::onInit() { param_loader.addYamlFile(network_config_path); } - const auto main_timer_rate = param_loader.loadParam2("main_timer_rate"); + const auto main_timer_rate = param_loader.loadParam2("main_timer_rate"); const auto _http_server_threads_ = param_loader.loadParam2("http_server_threads"); - const auto no_message_timeout = param_loader.loadParam2("no_message_timeout"); + const auto no_message_timeout = param_loader.loadParam2("no_message_timeout"); - const auto url = param_loader.loadParam2("url"); + const auto url = param_loader.loadParam2("url"); const auto client_port = param_loader.loadParam2("client_port"); const auto server_port = param_loader.loadParam2("server_port"); const auto robot_names = param_loader.loadParam2>("network/robot_names"); - max_linear_speed_ = param_loader.loadParam2("remote_control_limits/max_linear_speed"); + max_linear_speed_ = param_loader.loadParam2("remote_control_limits/max_linear_speed"); max_heading_rate_ = param_loader.loadParam2("remote_control_limits/max_heading_rate"); // Remove ground-station hostname from robot names - std::string hostname_str(hostname.data()); + std::string hostname_str(hostname.data()); std::vector filtered_robot_names = robot_names; auto it = std::remove(filtered_robot_names.begin(), filtered_robot_names.end(), hostname_str); @@ -325,35 +302,36 @@ void IROCBridge::onInit() { // Server // Do we need this (set_path)? - CROW_ROUTE(http_srv_, "/set_path").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return pathCallback(req); }); - CROW_ROUTE(http_srv_, "/safety-area/origin").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return setOriginCallback(req); }); - CROW_ROUTE(http_srv_, "/safety-area/borders").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return setSafetyBorderCallback(req); }); - CROW_ROUTE(http_srv_, "/safety-area/obstacles").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return setObstacleCallback(req); }); - CROW_ROUTE(http_srv_, "/mission").methods(crow::HTTPMethod::Post)([this](const crow::request& req){ return missionCallback(req); }); + CROW_ROUTE(http_srv_, "/set_path").methods(crow::HTTPMethod::Post)([this](const crow::request& req) { return pathCallback(req); }); + CROW_ROUTE(http_srv_, "/safety-area/origin").methods(crow::HTTPMethod::Post)([this](const crow::request& req) { return setOriginCallback(req); }); + CROW_ROUTE(http_srv_, "/safety-area/borders").methods(crow::HTTPMethod::Post)([this](const crow::request& req) { return setSafetyBorderCallback(req); }); + CROW_ROUTE(http_srv_, "/safety-area/obstacles").methods(crow::HTTPMethod::Post)([this](const crow::request& req) { return setObstacleCallback(req); }); + CROW_ROUTE(http_srv_, "/mission").methods(crow::HTTPMethod::Post)([this](const crow::request& req) { return missionCallback(req); }); // Missions - //TODO: CROW_REGEX_ROUTE(http_srv_, R"(/fleet/mission/(start|stop|pause))") - CROW_ROUTE(http_srv_, "/mission/") - .methods(crow::HTTPMethod::Post)([this](const crow::request& req, const std::string& type){ return changeFleetMissionStateCallback(req, type); }); - //TODO: CROW_REGEX_ROUTE(http_srv_, R"(/robots/(\w+)/mission/(start|stop|pause))") + // TODO: CROW_REGEX_ROUTE(http_srv_, R"(/fleet/mission/(start|stop|pause))") + CROW_ROUTE(http_srv_, "/mission/").methods(crow::HTTPMethod::Post)([this](const crow::request& req, const std::string& type) { + return changeFleetMissionStateCallback(req, type); + }); + // TODO: CROW_REGEX_ROUTE(http_srv_, R"(/robots/(\w+)/mission/(start|stop|pause))") CROW_ROUTE(http_srv_, "/robots//mission/") - .methods(crow::HTTPMethod::Post)( - [this](const crow::request& req, const std::string& robot_name, const std::string& type){ return changeRobotMissionStateCallback(req, robot_name, type); }); + .methods(crow::HTTPMethod::Post)([this](const crow::request& req, const std::string& robot_name, const std::string& type) { + return changeRobotMissionStateCallback(req, robot_name, type); + }); // Available robots endpoint - CROW_ROUTE(http_srv_, "/robots").methods(crow::HTTPMethod::Get)([this](const crow::request& req){ return availableRobotsCallback(req); }); + CROW_ROUTE(http_srv_, "/robots").methods(crow::HTTPMethod::Get)([this](const crow::request& req) { return availableRobotsCallback(req); }); - // Command endpoints with robot name in the path (land, takeoff, hover, home) - CROW_ROUTE(http_srv_, "/robots//").methods(crow::HTTPMethod::Post) - ([this](const crow::request& req, const std::string& robot_name, const std::string& command_type) { - return commandCallback(req, command_type, robot_name); - }); + // Command endpoints with robot name in the path (land, takeoff, hover, home) + CROW_ROUTE(http_srv_, "/robots//") + .methods(crow::HTTPMethod::Post)([this](const crow::request& req, const std::string& robot_name, const std::string& command_type) { + return commandCallback(req, command_type, robot_name); + }); - // Command endpoint for all robots (land, takeoff, hover, home) - CROW_ROUTE(http_srv_, "/robots/").methods(crow::HTTPMethod::Post) - ([this](const crow::request& req, const std::string& command_type) { - return commandCallback(req, command_type, std::nullopt); - }); + // Command endpoint for all robots (land, takeoff, hover, home) + CROW_ROUTE(http_srv_, "/robots/").methods(crow::HTTPMethod::Post)([this](const crow::request& req, const std::string& command_type) { + return commandCallback(req, command_type, std::nullopt); + }); // Remote control websocket CROW_WEBSOCKET_ROUTE(http_srv_, "/rc") @@ -391,13 +369,13 @@ void IROCBridge::onInit() { // | ----------------------- subscribers ---------------------- | mrs_lib::SubscribeHandlerOptions shopts; - shopts.nh = nh_; - shopts.node_name = "IROCBridge"; + shopts.nh = nh_; + shopts.node_name = "IROCBridge"; shopts.no_message_timeout = no_message_timeout; - shopts.threadsafe = true; - shopts.autostart = true; - shopts.queue_size = 10; - shopts.transport_hints = ros::TransportHints().tcpNoDelay(); + shopts.threadsafe = true; + shopts.autostart = true; + shopts.queue_size = 10; + shopts.transport_hints = ros::TransportHints().tcpNoDelay(); // populate the robot handlers vector { @@ -416,14 +394,14 @@ void IROCBridge::onInit() { robot_handler.sh_state_estimation_info = mrs_lib::SubscribeHandler(shopts, state_estimation_info_topic_name); const std::string control_info_topic_name = "/" + robot_name + nh_.resolveName("in/control_info"); - robot_handler.sh_control_info = mrs_lib::SubscribeHandler(shopts, control_info_topic_name); + robot_handler.sh_control_info = mrs_lib::SubscribeHandler(shopts, control_info_topic_name); const std::string collision_avoidance_info_topic_name = "/" + robot_name + nh_.resolveName("in/collision_avoidance_info"); robot_handler.sh_collision_avoidance_info = mrs_lib::SubscribeHandler(shopts, collision_avoidance_info_topic_name); const std::string uav_info_topic_name = "/" + robot_name + nh_.resolveName("in/uav_info"); - robot_handler.sh_uav_info = mrs_lib::SubscribeHandler(shopts, uav_info_topic_name); + robot_handler.sh_uav_info = mrs_lib::SubscribeHandler(shopts, uav_info_topic_name); const std::string system_health_info_topic_name = "/" + robot_name + nh_.resolveName("in/system_health_info"); robot_handler.sh_system_health_info = mrs_lib::SubscribeHandler(shopts, system_health_info_topic_name); @@ -449,7 +427,8 @@ void IROCBridge::onInit() { robot_handler.sc_set_obstacle = nh_.serviceClient("/" + robot_name + nh_.resolveName("svc/set_obstacle")); ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc/set_obstacle\' -> \'%s\'", robot_handler.sc_set_obstacle.getService().c_str()); - robot_handler.sc_velocity_reference = nh_.serviceClient("/" + robot_name + nh_.resolveName("svc/velocity_reference")); + robot_handler.sc_velocity_reference = + nh_.serviceClient("/" + robot_name + nh_.resolveName("svc/velocity_reference")); ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc/velocity_reference\' -> \'%s\'", robot_handler.sc_velocity_reference.getService().c_str()); // | ----------------------- publishers ----------------------- | @@ -471,14 +450,14 @@ void IROCBridge::onInit() { /* // | --------------------- action clients --------------------- | */ - //Mission action client + // Mission action client const std::string action_client_topic = nh_.resolveName("ac/mission"); - fleet_manager_action_client_ = std::make_unique(action_client_topic, false); + fleet_manager_action_client_ = std::make_unique(action_client_topic, false); ROS_INFO("[IROCBridge]: Created action client on topic \'ac/mission\' -> \'%s\'", action_client_topic.c_str()); // | ------------------------- timers ------------------------- | - timer_main_ = nh_.createTimer(ros::Rate(main_timer_rate), &IROCBridge::timerMain, this); + timer_main_ = nh_.createTimer(ros::Rate(main_timer_rate), &IROCBridge::timerMain, this); th_death_check_ = std::thread(&IROCBridge::routine_death_check, this); th_death_check_.detach(); @@ -494,12 +473,12 @@ void IROCBridge::onInit() { // -------------------------------------------------------------- /* timerMain() //{ */ -void IROCBridge::timerMain([[maybe_unused]] const ros::TimerEvent &event) { +void IROCBridge::timerMain([[maybe_unused]] const ros::TimerEvent& event) { std::scoped_lock lck(robot_handlers_.mtx); - // Parsing the messages to send in telemetry - for (auto &rh : robot_handlers_.handlers) { - const auto &robot_name = rh.robot_name; + // Parsing the messages to send in telemetry + for (auto& rh : robot_handlers_.handlers) { + const auto& robot_name = rh.robot_name; if (rh.sh_general_robot_info.newMsg()) { parseGeneralRobotInfo(rh.sh_general_robot_info.getMsg(), robot_name); @@ -510,7 +489,7 @@ void IROCBridge::timerMain([[maybe_unused]] const ros::TimerEvent &event) { } if (rh.sh_control_info.newMsg()) { - parseControlInfo(rh.sh_control_info.getMsg(),robot_name); + parseControlInfo(rh.sh_control_info.getMsg(), robot_name); } if (rh.sh_collision_avoidance_info.newMsg()) { @@ -543,15 +522,14 @@ void IROCBridge::missionActiveCallback() { template void IROCBridge::missionDoneCallback(const SimpleClientGoalState& state, const boost::shared_ptr& result) { if (result == NULL) { - ROS_WARN( - "[IROCBridge]: Probably fleet_manager died, and action server connection was lost!, reconnection is not currently handled, if mission manager was " - "restarted need to upload a new mission!"); + ROS_WARN("[IROCBridge]: Probably fleet_manager died, and action server connection was lost!, reconnection is not currently handled, if mission manager was " + "restarted need to upload a new mission!"); // Create JSON with Crow json json_msg = { - {"success", false}, - {"message", "Fleet manager died in ongoing mission"}, - {"robot_results", "Fleet manager died in ongoing mission"}, + {"success", false}, + {"message", "Fleet manager died in ongoing mission"}, + {"robot_results", "Fleet manager died in ongoing mission"}, }; sendTelemetryJsonMessage("results", json_msg); @@ -562,22 +540,16 @@ void IROCBridge::missionDoneCallback(const SimpleClientGoalState& state, const b ROS_INFO_STREAM("[IROCBridge]: Mission Action server finished with state: \"" << state.toString() << "\""); } - json robot_results = json::list(); + json robot_results = json::list(); for (size_t i = 0; i < result->robot_results.size(); i++) { - robot_results[i] = { - {"robot_name", result->robot_results[i].name}, - {"success", static_cast(result->robot_results[i].success)}, - {"message", result->robot_results[i].message} - }; + robot_results[i] = {{"robot_name", result->robot_results[i].name}, + {"success", static_cast(result->robot_results[i].success)}, + {"message", result->robot_results[i].message}}; } // Create the main JSON object - json json_msg = { - {"success", static_cast(result->success)}, - {"message", result->message}, - {"robot_results", robot_results} - }; + json json_msg = {{"success", static_cast(result->success)}, {"message", result->message}, {"robot_results", robot_results}}; sendJsonMessage("results", json_msg); } @@ -586,50 +558,39 @@ void IROCBridge::missionDoneCallback(const SimpleClientGoalState& state, const b /* missionFeedbackCallback //{ */ template - void IROCBridge::missionFeedbackCallback(const boost::shared_ptr& feedback) { +void IROCBridge::missionFeedbackCallback(const boost::shared_ptr& feedback) { auto robot_feedbacks = feedback->info.robot_feedbacks; - + // Create a list for robot feedback json json_msgs = json::list(); // Collect each robot feedback and create a json for each for (size_t i = 0; i < robot_feedbacks.size(); i++) { const auto& rfb = robot_feedbacks[i]; - - json robot_json = { - {"robot_name", rfb.name}, - {"message", rfb.message}, - {"mission_progress", rfb.mission_progress}, - {"current_goal", rfb.goal_idx}, - {"distance_to_goal", rfb.distance_to_closest_goal}, - {"goal_estimated_arrival_time", rfb.goal_estimated_arrival_time}, - {"goal_progress", rfb.goal_progress}, - {"distance_to_finish", rfb.distance_to_finish}, - {"finish_estimated_arrival_time", rfb.finish_estimated_arrival_time} - }; - ROS_DEBUG_STREAM("[IROCBridge]: Mission feedback for robot: " << rfb.name - << ", message: " << rfb.message - << ", progress: " << rfb.mission_progress - << ", current goal: " << rfb.goal_idx - << ", distance to goal: " << rfb.distance_to_closest_goal - << ", goal estimated arrival time: " << rfb.goal_estimated_arrival_time - << ", goal progress: " << rfb.goal_progress - << ", distance to finish: " << rfb.distance_to_finish - << ", finish estimated arrival time: " << rfb.finish_estimated_arrival_time); - + json robot_json = {{"robot_name", rfb.name}, + {"message", rfb.message}, + {"mission_progress", rfb.mission_progress}, + {"current_goal", rfb.goal_idx}, + {"distance_to_goal", rfb.distance_to_closest_goal}, + {"goal_estimated_arrival_time", rfb.goal_estimated_arrival_time}, + {"goal_progress", rfb.goal_progress}, + {"distance_to_finish", rfb.distance_to_finish}, + {"finish_estimated_arrival_time", rfb.finish_estimated_arrival_time}}; + + ROS_DEBUG_STREAM("[IROCBridge]: Mission feedback for robot: " + << rfb.name << ", message: " << rfb.message << ", progress: " << rfb.mission_progress << ", current goal: " << rfb.goal_idx + << ", distance to goal: " << rfb.distance_to_closest_goal << ", goal estimated arrival time: " << rfb.goal_estimated_arrival_time + << ", goal progress: " << rfb.goal_progress << ", distance to finish: " << rfb.distance_to_finish + << ", finish estimated arrival time: " << rfb.finish_estimated_arrival_time); + // Add to the list at index i json_msgs[i] = std::move(robot_json); } // Create the main JSON message - json json_msg = { - {"progress", feedback->info.progress}, - {"mission_state", feedback->info.state}, - {"message", feedback->info.message}, - {"robots", json_msgs} - }; - + json json_msg = {{"progress", feedback->info.progress}, {"mission_state", feedback->info.state}, {"message", feedback->info.message}, {"robots", json_msgs}}; + sendTelemetryJsonMessage("MissionFeedback", json_msg); } //} @@ -642,25 +603,17 @@ template void IROCBridge::parseGeneralRobotInfo(mrs_robot_diagnostics::GeneralRobotInfo::ConstPtr general_robot_info, const std::string& robot_name) { json json_msg = { - {"robot_name", general_robot_info->robot_name}, - {"robot_type", general_robot_info->robot_type}, - {"battery_state", { - {"voltage", general_robot_info->battery_state.voltage}, - {"percentage", general_robot_info->battery_state.percentage}, - {"wh_drained", general_robot_info->battery_state.wh_drained} - }}, - {"ready_to_start", general_robot_info->ready_to_start}, - {"problems_preventing_start", json::list( - general_robot_info->problems_preventing_start.begin(), - general_robot_info->problems_preventing_start.end()) - }, - {"errors", json::list( - general_robot_info->errors.begin(), - general_robot_info->errors.end()) - } - }; - - sendTelemetryJsonMessage("GeneralRobotInfo", json_msg); + {"robot_name", general_robot_info->robot_name}, + {"robot_type", general_robot_info->robot_type}, + {"battery_state", + {{"voltage", general_robot_info->battery_state.voltage}, + {"percentage", general_robot_info->battery_state.percentage}, + {"wh_drained", general_robot_info->battery_state.wh_drained}}}, + {"ready_to_start", general_robot_info->ready_to_start}, + {"problems_preventing_start", json::list(general_robot_info->problems_preventing_start.begin(), general_robot_info->problems_preventing_start.end())}, + {"errors", json::list(general_robot_info->errors.begin(), general_robot_info->errors.end())}}; + + sendTelemetryJsonMessage("GeneralRobotInfo", json_msg); } //} @@ -669,61 +622,44 @@ void IROCBridge::parseGeneralRobotInfo(mrs_robot_diagnostics::GeneralRobotInfo:: void IROCBridge::parseStateEstimationInfo(mrs_robot_diagnostics::StateEstimationInfo::ConstPtr state_estimation_info, const std::string& robot_name) { // Create the JSON structure using initializer lists json json_msg = { - {"robot_name", robot_name}, - {"estimation_frame", state_estimation_info->header.frame_id}, - {"above_ground_level_height", state_estimation_info->above_ground_level_height}, - {"current_estimator", state_estimation_info->current_estimator}, - - {"local_pose", { - {"x", state_estimation_info->local_pose.position.x}, - {"y", state_estimation_info->local_pose.position.y}, - {"z", state_estimation_info->local_pose.position.z}, - {"heading", state_estimation_info->local_pose.heading} - }}, - - {"global_pose", { - {"latitude", state_estimation_info->global_pose.position.x}, - {"longitude", state_estimation_info->global_pose.position.y}, - {"altitude", state_estimation_info->global_pose.position.z}, - {"heading", state_estimation_info->global_pose.heading} - }}, - - {"velocity", { - {"linear", { - {"x", state_estimation_info->velocity.linear.x}, - {"y", state_estimation_info->velocity.linear.y}, - {"z", state_estimation_info->velocity.linear.z} - }}, - {"angular", { - {"x", state_estimation_info->velocity.angular.x}, - {"y", state_estimation_info->velocity.angular.y}, - {"z", state_estimation_info->velocity.angular.z} - }} - }}, - - {"acceleration", { - {"linear", { - {"x", state_estimation_info->acceleration.linear.x}, - {"y", state_estimation_info->acceleration.linear.y}, - {"z", state_estimation_info->acceleration.linear.z} - }}, - {"angular", { - {"x", state_estimation_info->acceleration.angular.x}, - {"y", state_estimation_info->acceleration.angular.y}, - {"z", state_estimation_info->acceleration.angular.z} - }} - }}, - - {"running_estimators", { json::list( - state_estimation_info->running_estimators.begin(), - state_estimation_info->running_estimators.end()) - }}, - - {"switchable_estimators", { json::list( - state_estimation_info->switchable_estimators.begin(), - state_estimation_info->switchable_estimators.end()) - }} - }; + {"robot_name", robot_name}, + {"estimation_frame", state_estimation_info->header.frame_id}, + {"above_ground_level_height", state_estimation_info->above_ground_level_height}, + {"current_estimator", state_estimation_info->current_estimator}, + + {"local_pose", + {{"x", state_estimation_info->local_pose.position.x}, + {"y", state_estimation_info->local_pose.position.y}, + {"z", state_estimation_info->local_pose.position.z}, + {"heading", state_estimation_info->local_pose.heading}}}, + + {"global_pose", + {{"latitude", state_estimation_info->global_pose.position.x}, + {"longitude", state_estimation_info->global_pose.position.y}, + {"altitude", state_estimation_info->global_pose.position.z}, + {"heading", state_estimation_info->global_pose.heading}}}, + + {"velocity", + {{"linear", + {{"x", state_estimation_info->velocity.linear.x}, {"y", state_estimation_info->velocity.linear.y}, {"z", state_estimation_info->velocity.linear.z}}}, + {"angular", + {{"x", state_estimation_info->velocity.angular.x}, + {"y", state_estimation_info->velocity.angular.y}, + {"z", state_estimation_info->velocity.angular.z}}}}}, + + {"acceleration", + {{"linear", + {{"x", state_estimation_info->acceleration.linear.x}, + {"y", state_estimation_info->acceleration.linear.y}, + {"z", state_estimation_info->acceleration.linear.z}}}, + {"angular", + {{"x", state_estimation_info->acceleration.angular.x}, + {"y", state_estimation_info->acceleration.angular.y}, + {"z", state_estimation_info->acceleration.angular.z}}}}}, + + {"running_estimators", {json::list(state_estimation_info->running_estimators.begin(), state_estimation_info->running_estimators.end())}}, + + {"switchable_estimators", {json::list(state_estimation_info->switchable_estimators.begin(), state_estimation_info->switchable_estimators.end())}}}; sendTelemetryJsonMessage("StateEstimationInfo", json_msg); } @@ -731,18 +667,12 @@ void IROCBridge::parseStateEstimationInfo(mrs_robot_diagnostics::StateEstimation /* parseControlInfo() //{ */ void IROCBridge::parseControlInfo(mrs_robot_diagnostics::ControlInfo::ConstPtr control_info, const std::string& robot_name) { - json json_msg = { - {"robot_name", robot_name}, - {"active_controller", control_info->active_controller}, - {"available_controllers", json::list( - control_info->available_controllers.begin(), - control_info->available_controllers.end())}, - {"active_tracker", control_info->active_tracker}, - {"available_trackers", json::list( - control_info->available_trackers.begin(), - control_info->available_trackers.end())}, - {"thrust", control_info->thrust} - }; + json json_msg = {{"robot_name", robot_name}, + {"active_controller", control_info->active_controller}, + {"available_controllers", json::list(control_info->available_controllers.begin(), control_info->available_controllers.end())}, + {"active_tracker", control_info->active_tracker}, + {"available_trackers", json::list(control_info->available_trackers.begin(), control_info->available_trackers.end())}, + {"thrust", control_info->thrust}}; sendTelemetryJsonMessage("ControlInfo", json_msg); } @@ -751,12 +681,10 @@ void IROCBridge::parseControlInfo(mrs_robot_diagnostics::ControlInfo::ConstPtr c /* parseCollisionAvoidanceInfo() //{ */ void IROCBridge::parseCollisionAvoidanceInfo(mrs_robot_diagnostics::CollisionAvoidanceInfo::ConstPtr collision_avoidance_info, const std::string& robot_name) { json json_msg = { - {"robot_name", robot_name}, - {"collision_avoidance_enabled", collision_avoidance_info->collision_avoidance_enabled}, - {"avoiding_collision", collision_avoidance_info->avoiding_collision}, - {"other_robots_visible", json::list( - collision_avoidance_info->other_robots_visible.begin(), - collision_avoidance_info->other_robots_visible.end())}, + {"robot_name", robot_name}, + {"collision_avoidance_enabled", collision_avoidance_info->collision_avoidance_enabled}, + {"avoiding_collision", collision_avoidance_info->avoiding_collision}, + {"other_robots_visible", json::list(collision_avoidance_info->other_robots_visible.begin(), collision_avoidance_info->other_robots_visible.end())}, }; sendTelemetryJsonMessage("CollisionAvoidanceInfo", json_msg); @@ -765,14 +693,12 @@ void IROCBridge::parseCollisionAvoidanceInfo(mrs_robot_diagnostics::CollisionAvo /* parseUavInfo() //{ */ void IROCBridge::parseUavInfo(mrs_robot_diagnostics::UavInfo::ConstPtr uav_info, const std::string& robot_name) { - json json_msg = { - {"robot_name", robot_name}, - {"armed", uav_info->armed}, - {"offboard", uav_info->offboard}, - {"flight_state", uav_info->flight_state}, - {"flight_duration", uav_info->flight_duration}, - {"mass_nominal", uav_info->mass_nominal} - }; + json json_msg = {{"robot_name", robot_name}, + {"armed", uav_info->armed}, + {"offboard", uav_info->offboard}, + {"flight_state", uav_info->flight_state}, + {"flight_duration", uav_info->flight_duration}, + {"mass_nominal", uav_info->mass_nominal}}; sendTelemetryJsonMessage("UavInfo", json_msg); } @@ -786,10 +712,7 @@ void IROCBridge::parseSystemHealthInfo(mrs_robot_diagnostics::SystemHealthInfo:: const auto& node_cpu_load = system_health_info->node_cpu_loads[i]; // Create a nested array for each node_cpu_load using initializer list - json node_entry = json::list({ - node_cpu_load.node_name, - node_cpu_load.cpu_load - }); + json node_entry = json::list({node_cpu_load.node_name, node_cpu_load.cpu_load}); node_cpu_loads[i] = std::move(node_entry); } @@ -801,29 +724,23 @@ void IROCBridge::parseSystemHealthInfo(mrs_robot_diagnostics::SystemHealthInfo:: // Create an object for each required_sensor using initializer list available_sensors[i] = { - {"name", available_sensor.name}, - {"status", available_sensor.status}, - {"ready", available_sensor.ready}, - {"rate", available_sensor.rate} - }; + {"name", available_sensor.name}, {"status", available_sensor.status}, {"ready", available_sensor.ready}, {"rate", available_sensor.rate}}; } // Create the main JSON object using initializer list - json json_msg = { - {"robot_name", robot_name}, - {"cpu_load", system_health_info->cpu_load}, - {"free_ram", system_health_info->free_ram}, - {"total_ram", system_health_info->total_ram}, - {"free_hdd", system_health_info->free_hdd}, - {"hw_api_rate", system_health_info->hw_api_rate}, - {"control_manager_rate", system_health_info->control_manager_rate}, - {"state_estimation_rate", system_health_info->state_estimation_rate}, - {"gnss_uncertainty", system_health_info->gnss_uncertainty}, - {"mag_strength", system_health_info->mag_strength}, - {"mag_uncertainty", system_health_info->mag_uncertainty}, - {"node_cpu_loads", node_cpu_loads}, - {"available_sensors", available_sensors} - }; + json json_msg = {{"robot_name", robot_name}, + {"cpu_load", system_health_info->cpu_load}, + {"free_ram", system_health_info->free_ram}, + {"total_ram", system_health_info->total_ram}, + {"free_hdd", system_health_info->free_hdd}, + {"hw_api_rate", system_health_info->hw_api_rate}, + {"control_manager_rate", system_health_info->control_manager_rate}, + {"state_estimation_rate", system_health_info->state_estimation_rate}, + {"gnss_uncertainty", system_health_info->gnss_uncertainty}, + {"mag_strength", system_health_info->mag_strength}, + {"mag_uncertainty", system_health_info->mag_uncertainty}, + {"node_cpu_loads", node_cpu_loads}, + {"available_sensors", available_sensors}}; sendTelemetryJsonMessage("SystemHealthInfo", json_msg); } @@ -833,11 +750,11 @@ void IROCBridge::parseSystemHealthInfo(mrs_robot_diagnostics::SystemHealthInfo:: // | helper methods | // -------------------------------------------------------------- /* sendJsonMessage() //{ */ -void IROCBridge::sendJsonMessage(const std::string& msg_type,json& json_msg) { - const std::string url = "/api/mission/" + msg_type; - const std::string body = json_msg.dump(); +void IROCBridge::sendJsonMessage(const std::string& msg_type, json& json_msg) { + const std::string url = "/api/mission/" + msg_type; + const std::string body = json_msg.dump(); const std::string content_type = "application/json"; - const auto res = http_client_->Post(url, body, content_type); + const auto res = http_client_->Post(url, body, content_type); if (res) ROS_DEBUG_STREAM_THROTTLE(1.0, res->status << ": " << res->body); @@ -851,7 +768,7 @@ void IROCBridge::sendJsonMessage(const std::string& msg_type,json& json_msg) { /* sendTelemetryJsonMessage() //{ */ void IROCBridge::sendTelemetryJsonMessage(const std::string& type, json& json_msg) { - json_msg["type"] = type; + json_msg["type"] = type; std::string message = json_msg.dump(); if (active_telemetry_connection_) { @@ -925,14 +842,22 @@ IROCBridge::robot_handler_t* IROCBridge::findRobotHandler(const std::string& rob /* getServiceClient() method //{ */ ros::ServiceClient* IROCBridge::getServiceClient(IROCBridge::robot_handler_t* rh_ptr, const IROCBridge::CommandType command_type) { switch (command_type) { - case CommandType::Takeoff: return &(rh_ptr->sc_takeoff); - case CommandType::Land: return &(rh_ptr->sc_land); - case CommandType::Hover: return &(rh_ptr->sc_hover); - case CommandType::Home: return &(rh_ptr->sc_land_home); - case CommandType::Set_Origin: return &(rh_ptr->sc_set_origin); - case CommandType::Set_SafetyBorder: return &(rh_ptr->sc_set_safety_area); - case CommandType::Set_Obstacle: return &(rh_ptr->sc_set_obstacle); - default: return nullptr; + case CommandType::Takeoff: + return &(rh_ptr->sc_takeoff); + case CommandType::Land: + return &(rh_ptr->sc_land); + case CommandType::Hover: + return &(rh_ptr->sc_hover); + case CommandType::Home: + return &(rh_ptr->sc_land_home); + case CommandType::Set_Origin: + return &(rh_ptr->sc_set_origin); + case CommandType::Set_SafetyBorder: + return &(rh_ptr->sc_set_safety_area); + case CommandType::Set_Obstacle: + return &(rh_ptr->sc_set_obstacle); + default: + return nullptr; } } @@ -942,14 +867,14 @@ ros::ServiceClient* IROCBridge::getServiceClient(IROCBridge::robot_handler_t* rh IROCBridge::action_result_t IROCBridge::commandAction(const std::vector& robot_names, const std::string& command_type) { std::scoped_lock lck(robot_handlers_.mtx); - bool everything_ok = true; + bool everything_ok = true; std::stringstream ss; - crow::status status_code = crow::status::ACCEPTED; + crow::status status_code = crow::status::ACCEPTED; ss << "Command: " << command_type << " Result: "; CommandType command_type_e = CommandType::Unknown; auto it = command_type_map_.find(command_type); - if (it != command_type_map_.end()) { + if (it != command_type_map_.end()) { command_type_e = it->second; } else { ss << "Command type \"" << command_type << "\" not found, skipping\n"; @@ -970,7 +895,7 @@ IROCBridge::action_result_t IROCBridge::commandAction(const std::vector -IROCBridge::action_result_t IROCBridge::commandAction(const std::vector& robot_names, const std::string& command_type, typename Svc_T::Request req) { +IROCBridge::action_result_t IROCBridge::commandAction(const std::vector& robot_names, const std::string& command_type, + typename Svc_T::Request req) { std::scoped_lock lck(robot_handlers_.mtx); - bool everything_ok = true; + bool everything_ok = true; std::stringstream ss; - crow::status status_code = crow::status::ACCEPTED; + crow::status status_code = crow::status::ACCEPTED; ss << "Command: " << command_type << " Result: "; CommandType command_type_e = CommandType::Unknown; auto it = command_type_map_.find(command_type); - if (it != command_type_map_.end()) { + if (it != command_type_map_.end()) { command_type_e = it->second; } else { ss << "Command type \"" << command_type << "\" not found, skipping\n"; @@ -1022,7 +948,7 @@ IROCBridge::action_result_t IROCBridge::commandAction(const std::vector json resultToJson(const boost::shared_ptr& result) { - json robot_results = json::list(); + json robot_results = json::list(); for (size_t i = 0; i < result->robot_results.size(); i++) { - robot_results[i] = { - {"robot_name", result->robot_results[i].name}, - {"success", static_cast(result->robot_results[i].success)}, - {"message", result->robot_results[i].message} - }; + robot_results[i] = {{"robot_name", result->robot_results[i].name}, + {"success", static_cast(result->robot_results[i].success)}, + {"message", result->robot_results[i].message}}; } // Create the main JSON object - json json_msg = { - {"success", static_cast(result->success)}, - {"message", result->message}, - {"robot_results", robot_results} - }; + json json_msg = {{"success", static_cast(result->success)}, {"message", result->message}, {"robot_results", robot_results}}; return json_msg; } @@ -1073,11 +993,7 @@ json resultToJson(const boost::shared_ptr& result) { json successMissionJson() { // Create the main JSON object - json json_msg = { - {"success", true}, - {"message", "Mission uploaded successfully"}, - {"robot_results", "All robots received their mission"} - }; + json json_msg = {{"success", true}, {"message", "Mission uploaded successfully"}, {"robot_results", "All robots received their mission"}}; return json_msg; } @@ -1095,8 +1011,7 @@ json successMissionJson() { * \param req The HTTP request containing the JSON message. * \return A response indicating the success or failure of the operation. */ -crow::response IROCBridge::pathCallback(const crow::request& req) -{ +crow::response IROCBridge::pathCallback(const crow::request& req) { ROS_INFO_STREAM("[IROCBridge]: Parsing a path message JSON -> ROS."); crow::json::rvalue json_msg = crow::json::load(req.body); @@ -1105,9 +1020,9 @@ crow::response IROCBridge::pathCallback(const crow::request& req) return crow::response(crow::status::BAD_REQUEST, "{\"message\": \"Bad json input: " + req.body + "\"}"); } - std::string robot_name = json_msg["robot_name"].s(); + std::string robot_name = json_msg["robot_name"].s(); std::scoped_lock lck(robot_handlers_.mtx); - auto* rh_ptr = findRobotHandler(robot_name, robot_handlers_); + auto* rh_ptr = findRobotHandler(robot_name, robot_handlers_); if (!rh_ptr) { ROS_WARN_STREAM("[IROCBridge]: Robot \"" << robot_name << "\" not found. Ignoring."); return crow::response(crow::status::BAD_REQUEST, "{\"message\": \"Robot \"" + robot_name + "\" not found, ignoring\"}"); @@ -1127,10 +1042,10 @@ crow::response IROCBridge::pathCallback(const crow::request& req) mrs_msgs::Path msg_path; msg_path.points.reserve(points.size()); - msg_path.header.stamp = ros::Time::now(); + msg_path.header.stamp = ros::Time::now(); msg_path.header.frame_id = frame_id; - msg_path.fly_now = true; - msg_path.use_heading = false; + msg_path.fly_now = true; + msg_path.use_heading = false; for (const auto& el : points) { mrs_msgs::Reference ref; @@ -1142,7 +1057,7 @@ crow::response IROCBridge::pathCallback(const crow::request& req) ref.heading = el["heading"].d(); msg_path.use_heading = true; } - + msg_path.points.push_back(ref); } @@ -1162,8 +1077,7 @@ crow::response IROCBridge::pathCallback(const crow::request& req) * \param req Crow request * \return res Crow response */ -crow::response IROCBridge::setOriginCallback(const crow::request& req) -{ +crow::response IROCBridge::setOriginCallback(const crow::request& req) { ROS_INFO_STREAM("[IROCBridge]: Parsing a setOriginCallback message JSON -> ROS."); @@ -1174,13 +1088,13 @@ crow::response IROCBridge::setOriginCallback(const crow::request& req) } // Get message properties - int frame_id = json_msg["frame_id"].i(); + int frame_id = json_msg["frame_id"].i(); // The service supports latlon and UTM, but we we can at the moment support only latlon for IROC - // TODO: It can be extended in the future to support UTM origin + // TODO: It can be extended in the future to support UTM origin mrs_msgs::ReferenceStampedSrv::Request service_request; - service_request.header.frame_id = "latlon_origin"; - service_request.header.stamp = ros::Time::now(); + service_request.header.frame_id = "latlon_origin"; + service_request.header.stamp = ros::Time::now(); service_request.reference.position.x = json_msg["x"].d(); service_request.reference.position.y = json_msg["y"].d(); @@ -1193,11 +1107,11 @@ crow::response IROCBridge::setOriginCallback(const crow::request& req) // check that all robot names are valid and find the corresponding robot handlers - const auto result = commandAction(robot_names, "set_origin" , service_request); + const auto result = commandAction(robot_names, "set_origin", service_request); if (result.success) { ROS_INFO_STREAM("[IROCBridge]: Set origin for " << robot_names.size() << " robots."); - world_origin_.x = json_msg["x"].d(); + world_origin_.x = json_msg["x"].d(); world_origin_.y = json_msg["y"].d(); } @@ -1213,8 +1127,7 @@ crow::response IROCBridge::setOriginCallback(const crow::request& req) * \param req Crow request * \return res Crow response */ -crow::response IROCBridge::setSafetyBorderCallback(const crow::request& req) -{ +crow::response IROCBridge::setSafetyBorderCallback(const crow::request& req) { ROS_INFO_STREAM("[IROCBridge]: Parsing a setSafetyBorderCallback message JSON -> ROS."); crow::json::rvalue json_msg = crow::json::load(req.body); @@ -1223,12 +1136,12 @@ crow::response IROCBridge::setSafetyBorderCallback(const crow::request& req) return crow::response(crow::status::BAD_REQUEST, "{\"message\": \"Bad json input: " + req.body + "\"}"); } - bool enabled = true; // Defined default as true - + bool enabled = true; // Defined default as true + // Get message properties - int height_id = json_msg["height_id"].i(); - int max_z = json_msg["max_z"].i(); - int min_z = json_msg["min_z"].i(); + int height_id = json_msg["height_id"].i(); + int max_z = json_msg["max_z"].i(); + int min_z = json_msg["min_z"].i(); std::vector points = json_msg["points"].lo(); std::string horizontal_frame = "latlon_origin"; @@ -1261,12 +1174,12 @@ crow::response IROCBridge::setSafetyBorderCallback(const crow::request& req) ROS_INFO("[IROCBridge]: Border points size %zu ", border_points.size()); mrs_msgs::SafetyBorder safety_border; - safety_border.enabled = enabled; + safety_border.enabled = enabled; safety_border.horizontal_frame = horizontal_frame; - safety_border.vertical_frame = vertical_frame; - safety_border.points = border_points; - safety_border.max_z = max_z; - safety_border.min_z = min_z; + safety_border.vertical_frame = vertical_frame; + safety_border.points = border_points; + safety_border.max_z = max_z; + safety_border.min_z = min_z; std::scoped_lock lck(robot_handlers_.mtx); @@ -1280,7 +1193,7 @@ crow::response IROCBridge::setSafetyBorderCallback(const crow::request& req) service_request.safety_border = safety_border; mrs_msgs::SetSafetyBorderSrv::Request req_srv = service_request; - const auto result = commandAction(robot_names, "set_safety_border" , req_srv); + const auto result = commandAction(robot_names, "set_safety_border", req_srv); return crow::response(result.status_code, result.message); } @@ -1294,8 +1207,7 @@ crow::response IROCBridge::setSafetyBorderCallback(const crow::request& req) * \param req Crow request * \return res Crow response */ -crow::response IROCBridge::setObstacleCallback(const crow::request& req) -{ +crow::response IROCBridge::setObstacleCallback(const crow::request& req) { ROS_INFO_STREAM("[IROCBridge]: Parsing a setObstacleCallback message JSON -> ROS."); @@ -1304,10 +1216,10 @@ crow::response IROCBridge::setObstacleCallback(const crow::request& req) return crow::response(crow::status::BAD_REQUEST, "Failed to parse JSON" + req.body); // Get message properties - int height_id = json_msg["height_id"].i(); - int max_z = json_msg["max_z"].i(); - int min_z = json_msg["min_z"].i(); - std::vector points = json_msg["points"].lo(); + int height_id = json_msg["height_id"].i(); + int max_z = json_msg["max_z"].i(); + int min_z = json_msg["min_z"].i(); + std::vector points = json_msg["points"].lo(); std::string horizontal_frame = "latlon_origin"; std::string vertical_frame; @@ -1318,7 +1230,7 @@ crow::response IROCBridge::setObstacleCallback(const crow::request& req) }; auto it = height_id_map.find(height_id); - if (it != height_id_map.end()) // check if the height_id is valid (exists in the map) + if (it != height_id_map.end()) // check if the height_id is valid (exists in the map) vertical_frame = it->second; else @@ -1327,7 +1239,7 @@ crow::response IROCBridge::setObstacleCallback(const crow::request& req) if (points.empty()) return crow::response(crow::status::BAD_REQUEST, "Empty points array: " + req.body); - //Process points + // Process points std::vector border_points; border_points.reserve(points.size()); @@ -1348,10 +1260,10 @@ crow::response IROCBridge::setObstacleCallback(const crow::request& req) mrs_msgs::SetObstacleSrvRequest obstacle_req; obstacle_req.horizontal_frame = horizontal_frame; - obstacle_req.vertical_frame = vertical_frame; - obstacle_req.points = border_points; - obstacle_req.max_z = max_z; - obstacle_req.min_z = min_z; + obstacle_req.vertical_frame = vertical_frame; + obstacle_req.points = border_points; + obstacle_req.max_z = max_z; + obstacle_req.min_z = min_z; std::scoped_lock lck(robot_handlers_.mtx); @@ -1374,8 +1286,7 @@ crow::response IROCBridge::setObstacleCallback(const crow::request& req) * \param req Crow request * \return res Crow response */ -crow::response IROCBridge::missionCallback(const crow::request& req) -{ +crow::response IROCBridge::missionCallback(const crow::request& req) { ROS_INFO_STREAM("[IROCBridge]: Parsing a missionCallback message JSON -> ROS."); try { @@ -1388,8 +1299,7 @@ crow::response IROCBridge::missionCallback(const crow::request& req) ROS_WARN_STREAM("[IROCBridge]: Action server is not connected. Check the iroc_fleet_manager node."); std::string msg = "Action server is not connected. Check iroc_fleet_manager node.\n"; return crow::response(crow::status::CONFLICT, "{\"message\": \"" + msg + "\"}"); - } - else if (!fleet_manager_action_client_->getState().isDone()) { + } else if (!fleet_manager_action_client_->getState().isDone()) { ROS_WARN_STREAM("[IROCBridge]: Mission is already running. Terminate the previous one, or wait until it is finished."); std::string msg = "Mission is already running. Terminate the previous one, or wait until it is finished"; return crow::response(crow::status::CONFLICT, "{\"message\": \"" + msg + "\"}"); @@ -1397,46 +1307,35 @@ crow::response IROCBridge::missionCallback(const crow::request& req) // Send the action goal to the fleet manager FleetManagerGoal action_goal; - std::string type = json_msg["type"].s(); + std::string type = json_msg["type"].s(); // Convert rvalue to wvalue, then dump to string crow::json::wvalue details_wvalue(json_msg["details"]); - std::string details = details_wvalue.dump(); + std::string details = details_wvalue.dump(); action_goal.type = type; action_goal.details = details; fleet_manager_action_client_->sendGoal( - action_goal, - [this](const auto& state, const auto& result) { - missionDoneCallback(state, result); - }, - [this]() { - missionActiveCallback(); - }, - [this](const auto& feedback) { - missionFeedbackCallback(feedback); - } - ); + action_goal, [this](const auto& state, const auto& result) { missionDoneCallback(state, result); }, + [this]() { missionActiveCallback(); }, [this](const auto& feedback) { missionFeedbackCallback(feedback); }); // Waiting in the case the trajectories are rejected. We can better wait will the state is pending ros::Duration(5).sleep(); - if (fleet_manager_action_client_->getState().isDone()) { // If the action is done, the action finished instantly + if (fleet_manager_action_client_->getState().isDone()) { // If the action is done, the action finished instantly auto result = fleet_manager_action_client_->getResult(); - auto json = resultToJson(result); + auto json = resultToJson(result); const auto message = result->message; ROS_WARN("[IROCBridge]: %s", message.c_str()); return crow::response(crow::status::BAD_REQUEST, json); - } - else { + } else { ROS_INFO("[IROCBridge]: Mission received successfully"); // TODO to replace with proper response, in ROS2 auto json = successMissionJson(); return crow::response(crow::status::CREATED, json); } - } - catch (const std::exception& e) { + } catch (const std::exception& e) { ROS_WARN_STREAM("[IROCBridge]: Failed to parse JSON from message: " << e.what()); return crow::response(crow::status::BAD_REQUEST, "{\"message\": \"Failed to parse JSON from message: " + std::string(e.what()) + "\"}"); } @@ -1451,8 +1350,7 @@ crow::response IROCBridge::missionCallback(const crow::request& req) * \param req Crow request * \return res Crow response */ -crow::response IROCBridge::changeFleetMissionStateCallback(const crow::request& req, const std::string& type) -{ +crow::response IROCBridge::changeFleetMissionStateCallback(const crow::request& req, const std::string& type) { std::scoped_lock lck(robot_handlers_.mtx); // Input validation @@ -1478,25 +1376,20 @@ crow::response IROCBridge::changeFleetMissionStateCallback(const crow::request& * \param req Crow request * \return res Crow response */ -crow::response IROCBridge::changeRobotMissionStateCallback(const crow::request& req, const std::string& robot_name, const std::string& type) -{ +crow::response IROCBridge::changeRobotMissionStateCallback(const crow::request& req, const std::string& robot_name, const std::string& type) { std::scoped_lock lck(robot_handlers_.mtx); // Input validation if (type != "start" && type != "stop" && type != "pause") return crow::response(crow::status::NOT_FOUND); - if (!std::any_of(robot_handlers_.handlers.begin(), - robot_handlers_.handlers.end(), - [&robot_name](const auto &rh) { - return rh.robot_name == robot_name; - })) + if (!std::any_of(robot_handlers_.handlers.begin(), robot_handlers_.handlers.end(), [&robot_name](const auto& rh) { return rh.robot_name == robot_name; })) return crow::response(crow::status::NOT_FOUND, "Robot not found"); json json_msg; iroc_fleet_manager::ChangeRobotMissionStateSrv ros_srv; ros_srv.request.robot_name = robot_name; - ros_srv.request.type = type; + ros_srv.request.type = type; const auto resp = callService(sc_change_robot_mission_state_, ros_srv.request); if (!resp.success) { @@ -1504,11 +1397,10 @@ crow::response IROCBridge::changeRobotMissionStateCallback(const crow::request& ROS_WARN_STREAM("[IROCBridge]: " << json_msg["message"].dump()); return crow::response(crow::status::INTERNAL_SERVER_ERROR, json_msg.dump()); - } - else { + } else { json_msg["message"] = "Call successful"; return crow::response(crow::status::ACCEPTED, json_msg.dump()); - } + } return crow::response(crow::status::NOT_FOUND); } @@ -1522,17 +1414,14 @@ crow::response IROCBridge::changeRobotMissionStateCallback(const crow::request& * * \return res Crow response */ -crow::response IROCBridge::commandCallback(const crow::request& req, - const std::string & command_type, - std::optional robot_name) -{ +crow::response IROCBridge::commandCallback(const crow::request& req, const std::string& command_type, std::optional robot_name) { std::scoped_lock lck(robot_handlers_.mtx); std::vector robot_names; if (robot_name.has_value()) { robot_names.push_back(robot_name.value()); } else { - robot_names.reserve(robot_handlers_.handlers.size()); + robot_names.reserve(robot_handlers_.handlers.size()); for (const auto& rh : robot_handlers_.handlers) { robot_names.push_back(rh.robot_name); } @@ -1551,8 +1440,7 @@ crow::response IROCBridge::commandCallback(const crow::request& req, * \param req Crow request * \return res Crow response */ -crow::response IROCBridge::availableRobotsCallback([[maybe_unused]] const crow::request& req) -{ +crow::response IROCBridge::availableRobotsCallback([[maybe_unused]] const crow::request& req) { std::vector robot_names; robot_names.reserve(robot_handlers_.handlers.size()); for (const auto& rh : robot_handlers_.handlers) @@ -1595,8 +1483,7 @@ crow::response IROCBridge::availableRobotsCallback([[maybe_unused]] const crow:: * - For unknown commands: `{"ok": false, "message": "Unknown command"}` * - For movement command failures: `{"ok": false, "message": "Failed to send movement command: [error details]"}` */ -void IROCBridge::remoteControlCallback(crow::websocket::connection& conn, const std::string& data, bool is_binary) -{ +void IROCBridge::remoteControlCallback(crow::websocket::connection& conn, const std::string& data, bool is_binary) { // Convert and check if the received data is a valid JSON crow::json::rvalue json_data = crow::json::load(data); if (!json_data || !json_data.has("command") || !json_data.has("data")) { @@ -1606,12 +1493,11 @@ void IROCBridge::remoteControlCallback(crow::websocket::connection& conn, const } json json_response; - std::string command = json_data["command"].s(); + std::string command = json_data["command"].s(); if (command == "message") { ROS_INFO_STREAM("[IROCBridge]: Received message from " << conn.get_remote_ip() << ": " << json_data["data"].s()); conn.send_text("{\"status\": \"Ok, received message\"}"); - } - else if (command == "move") { + } else if (command == "move") { std::scoped_lock lck(robot_handlers_.mtx); // Robot id validation @@ -1622,9 +1508,8 @@ void IROCBridge::remoteControlCallback(crow::websocket::connection& conn, const } std::string robot_name = json_data["robot_name"].s(); - if (!std::any_of(robot_handlers_.handlers.begin(), robot_handlers_.handlers.end(), [robot_name](const robot_handler_t& rh) { - return rh.robot_name == robot_name; - })) { + if (!std::any_of(robot_handlers_.handlers.begin(), robot_handlers_.handlers.end(), + [robot_name](const robot_handler_t& rh) { return rh.robot_name == robot_name; })) { ROS_WARN_STREAM("[IROCBridge]: Robot \"" << robot_name << "\" not found. Ignoring."); conn.send_text("{\"error\": \"Robot not found\"}"); return; @@ -1635,34 +1520,32 @@ void IROCBridge::remoteControlCallback(crow::websocket::connection& conn, const mrs_msgs::VelocityReferenceStampedSrvRequest req; req.reference.header.frame_id = "fcu_untilted"; - req.reference.reference.velocity.x = movement_data["x"].d() * max_linear_speed_; - req.reference.reference.velocity.y = movement_data["y"].d() * max_linear_speed_; - req.reference.reference.velocity.z = movement_data["z"].d() * max_linear_speed_; - req.reference.reference.heading_rate = movement_data["heading"].d() * max_heading_rate_; + req.reference.reference.velocity.x = movement_data["x"].d() * max_linear_speed_; + req.reference.reference.velocity.y = movement_data["y"].d() * max_linear_speed_; + req.reference.reference.velocity.z = movement_data["z"].d() * max_linear_speed_; + req.reference.reference.heading_rate = movement_data["heading"].d() * max_heading_rate_; req.reference.reference.use_heading_rate = true; auto* robot_handler_ptr = findRobotHandler(robot_name, robot_handlers_); - auto res = callService(robot_handler_ptr->sc_velocity_reference, req); + auto res = callService(robot_handler_ptr->sc_velocity_reference, req); if (res.success) { - json_response["ok"] = true; + json_response["ok"] = true; json_response["message"] = "Movement command sent"; - } - else { - json_response["ok"] = false; + } else { + json_response["ok"] = false; json_response["message"] = "Failed to send movement command: " + res.message; } - } - else { + } else { ROS_WARN_STREAM("[IROCBridge]: Unknown command in websocket message: " << command); - json_response["ok"] = false; + json_response["ok"] = false; json_response["message"] = "Unknown command"; } conn.send_text(json_response.dump()); } //} -} // namespace iroc_bridge +} // namespace iroc_bridge #include PLUGINLIB_EXPORT_CLASS(iroc_bridge::IROCBridge, nodelet::Nodelet); From ee1f9070013915a71f5e1456fe6469f000602508 Mon Sep 17 00:00:00 2001 From: marlonriv Date: Mon, 18 Aug 2025 09:07:58 +0200 Subject: [PATCH 07/33] update of json examples Following new mission request structure --- json_examples/autonomy_test.json | 7 -- json_examples/missions/autonomy_test.json | 11 +++ .../coverage.json} | 9 +- json_examples/missions/heading.json | 83 +++++++++++++++---- json_examples/missions/new_mission.json | 18 ---- json_examples/missions/one_drone.json | 37 ++++++--- json_examples/missions/waypoint.json | 63 ++++++++++++++ 7 files changed, 172 insertions(+), 56 deletions(-) delete mode 100644 json_examples/autonomy_test.json create mode 100644 json_examples/missions/autonomy_test.json rename json_examples/{coverage_mission.json => missions/coverage.json} (78%) delete mode 100644 json_examples/missions/new_mission.json create mode 100644 json_examples/missions/waypoint.json diff --git a/json_examples/autonomy_test.json b/json_examples/autonomy_test.json deleted file mode 100644 index fd92bcd..0000000 --- a/json_examples/autonomy_test.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - "type": "autonomy-test", - "details": { - "robot_name": "uav1", - "segment_length": 5 - } -} diff --git a/json_examples/missions/autonomy_test.json b/json_examples/missions/autonomy_test.json new file mode 100644 index 0000000..33b6275 --- /dev/null +++ b/json_examples/missions/autonomy_test.json @@ -0,0 +1,11 @@ +{ + "type": "AutonomyTestPlanner", + "details": { + "robots": [ + { + "name": "uav1", + "segment_length": 5 + } + ] + } +} diff --git a/json_examples/coverage_mission.json b/json_examples/missions/coverage.json similarity index 78% rename from json_examples/coverage_mission.json rename to json_examples/missions/coverage.json index 807b2cb..ac36cb3 100644 --- a/json_examples/coverage_mission.json +++ b/json_examples/missions/coverage.json @@ -1,7 +1,10 @@ { - "type": "coverage", + "type": "CoveragePlanner", "details": { - "robots": ["uav1", "uav2"], + "robots": [ + "uav1", + "uav2" + ], "search_area": [ { "x": 47.397978, @@ -21,7 +24,7 @@ } ], "height_id": 0, - "height": 10, + "height": 5, "terminal_action": 0 } } diff --git a/json_examples/missions/heading.json b/json_examples/missions/heading.json index b2a267f..b6ee999 100644 --- a/json_examples/missions/heading.json +++ b/json_examples/missions/heading.json @@ -1,18 +1,69 @@ { - "mission": [ - { - "robot_name": "uav1", - "frame_id": 0, - "height_id": 0, - "points": [ - { "x": 20, "y": 10, "z": 3, "heading": 1 }, - { "x": 20, "y": 10, "z": 3, "heading": 3 }, - { "x": -20, "y": -20, "z": 4, "heading": 3, "subtasks": [{ "type": 1, "parameters": "[0.5,0.5,0.5]" }] }, - { "x": -10, "y": 10, "z": 5, "heading": 3 }, - { "x": 10, "y": -10, "z": 4, "heading": 3, "subtasks": [{ "type": 0, "parameters": "5.6" }] }, - { "x": 20, "y": 10, "z": 3, "heading": 1 } - ], - "terminal_action": 0 - } - ] + "type": "WaypointPlanner", + "details": { + "robots": [ + { + "name": "uav1", + "frame_id": 0, + "height_id": 0, + "points": [ + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1 + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 3 + }, + { + "x": -20, + "y": -20, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": "gimbal", + "parameters": [0.5, 0.5, 0.5] + } + ] + }, + { + "x": -10, + "y": 10, + "z": 5, + "heading": 3 + }, + { + "x": 10, + "y": -10, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": "wait", + "parameters": 5.6 + } + ] + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1, + "subtasks": [ + { + "type": "wait", + "parameters": 5.6 + } + ] + } + ], + "terminal_action": 0 + } + ] + } } diff --git a/json_examples/missions/new_mission.json b/json_examples/missions/new_mission.json deleted file mode 100644 index b2a267f..0000000 --- a/json_examples/missions/new_mission.json +++ /dev/null @@ -1,18 +0,0 @@ -{ - "mission": [ - { - "robot_name": "uav1", - "frame_id": 0, - "height_id": 0, - "points": [ - { "x": 20, "y": 10, "z": 3, "heading": 1 }, - { "x": 20, "y": 10, "z": 3, "heading": 3 }, - { "x": -20, "y": -20, "z": 4, "heading": 3, "subtasks": [{ "type": 1, "parameters": "[0.5,0.5,0.5]" }] }, - { "x": -10, "y": 10, "z": 5, "heading": 3 }, - { "x": 10, "y": -10, "z": 4, "heading": 3, "subtasks": [{ "type": 0, "parameters": "5.6" }] }, - { "x": 20, "y": 10, "z": 3, "heading": 1 } - ], - "terminal_action": 0 - } - ] -} diff --git a/json_examples/missions/one_drone.json b/json_examples/missions/one_drone.json index 65fa79b..2cdb07e 100644 --- a/json_examples/missions/one_drone.json +++ b/json_examples/missions/one_drone.json @@ -1,14 +1,27 @@ { - "mission": [ - { - "robot_name": "uav1", - "frame_id": 0, - "height_id": 0, - "points": [ - { "x": 10, "y": 10, "z": 2, "heading": 1 }, - { "x": -10, "y": 10, "z": 2, "heading": 3 } - ], - "terminal_action": 0 - } - ] + "type": "WaypointPlanner", + "details": { + "robots": [ + { + "name": "uav1", + "frame_id": 0, + "height_id": 0, + "points": [ + { + "x": 10, + "y": 10, + "z": 2, + "heading": 1 + }, + { + "x": -10, + "y": 10, + "z": 2, + "heading": 3 + } + ], + "terminal_action": 0 + } + ] + } } diff --git a/json_examples/missions/waypoint.json b/json_examples/missions/waypoint.json new file mode 100644 index 0000000..c18001b --- /dev/null +++ b/json_examples/missions/waypoint.json @@ -0,0 +1,63 @@ +{ + "type": "WaypointPlanner", + "details": { + "robots": [ + { + "name": "uav1", + "frame_id": 0, + "height_id": 0, + "points": [ + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1 + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 3 + }, + { + "x": -20, + "y": -20, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": 1, + "parameters": [0.5,0.5,0.5] + } + ] + }, + { + "x": -10, + "y": 10, + "z": 5, + "heading": 3 + }, + { + "x": 10, + "y": -10, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": 0, + "parameters": "5.6" + } + ] + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1 + } + ], + "terminal_action": 0 + } + ] + } +} From c1bd308b9c19f94294fb9f8f1853c3e1440b74d7 Mon Sep 17 00:00:00 2001 From: marlonriv Date: Mon, 18 Aug 2025 15:18:36 +0200 Subject: [PATCH 08/33] Fixed custom config loading, and cleaning --- config/config.yaml | 13 ++++----- launch/iroc_bridge.launch | 11 +++----- src/iroc_bridge.cpp | 55 ++++++++++++++++++--------------------- 3 files changed, 36 insertions(+), 43 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index 7349757..c411187 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,8 +1,9 @@ -main_timer_rate: 100.0 # [Hz] -no_message_timeout: 5.0 # [s] -http_server_threads: 8 +iroc_bridge: + main_timer_rate: 100.0 # [Hz] + no_message_timeout: 5.0 # [s] + http_server_threads: 8 -remote_control_limits: - max_linear_speed: 1.0 # [m/s] - max_heading_rate: 0.8 # [rad/s] + remote_control_limits: + max_linear_speed: 1.0 # [m/s] + max_heading_rate: 0.8 # [rad/s] diff --git a/launch/iroc_bridge.launch b/launch/iroc_bridge.launch index b6b2ed0..2abcce5 100644 --- a/launch/iroc_bridge.launch +++ b/launch/iroc_bridge.launch @@ -3,15 +3,14 @@ - - - + + @@ -31,14 +30,12 @@ + + - - - - diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index ce6c8cc..4cfb6b3 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -53,12 +53,7 @@ #include #include -#include #include -#include -#include -#include - #include #include @@ -106,7 +101,17 @@ class IROCBridge : public nodelet::Nodelet { }; // | ---------------------- Command types --------------------- | - enum class CommandType { Takeoff, Land, Hover, Home, Set_Origin, Set_SafetyBorder, Set_Obstacle, Unknown }; + enum class CommandType + { + Takeoff, + Land, + Hover, + Home, + Set_Origin, + Set_SafetyBorder, + Set_Obstacle, + Unknown + }; enum class Change_SvC_T { FleetWaypoint, RobotWaypoint, FleetCoverage, RobotCoverage, RobotAutonomyTest }; @@ -249,35 +254,19 @@ void IROCBridge::onInit() { mrs_lib::ParamLoader param_loader(nh_, "IROCBridge"); std::string custom_config_path; - std::string network_config_path; - param_loader.loadParam("custom_config", custom_config_path); + // Custom config loaded first to have the priority, if not given it takes the default config file if (custom_config_path != "") { + ROS_INFO("Added custom config"); param_loader.addYamlFile(custom_config_path); } + param_loader.addYamlFileFromParam("network_config"); param_loader.addYamlFileFromParam("config"); - param_loader.loadParam("network_config", network_config_path); - - if (network_config_path != "") { - param_loader.addYamlFile(network_config_path); - } - - const auto main_timer_rate = param_loader.loadParam2("main_timer_rate"); - const auto _http_server_threads_ = param_loader.loadParam2("http_server_threads"); - const auto no_message_timeout = param_loader.loadParam2("no_message_timeout"); - - const auto url = param_loader.loadParam2("url"); - const auto client_port = param_loader.loadParam2("client_port"); - const auto server_port = param_loader.loadParam2("server_port"); - const auto robot_names = param_loader.loadParam2>("network/robot_names"); - max_linear_speed_ = param_loader.loadParam2("remote_control_limits/max_linear_speed"); - max_heading_rate_ = param_loader.loadParam2("remote_control_limits/max_heading_rate"); - // Remove ground-station hostname from robot names std::string hostname_str(hostname.data()); std::vector filtered_robot_names = robot_names; @@ -285,11 +274,17 @@ void IROCBridge::onInit() { auto it = std::remove(filtered_robot_names.begin(), filtered_robot_names.end(), hostname_str); filtered_robot_names.erase(it, filtered_robot_names.end()); - std::cout << "Filtered robot names: "; - for (const auto& name : filtered_robot_names) { - std::cout << name << " "; - } - std::cout << std::endl; + // Arguments given in launchfile + const auto url = param_loader.loadParam2("url"); + const auto client_port = param_loader.loadParam2("client_port"); + const auto server_port = param_loader.loadParam2("server_port"); + + const auto main_timer_rate = param_loader.loadParam2("iroc_bridge/main_timer_rate"); + const auto _http_server_threads_ = param_loader.loadParam2("iroc_bridge/http_server_threads"); + const auto no_message_timeout = param_loader.loadParam2("iroc_bridge/no_message_timeout"); + + max_linear_speed_ = param_loader.loadParam2("iroc_bridge/remote_control_limits/max_linear_speed"); + max_heading_rate_ = param_loader.loadParam2("iroc_bridge/remote_control_limits/max_heading_rate"); if (!param_loader.loadedSuccessfully()) { ROS_ERROR("[IROCBridge]: Could not load all parameters!"); From 766e3c57d2343f2c332d4997c425bc2acbce8107 Mon Sep 17 00:00:00 2001 From: marlonriv Date: Tue, 19 Aug 2025 11:51:29 +0200 Subject: [PATCH 09/33] Fix of action name from fleet manager --- src/iroc_bridge.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index 4cfb6b3..f79da95 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -53,7 +53,7 @@ #include #include -#include +#include #include #include @@ -70,9 +70,9 @@ using vec4_t = Eigen::Vector4d; using namespace actionlib; -using FleetManagerActionClient = SimpleActionClient; +using FleetManagerActionClient = SimpleActionClient; // Waypoint mission goal -typedef iroc_fleet_manager::FleetManagerGoal FleetManagerGoal; +typedef iroc_fleet_manager::IROCFleetManagerGoal FleetManagerGoal; typedef mrs_robot_diagnostics::robot_type_t robot_type_t; /* class IROCBridge //{ */ @@ -258,7 +258,6 @@ void IROCBridge::onInit() { // Custom config loaded first to have the priority, if not given it takes the default config file if (custom_config_path != "") { - ROS_INFO("Added custom config"); param_loader.addYamlFile(custom_config_path); } @@ -1312,8 +1311,8 @@ crow::response IROCBridge::missionCallback(const crow::request& req) { action_goal.details = details; fleet_manager_action_client_->sendGoal( - action_goal, [this](const auto& state, const auto& result) { missionDoneCallback(state, result); }, - [this]() { missionActiveCallback(); }, [this](const auto& feedback) { missionFeedbackCallback(feedback); }); + action_goal, [this](const auto& state, const auto& result) { missionDoneCallback(state, result); }, + [this]() { missionActiveCallback(); }, [this](const auto& feedback) { missionFeedbackCallback(feedback); }); // Waiting in the case the trajectories are rejected. We can better wait will the state is pending ros::Duration(5).sleep(); From cf62666d0fd31692a34431a7c6386e95e4dc606e Mon Sep 17 00:00:00 2001 From: Vojtech Spurny Date: Fri, 22 Aug 2025 16:06:50 +0200 Subject: [PATCH 10/33] remove unused dependency and include --- CMakeLists.txt | 1 - package.xml | 1 - src/iroc_bridge.cpp | 1 - 3 files changed, 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3772901..27a6a37 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,7 +13,6 @@ set(CATKIN_DEPENDENCIES mrs_msgs mrs_robot_diagnostics mrs_lib - iroc_mission_handler iroc_fleet_manager actionlib ) diff --git a/package.xml b/package.xml index 5213f77..4a6d501 100644 --- a/package.xml +++ b/package.xml @@ -20,7 +20,6 @@ mrs_msgs mrs_robot_diagnostics mrs_lib - iroc_mission_handler iroc_fleet_manager diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index f79da95..c86af47 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -54,7 +54,6 @@ #include #include -#include #include #include From a4b19a9e04ff676c39c0de638eae053b2a139379 Mon Sep 17 00:00:00 2001 From: marlonriv Date: Fri, 22 Aug 2025 17:31:23 +0200 Subject: [PATCH 11/33] Getters for safety area_borders and world origin --- launch/iroc_bridge.launch | 3 + src/iroc_bridge.cpp | 179 +++++++++++++++++++++++++++++++++++++- 2 files changed, 180 insertions(+), 2 deletions(-) diff --git a/launch/iroc_bridge.launch b/launch/iroc_bridge.launch index 2abcce5..c2db99f 100644 --- a/launch/iroc_bridge.launch +++ b/launch/iroc_bridge.launch @@ -67,6 +67,9 @@ + + + diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index c86af47..ac76c8e 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -29,6 +29,9 @@ #include #include +#include +#include +#include #include #include @@ -164,6 +167,9 @@ class IROCBridge : public nodelet::Nodelet { // | ----------------------- ROS Clients ----------------------- | ros::ServiceClient sc_change_fleet_mission_state_; ros::ServiceClient sc_change_robot_mission_state_; + ros::ServiceClient sc_get_world_origin_; + ros::ServiceClient sc_get_safety_border_; + ros::ServiceClient sc_get_obstacles_; // | ----------------- action client callbacks ---------------- | @@ -198,8 +204,11 @@ class IROCBridge : public nodelet::Nodelet { // REST API callbacks crow::response pathCallback(const crow::request& req); crow::response setOriginCallback(const crow::request& req); + crow::response getOriginCallback(const crow::request& req); crow::response setSafetyBorderCallback(const crow::request& req); + crow::response getSafetyBorderCallback(const crow::request& req); crow::response setObstacleCallback(const crow::request& req); + crow::response getObstaclesCallback(const crow::request& req); crow::response missionCallback(const crow::request& req); crow::response changeFleetMissionStateCallback(const crow::request& req, const std::string& type); @@ -215,6 +224,10 @@ class IROCBridge : public nodelet::Nodelet { template result_t callService(ros::ServiceClient& sc, typename Svc_T::Request req); + template + result_t callService(ros::ServiceClient& sc, typename Svc_T::Request req, typename Svc_T::Response &res); + + template result_t callService(ros::ServiceClient& sc); @@ -296,11 +309,16 @@ void IROCBridge::onInit() { // Server // Do we need this (set_path)? CROW_ROUTE(http_srv_, "/set_path").methods(crow::HTTPMethod::Post)([this](const crow::request& req) { return pathCallback(req); }); - CROW_ROUTE(http_srv_, "/safety-area/origin").methods(crow::HTTPMethod::Post)([this](const crow::request& req) { return setOriginCallback(req); }); + CROW_ROUTE(http_srv_, "/safety-area/world-origin").methods(crow::HTTPMethod::Post)([this](const crow::request& req) { return setOriginCallback(req); }); CROW_ROUTE(http_srv_, "/safety-area/borders").methods(crow::HTTPMethod::Post)([this](const crow::request& req) { return setSafetyBorderCallback(req); }); CROW_ROUTE(http_srv_, "/safety-area/obstacles").methods(crow::HTTPMethod::Post)([this](const crow::request& req) { return setObstacleCallback(req); }); CROW_ROUTE(http_srv_, "/mission").methods(crow::HTTPMethod::Post)([this](const crow::request& req) { return missionCallback(req); }); + // Getters + CROW_ROUTE(http_srv_, "/safety-area/world-origin").methods(crow::HTTPMethod::Get)([this](const crow::request& req) { return getOriginCallback(req); }); + CROW_ROUTE(http_srv_, "/safety-area/borders").methods(crow::HTTPMethod::Get)([this](const crow::request& req) { return getSafetyBorderCallback(req); }); + CROW_ROUTE(http_srv_, "/safety-area/obstacles").methods(crow::HTTPMethod::Get)([this](const crow::request& req) { return getObstaclesCallback(req); }); + // Missions // TODO: CROW_REGEX_ROUTE(http_srv_, R"(/fleet/mission/(start|stop|pause))") CROW_ROUTE(http_srv_, "/mission/").methods(crow::HTTPMethod::Post)([this](const crow::request& req, const std::string& type) { @@ -441,6 +459,18 @@ void IROCBridge::onInit() { ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/change_robot_mission_state\' -> \'%s\'", sc_change_robot_mission_state_.getService().c_str()); + sc_get_world_origin_ = nh_.serviceClient(nh_.resolveName("svc/get_world_origin")); + ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/get_world_origin\' -> \'%s\'", + sc_get_world_origin_.getService().c_str()); + + sc_get_safety_border_ = nh_.serviceClient(nh_.resolveName("svc/get_safety_border")); + ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/get_safety_border\' -> \'%s\'", + sc_get_safety_border_.getService().c_str()); + + sc_get_obstacles_= nh_.serviceClient(nh_.resolveName("svc/get_obstacles")); + ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/get_obstacles\' -> \'%s\'", + sc_get_obstacles_.getService().c_str()); + /* // | --------------------- action clients --------------------- | */ // Mission action client @@ -794,6 +824,24 @@ IROCBridge::result_t IROCBridge::callService(ros::ServiceClient& sc, typename Sv } } +/* callService() //{ */ +template +IROCBridge::result_t IROCBridge::callService(ros::ServiceClient& sc, typename Svc_T::Request req, typename Svc_T::Response &res) { + if (sc.call(req, res)) { + if (res.success) { + ROS_INFO_STREAM_THROTTLE(1.0, "Called service \"" << sc.getService() << "\" with response \"" << res.message << "\"."); + return {true, res.message}; + } else { + ROS_WARN_STREAM_THROTTLE(1.0, "Called service \"" << sc.getService() << "\" with response \"" << res.message << "\"."); + return {false, res.message}; + } + } else { + const std::string msg = "Failed to call service \"" + sc.getService() + "\"."; + ROS_WARN_STREAM(msg); + return {false, msg}; + } +} + template IROCBridge::result_t IROCBridge::callService(ros::ServiceClient& sc) { return callService(sc, {}); @@ -1112,6 +1160,37 @@ crow::response IROCBridge::setOriginCallback(const crow::request& req) { } //} +/* getOriginCallback() method //{ */ + +/** + * \brief Callback to get the world origin. + * + * \param req Crow request + * \return res Crow response + */ +crow::response IROCBridge::getOriginCallback(const crow::request &req) { + + ROS_INFO_STREAM("[IROCBridge]: Processing a getOriginCallback"); + + iroc_fleet_manager::GetWorldOriginSrv world_origin_service; + + const auto resp = callService(sc_get_world_origin_, world_origin_service.request, world_origin_service.response); + + json json_msg; + if (!resp.success) { + json_msg["message"] = "Call was not successful with message: " + resp.message; + ROS_WARN_STREAM("[IROCBridge]: " << json_msg["message"].dump()); + return crow::response(crow::status::CONFLICT, json_msg.dump()); + } else { + json_msg["message"] = world_origin_service.response.message; + json_msg["x"] = world_origin_service.response.origin_x; + json_msg["y"] = world_origin_service.response.origin_y; + return crow::response(crow::status::ACCEPTED, json_msg.dump()); + } +} +//} + + /* setSafetyBorderCallback() method //{ */ /** @@ -1192,6 +1271,48 @@ crow::response IROCBridge::setSafetyBorderCallback(const crow::request& req) { } //} +/* getSafetyBorderCallback() method //{ */ + +/** + * \brief Callback to get the safety area border. + * + * \param req Crow request + * \return res Crow response + */ +crow::response IROCBridge::getSafetyBorderCallback(const crow::request &req) { + + ROS_INFO_STREAM("[IROCBridge]: Processing a getSafetyBorderCallback message ROS -> JSON."); + iroc_fleet_manager::GetSafetyBorderSrv get_safety_border_service; + + const auto resp = callService(sc_get_safety_border_, get_safety_border_service.request, get_safety_border_service.response); + + json json_msg; + if (!resp.success) { + json_msg["message"] = "Call was not successful with message: " + resp.message; + ROS_WARN_STREAM("[IROCBridge]: " << json_msg["message"].dump()); + return crow::response(crow::status::CONFLICT, json_msg.dump()); + } else { + json_msg["message"] = get_safety_border_service.response.message; + + json points = json::list(); + auto vector_points = get_safety_border_service.response.border.points; + double max_z = get_safety_border_service.response.border.max_z; + double min_z = get_safety_border_service.response.border.min_z; + std::string horizontal_frame = get_safety_border_service.response.border.horizontal_frame; + std::string vertical_frame = get_safety_border_service.response.border.vertical_frame; + + for (size_t i = 0; i < vector_points.size(); i++) { + const auto point = vector_points.at(i); + json point_json = {{"x", point.x}, {"y", point.y}}; + points[i] = std::move(point_json); + } + + json json_msg = {{"message", "All robots in the fleet with the same safety border"}, {"points", points}, {"max_z", max_z}, {"min_z", min_z}}; + return crow::response(crow::status::ACCEPTED, json_msg.dump()); + } +} +//} + /* setObstacleCallback() method //{ */ /** @@ -1271,6 +1392,60 @@ crow::response IROCBridge::setObstacleCallback(const crow::request& req) { } //} +/* getObstaclesCallback() method //{ */ + +/** + * \brief Callback to get the obstacles. + * + * \param req Crow request + * \return res Crow response + */ +crow::response IROCBridge::getObstaclesCallback(const crow::request &req) { + + ROS_INFO_STREAM("[IROCBridge]: Processing a getObstaclesCallback message ROS -> JSON."); + iroc_fleet_manager::GetObstaclesSrv get_obstacles_service; + + const auto resp = callService(sc_get_obstacles_, get_obstacles_service.request, get_obstacles_service.response); + + json json_msg; + if (!resp.success) { + json_msg["message"] = "Call was not successful with message: " + resp.message; + ROS_WARN_STREAM("[IROCBridge]: " << json_msg["message"].dump()); + return crow::response(crow::status::CONFLICT, json_msg.dump()); + } else { + + json_msg["message"] = get_obstacles_service.response.message; + int number_of_obstacles = get_obstacles_service.response.obstacles.rows.size(); + auto obstacles = get_obstacles_service.response.obstacles; + json obstacles_json_list = json::list(); + + size_t point_index = 0; + + for (size_t i = 0; i < number_of_obstacles; i++) { + int number_of_vertices = obstacles.rows.at(i); + double max_z = obstacles.max_z.at(i); + double min_z = obstacles.min_z.at(i); + + // Extract the points for the obstacle + json points_list = json::list(); + for (int j = 0; j < number_of_vertices; j++) { + if (point_index < obstacles.data.size()) { + json point = {{"x", obstacles.data.at(point_index).x}, {"y", obstacles.data.at(point_index).y}}; + points_list[j] = std::move(point); + point_index++; // Move to next point + } + } + + json obstacle_json = {{"points", points_list},{"max_z", max_z}, {"min_z", min_z}}; + obstacles_json_list[i] = std::move(obstacle_json); + } + + json json_msg = {{"message", "All robots in the fleet with the same obstacles"}, {"obstacles", obstacles_json_list}}; + return crow::response(crow::status::ACCEPTED, json_msg.dump()); + } +} +//} + /* missionCallback() method //{ */ /** @@ -1436,7 +1611,7 @@ crow::response IROCBridge::commandCallback(const crow::request& req, const std:: crow::response IROCBridge::availableRobotsCallback([[maybe_unused]] const crow::request& req) { std::vector robot_names; robot_names.reserve(robot_handlers_.handlers.size()); - for (const auto& rh : robot_handlers_.handlers) + for (const auto &rh : robot_handlers_.handlers) robot_names.push_back(rh.robot_name); json json_msg; From 0b8ccce1acfcccd0d6a4c3cce569789246e262a9 Mon Sep 17 00:00:00 2001 From: marlonriv Date: Mon, 25 Aug 2025 17:28:12 +0200 Subject: [PATCH 12/33] Obtain the mission points from fleet manager Responding to the REST API client with the robot "points" from the requested mission. This feature is useful for the coverage planner mission, for the UI to display the points. --- .../missions/sequential_subtask.json | 82 +++++++++++++++++++ launch/iroc_bridge.launch | 1 + src/iroc_bridge.cpp | 76 ++++++++++++----- 3 files changed, 138 insertions(+), 21 deletions(-) create mode 100644 json_examples/missions/sequential_subtask.json diff --git a/json_examples/missions/sequential_subtask.json b/json_examples/missions/sequential_subtask.json new file mode 100644 index 0000000..8b01918 --- /dev/null +++ b/json_examples/missions/sequential_subtask.json @@ -0,0 +1,82 @@ +{ + "type": "WaypointPlanner", + "details": { + "robots": [ + { + "name": "uav1", + "frame_id": 0, + "height_id": 0, + "points": [ + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1 + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 3 + }, + { + "x": -20, + "y": -20, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": "test", + "parameters": [ + 0.5, + 0.5, + 0.5 + ], + "continue_without_waiting": false, + "stop_on_failure": false, + "max_retries": 1, + "retry_delay": 0 + }, + { + "type": "wait", + "parameters": 5.6 + } + ], + "parallel_execution": true + }, + { + "x": -10, + "y": 10, + "z": 5, + "heading": 3 + }, + { + "x": 10, + "y": -10, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": "wait", + "parameters": 5.6 + } + ] + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1, + "subtasks": [ + { + "type": "wait", + "parameters": 5.6 + } + ] + } + ], + "terminal_action": 0 + } + ] + } +} diff --git a/launch/iroc_bridge.launch b/launch/iroc_bridge.launch index c2db99f..bc4f6ec 100644 --- a/launch/iroc_bridge.launch +++ b/launch/iroc_bridge.launch @@ -70,6 +70,7 @@ + diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index ac76c8e..a34bb0e 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include @@ -170,6 +171,7 @@ class IROCBridge : public nodelet::Nodelet { ros::ServiceClient sc_get_world_origin_; ros::ServiceClient sc_get_safety_border_; ros::ServiceClient sc_get_obstacles_; + ros::ServiceClient sc_get_mission_points_; // | ----------------- action client callbacks ---------------- | @@ -467,10 +469,14 @@ void IROCBridge::onInit() { ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/get_safety_border\' -> \'%s\'", sc_get_safety_border_.getService().c_str()); - sc_get_obstacles_= nh_.serviceClient(nh_.resolveName("svc/get_obstacles")); + sc_get_obstacles_ = nh_.serviceClient(nh_.resolveName("svc/get_obstacles")); ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/get_obstacles\' -> \'%s\'", sc_get_obstacles_.getService().c_str()); + sc_get_mission_points_ = nh_.serviceClient(nh_.resolveName("svc/get_mission_points")); + ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/get_mission_points\' -> \'%s\'", + sc_get_mission_points_.getService().c_str()); + /* // | --------------------- action clients --------------------- | */ // Mission action client @@ -1029,17 +1035,6 @@ json resultToJson(const boost::shared_ptr& result) { } //} -/* successMissionJson() method //{ */ - -json successMissionJson() { - - // Create the main JSON object - json json_msg = {{"success", true}, {"message", "Mission uploaded successfully"}, {"robot_results", "All robots received their mission"}}; - - return json_msg; -} -//} - // -------------------------------------------------------------- // | REST API callbacks | // -------------------------------------------------------------- @@ -1454,7 +1449,7 @@ crow::response IROCBridge::getObstaclesCallback(const crow::request &req) { * \param req Crow request * \return res Crow response */ -crow::response IROCBridge::missionCallback(const crow::request& req) { +crow::response IROCBridge::missionCallback(const crow::request &req) { ROS_INFO_STREAM("[IROCBridge]: Parsing a missionCallback message JSON -> ROS."); try { @@ -1481,29 +1476,68 @@ crow::response IROCBridge::missionCallback(const crow::request& req) { crow::json::wvalue details_wvalue(json_msg["details"]); std::string details = details_wvalue.dump(); - action_goal.type = type; + action_goal.type = type; action_goal.details = details; fleet_manager_action_client_->sendGoal( - action_goal, [this](const auto& state, const auto& result) { missionDoneCallback(state, result); }, - [this]() { missionActiveCallback(); }, [this](const auto& feedback) { missionFeedbackCallback(feedback); }); + action_goal, [this](const auto &state, const auto &result) { missionDoneCallback(state, result); }, + [this]() { missionActiveCallback(); }, + [this](const auto &feedback) { missionFeedbackCallback(feedback); }); // Waiting in the case the trajectories are rejected. We can better wait will the state is pending ros::Duration(5).sleep(); if (fleet_manager_action_client_->getState().isDone()) { // If the action is done, the action finished instantly - auto result = fleet_manager_action_client_->getResult(); - auto json = resultToJson(result); + auto result = fleet_manager_action_client_->getResult(); + // TODO + auto json = resultToJson(result); const auto message = result->message; ROS_WARN("[IROCBridge]: %s", message.c_str()); return crow::response(crow::status::BAD_REQUEST, json); } else { ROS_INFO("[IROCBridge]: Mission received successfully"); // TODO to replace with proper response, in ROS2 - auto json = successMissionJson(); - return crow::response(crow::status::CREATED, json); + iroc_fleet_manager::GetMissionPointsSrv get_mission_points_service; + + const auto resp = + callService(sc_get_mission_points_, get_mission_points_service.request, get_mission_points_service.response); + + json json_msg; + if (!resp.success) { + json_msg["message"] = resp.message; + json_msg["success"] = false; + json_msg["robot_goals"] = NULL; + ROS_WARN_STREAM("[IROCBridge]: " << json_msg["message"].dump()); + return crow::response(crow::status::INTERNAL_SERVER_ERROR, json_msg.dump()); + } else { + + json_msg["message"] = get_mission_points_service.response.message; + auto robot_goals = get_mission_points_service.response.mission_robots; + json mission = json::list(); + + for (size_t i = 0; i < robot_goals.size(); i++) { + std::string robot_name = robot_goals.at(i).name; + ROS_INFO("Robot name %s", robot_name.c_str()); + auto points = robot_goals.at(i).points; + int frame_id = robot_goals.at(i).frame_id; + int height_id = robot_goals.at(i).height_id; + + // Extract the points + json points_list = json::list(); + for (int j = 0; j < points.size(); j++) { + mrs_msgs::Reference reference = points.at(j).reference; + json point = {{"x", reference.position.x}, {"y", reference.position.y}, {"z", reference.position.z}, {"heading", reference.heading}}; + points_list[j] = std::move(point); + } + json robot_goal = {{"robot", robot_name}, {"points", points_list}, {"frame_id", frame_id}, {"height_id", height_id}}; + mission[i] = std::move(robot_goal); + } + json json_msg = {{"success", true}, {"message", "Mission uploaded successfully"}, {"robot_goals", mission}}; + return crow::response(crow::status::CREATED, json_msg.dump()); + } } - } catch (const std::exception& e) { + } + catch (const std::exception &e) { ROS_WARN_STREAM("[IROCBridge]: Failed to parse JSON from message: " << e.what()); return crow::response(crow::status::BAD_REQUEST, "{\"message\": \"Failed to parse JSON from message: " + std::string(e.what()) + "\"}"); } From e3e3a15a7adf4c77707e91cdd4eb48ef00d6e69b Mon Sep 17 00:00:00 2001 From: marlonriv Date: Tue, 26 Aug 2025 13:45:30 +0200 Subject: [PATCH 13/33] Cleaning and improve consistency with the mission response --- src/iroc_bridge.cpp | 100 +++++++++++++++++++++++--------------------- 1 file changed, 52 insertions(+), 48 deletions(-) diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index a34bb0e..44f8d3b 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -1016,25 +1016,53 @@ IROCBridge::action_result_t IROCBridge::commandAction(const std::vector json resultToJson(const boost::shared_ptr& result) { json robot_results = json::list(); - for (size_t i = 0; i < result->robot_results.size(); i++) { - robot_results[i] = {{"robot_name", result->robot_results[i].name}, + robot_results[i] = {{"robot", result->robot_results[i].name}, {"success", static_cast(result->robot_results[i].success)}, - {"message", result->robot_results[i].message}}; + {"message", result->robot_results[i].message}, + {"mission", json::list()}}; } - // Create the main JSON object - json json_msg = {{"success", static_cast(result->success)}, {"message", result->message}, {"robot_results", robot_results}}; - + json json_msg = {{"success", static_cast(result->success)}, {"message", result->message}, {"robot_data", robot_results}}; return json_msg; } //} +/* missionGoalToJson() method //{ */ + +json missionGoalToJson(std::vector &mission_goals) { + json robots_data = json::list(); + + for (size_t i = 0; i < mission_goals.size(); i++) { + std::string robot_name = mission_goals.at(i).name; + auto points = mission_goals.at(i).points; + int frame_id = mission_goals.at(i).frame_id; + int height_id = mission_goals.at(i).height_id; + + // Extract the points + json points_list = json::list(); + for (int j = 0; j < points.size(); j++) { + mrs_msgs::Reference reference = points.at(j).reference; + json point = {{"x", reference.position.x}, {"y", reference.position.y}, {"z", reference.position.z}, {"heading", reference.heading}}; + points_list[j] = std::move(point); + } + json mission = {{"points", points_list}, {"frame_id", frame_id}, {"height_id", height_id}}; + json robot_data = {{"robot", robot_name}, {"success", true}, {"message", "Mission loaded successfully"}, {"mission", mission}}; + robots_data[i] = std::move(robot_data); + } + + json response = {{"success", true}, {"message", "Mission uploaded successfully"}, {"robot_data", robots_data}}; + + return response; +} +//} + + // -------------------------------------------------------------- // | REST API callbacks | // -------------------------------------------------------------- @@ -1489,53 +1517,29 @@ crow::response IROCBridge::missionCallback(const crow::request &req) { if (fleet_manager_action_client_->getState().isDone()) { // If the action is done, the action finished instantly auto result = fleet_manager_action_client_->getResult(); - // TODO auto json = resultToJson(result); const auto message = result->message; ROS_WARN("[IROCBridge]: %s", message.c_str()); return crow::response(crow::status::BAD_REQUEST, json); - } else { - ROS_INFO("[IROCBridge]: Mission received successfully"); - // TODO to replace with proper response, in ROS2 - iroc_fleet_manager::GetMissionPointsSrv get_mission_points_service; - - const auto resp = - callService(sc_get_mission_points_, get_mission_points_service.request, get_mission_points_service.response); - - json json_msg; - if (!resp.success) { - json_msg["message"] = resp.message; - json_msg["success"] = false; - json_msg["robot_goals"] = NULL; - ROS_WARN_STREAM("[IROCBridge]: " << json_msg["message"].dump()); - return crow::response(crow::status::INTERNAL_SERVER_ERROR, json_msg.dump()); - } else { + } - json_msg["message"] = get_mission_points_service.response.message; - auto robot_goals = get_mission_points_service.response.mission_robots; - json mission = json::list(); - - for (size_t i = 0; i < robot_goals.size(); i++) { - std::string robot_name = robot_goals.at(i).name; - ROS_INFO("Robot name %s", robot_name.c_str()); - auto points = robot_goals.at(i).points; - int frame_id = robot_goals.at(i).frame_id; - int height_id = robot_goals.at(i).height_id; - - // Extract the points - json points_list = json::list(); - for (int j = 0; j < points.size(); j++) { - mrs_msgs::Reference reference = points.at(j).reference; - json point = {{"x", reference.position.x}, {"y", reference.position.y}, {"z", reference.position.z}, {"heading", reference.heading}}; - points_list[j] = std::move(point); - } - json robot_goal = {{"robot", robot_name}, {"points", points_list}, {"frame_id", frame_id}, {"height_id", height_id}}; - mission[i] = std::move(robot_goal); - } - json json_msg = {{"success", true}, {"message", "Mission uploaded successfully"}, {"robot_goals", mission}}; - return crow::response(crow::status::CREATED, json_msg.dump()); - } + // TODO to replace with proper action response in ROS2 + iroc_fleet_manager::GetMissionPointsSrv get_mission_points_service; + const auto resp = + callService(sc_get_mission_points_, get_mission_points_service.request, get_mission_points_service.response); + + if (!resp.success) { + json error_response; + error_response["message"] = resp.message; + error_response["success"] = false; + error_response["robot_data"] = json::list(); + ROS_WARN_STREAM("[IROCBridge]: " << error_response["message"].dump()); + return crow::response(crow::status::INTERNAL_SERVER_ERROR, error_response.dump()); } + + auto mission_goal = get_mission_points_service.response.mission_robots; + auto json = missionGoalToJson(mission_goal); + return crow::response(crow::status::CREATED, json.dump()); } catch (const std::exception &e) { ROS_WARN_STREAM("[IROCBridge]: Failed to parse JSON from message: " << e.what()); From 4b1e93ddab7c8b3c3adcff924b19ebc86568528a Mon Sep 17 00:00:00 2001 From: marlonriv Date: Tue, 26 Aug 2025 17:33:14 +0200 Subject: [PATCH 14/33] Replace NULL to nullptr --- src/iroc_bridge.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index 44f8d3b..b6f8e3b 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -550,7 +550,7 @@ void IROCBridge::missionActiveCallback() { /* missionDoneCallback //{ */ template void IROCBridge::missionDoneCallback(const SimpleClientGoalState& state, const boost::shared_ptr& result) { - if (result == NULL) { + if (result == nullptr) { ROS_WARN("[IROCBridge]: Probably fleet_manager died, and action server connection was lost!, reconnection is not currently handled, if mission manager was " "restarted need to upload a new mission!"); From acb19b02c167b13ae4dd1621744ea787eb773de3 Mon Sep 17 00:00:00 2001 From: marlonriv Date: Thu, 28 Aug 2025 18:27:25 +0200 Subject: [PATCH 15/33] Obstacles improvement Possibility to add multiple obstacles at once, and possibility to keep obstacles when setting a new safety area border. --- json_examples/set_border.json | 3 +- json_examples/set_obstacle.json | 80 ++++++++++++++++++--- src/iroc_bridge.cpp | 124 +++++++++++++++++++------------- 3 files changed, 148 insertions(+), 59 deletions(-) diff --git a/json_examples/set_border.json b/json_examples/set_border.json index de6bbaa..c00bc30 100644 --- a/json_examples/set_border.json +++ b/json_examples/set_border.json @@ -7,5 +7,6 @@ ], "height_id": 0, "max_z": 15.4, - "min_z": 0.5 + "min_z": 0.5, + "keep_obstacles": true } diff --git a/json_examples/set_obstacle.json b/json_examples/set_obstacle.json index 04e5a0d..9416bcb 100644 --- a/json_examples/set_obstacle.json +++ b/json_examples/set_obstacle.json @@ -1,11 +1,73 @@ { - "points": [ - {"x": 47.397760, "y": 8.545254}, - {"x": 47.397719, "y": 8.545436}, - {"x": 47.397601, "y": 8.545367}, - {"x": 47.397657, "y": 8.545191} - ], - "height_id": 1, - "max_z": 347, - "min_z": 343 + "obstacles": [ + { + "points": [ + { + "x": 47.39776, + "y": 8.545254 + }, + { + "x": 47.397719, + "y": 8.545436 + }, + { + "x": 47.397601, + "y": 8.545367 + }, + { + "x": 47.397657, + "y": 8.545191 + } + ], + "height_id": 1, + "max_z": 347, + "min_z": 343 + }, + { + "points": [ + { + "x": 47.397900, + "y": 8.545800 + }, + { + "x": 47.397855, + "y": 8.545950 + }, + { + "x": 47.397750, + "y": 8.545890 + }, + { + "x": 47.397795, + "y": 8.545740 + } + ], + "height_id": 1, + "max_z": 350, + "min_z": 345 + }, + { + "points": [ + { + "x": 47.398100, + "y": 8.545100 + }, + { + "x": 47.398050, + "y": 8.545250 + }, + { + "x": 47.397950, + "y": 8.545200 + }, + { + "x": 47.398000, + "y": 8.545050 + } + ], + "height_id": 1, + "max_z": 352, + "min_z": 348 + } + ] } diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index b6f8e3b..c54f9b6 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -1234,9 +1234,10 @@ crow::response IROCBridge::setSafetyBorderCallback(const crow::request& req) { bool enabled = true; // Defined default as true // Get message properties - int height_id = json_msg["height_id"].i(); - int max_z = json_msg["max_z"].i(); - int min_z = json_msg["min_z"].i(); + int height_id = json_msg["height_id"].i(); + int max_z = json_msg["max_z"].i(); + int min_z = json_msg["min_z"].i(); + bool keep_obstacles = json_msg["keep_obstacles"].b(); std::vector points = json_msg["points"].lo(); std::string horizontal_frame = "latlon_origin"; @@ -1285,7 +1286,8 @@ crow::response IROCBridge::setSafetyBorderCallback(const crow::request& req) { // check that all robot names are valid and find the corresponding robot handlers mrs_msgs::SetSafetyBorderSrvRequest service_request; - service_request.safety_border = safety_border; + service_request.safety_border = safety_border; + service_request.keep_obstacles = keep_obstacles; mrs_msgs::SetSafetyBorderSrv::Request req_srv = service_request; const auto result = commandAction(robot_names, "set_safety_border", req_srv); @@ -1344,74 +1346,98 @@ crow::response IROCBridge::getSafetyBorderCallback(const crow::request &req) { * \param req Crow request * \return res Crow response */ -crow::response IROCBridge::setObstacleCallback(const crow::request& req) { - +crow::response IROCBridge::setObstacleCallback(const crow::request &req) { ROS_INFO_STREAM("[IROCBridge]: Parsing a setObstacleCallback message JSON -> ROS."); - crow::json::rvalue json_msg = crow::json::load(req.body); if (!json_msg) return crow::response(crow::status::BAD_REQUEST, "Failed to parse JSON" + req.body); - // Get message properties - int height_id = json_msg["height_id"].i(); - int max_z = json_msg["max_z"].i(); - int min_z = json_msg["min_z"].i(); - std::vector points = json_msg["points"].lo(); + // Check if obstacles array exists + if (!json_msg.has("obstacles") || !json_msg["obstacles"]) { + return crow::response(crow::status::BAD_REQUEST, "Missing obstacles array: " + req.body); + } - std::string horizontal_frame = "latlon_origin"; - std::string vertical_frame; + std::vector obstacles = json_msg["obstacles"].lo(); + if (obstacles.empty()) { + return crow::response(crow::status::BAD_REQUEST, "Empty obstacles array: " + req.body); + } + std::string horizontal_frame = "latlon_origin"; std::map height_id_map = { {0, "world_origin"}, {1, "latlon_origin"}, }; - auto it = height_id_map.find(height_id); - if (it != height_id_map.end()) // check if the height_id is valid (exists in the map) - vertical_frame = it->second; - - else - return crow::response(crow::status::BAD_REQUEST, "Unknown height_id field: " + req.body); - - if (points.empty()) - return crow::response(crow::status::BAD_REQUEST, "Empty points array: " + req.body); + // Get robot names once (outside the loop) + std::scoped_lock lck(robot_handlers_.mtx); + std::vector robot_names; + robot_names.reserve(robot_handlers_.handlers.size()); + for (const auto &rh : robot_handlers_.handlers) + robot_names.push_back(rh.robot_name); - // Process points - std::vector border_points; - border_points.reserve(points.size()); + // Process each obstacle + for (size_t i = 0; i < obstacles.size(); ++i) { + const auto &obstacle = obstacles[i]; - for (const auto& el : points) { - if (!el.has("x") || !el.has("y")) - return crow::response(crow::status::BAD_REQUEST, "Missing x or y in point: " + req.body); + // Get obstacle properties + if (!obstacle.has("height_id") || !obstacle.has("max_z") || !obstacle.has("min_z") || !obstacle.has("points")) { + return crow::response(crow::status::BAD_REQUEST, "Missing required fields in obstacle " + std::to_string(i) + ": " + req.body); + } - mrs_msgs::Point2D pt; - pt.x = el["x"].d(); - pt.y = el["y"].d(); + int height_id = obstacle["height_id"].i(); + int max_z = obstacle["max_z"].i(); + int min_z = obstacle["min_z"].i(); + std::vector points = obstacle["points"].lo(); - border_points.push_back(pt); - } + // Validate height_id + std::string vertical_frame; + auto it = height_id_map.find(height_id); + if (it != height_id_map.end()) { + vertical_frame = it->second; + } else { + return crow::response(crow::status::BAD_REQUEST, "Unknown height_id field in obstacle " + std::to_string(i) + ": " + req.body); + } - // Logging - ROS_INFO("[IROCBridge]: Obstacle border points size %zu ", border_points.size()); + if (points.empty()) { + return crow::response(crow::status::BAD_REQUEST, "Empty points array in obstacle " + std::to_string(i) + ": " + req.body); + } - mrs_msgs::SetObstacleSrvRequest obstacle_req; + // Process points + std::vector border_points; + border_points.reserve(points.size()); + for (const auto &el : points) { + if (!el.has("x") || !el.has("y")) { + return crow::response(crow::status::BAD_REQUEST, "Missing x or y in point for obstacle " + std::to_string(i) + ": " + req.body); + } + mrs_msgs::Point2D pt; + pt.x = el["x"].d(); + pt.y = el["y"].d(); + border_points.push_back(pt); + } - obstacle_req.horizontal_frame = horizontal_frame; - obstacle_req.vertical_frame = vertical_frame; - obstacle_req.points = border_points; - obstacle_req.max_z = max_z; - obstacle_req.min_z = min_z; + // Logging + ROS_INFO("[IROCBridge]: Obstacle %zu border points size %zu", i, border_points.size()); - std::scoped_lock lck(robot_handlers_.mtx); + // Create obstacle request + mrs_msgs::SetObstacleSrvRequest obstacle_req; + obstacle_req.horizontal_frame = horizontal_frame; + obstacle_req.vertical_frame = vertical_frame; + obstacle_req.points = border_points; + obstacle_req.max_z = max_z; + obstacle_req.min_z = min_z; - std::vector robot_names; - robot_names.reserve(robot_handlers_.handlers.size()); - for (const auto& rh : robot_handlers_.handlers) - robot_names.push_back(rh.robot_name); + // Call service for this obstacle + const auto result = commandAction(robot_names, "set_obstacle", obstacle_req); - const auto result = commandAction(robot_names, "set_obstacle", obstacle_req); + // Check if the service call failed + if (!result.success) { + ROS_ERROR("[IROCBridge]: Failed to set obstacle %zu: %s", i, result.message.c_str()); + return crow::response(result.status_code, "Failed to set obstacle " + std::to_string(i) + ": " + result.message); + } + } - return crow::response(result.status_code, result.message); + ROS_INFO("[IROCBridge]: Successfully set %zu obstacles", obstacles.size()); + return crow::response(crow::status::OK, "Successfully set " + std::to_string(obstacles.size()) + " obstacles"); } //} From 4f5f77f320b150871d7569c95069ea942943158e Mon Sep 17 00:00:00 2001 From: marlonriv Date: Fri, 29 Aug 2025 15:43:36 +0200 Subject: [PATCH 16/33] Getter of loaded mission Added type and UUID useful for synchronization with the UI backend. --- json_examples/missions/autonomy_test.json | 1 + json_examples/missions/coverage.json | 6 +-- json_examples/missions/waypoint.json | 9 ++-- src/iroc_bridge.cpp | 57 +++++++++++++++++++---- 4 files changed, 55 insertions(+), 18 deletions(-) diff --git a/json_examples/missions/autonomy_test.json b/json_examples/missions/autonomy_test.json index 33b6275..88b533b 100644 --- a/json_examples/missions/autonomy_test.json +++ b/json_examples/missions/autonomy_test.json @@ -1,5 +1,6 @@ { "type": "AutonomyTestPlanner", + "uuid": "20ab7a6c-231b-48ed-83cc-864041ae40bd", "details": { "robots": [ { diff --git a/json_examples/missions/coverage.json b/json_examples/missions/coverage.json index ac36cb3..e392f37 100644 --- a/json_examples/missions/coverage.json +++ b/json_examples/missions/coverage.json @@ -1,10 +1,8 @@ { "type": "CoveragePlanner", + "uuid": "b5aaa323-64e5-4eb8-8615-e4059fe84997", "details": { - "robots": [ - "uav1", - "uav2" - ], + "robots": ["uav1", "uav2"], "search_area": [ { "x": 47.397978, diff --git a/json_examples/missions/waypoint.json b/json_examples/missions/waypoint.json index c18001b..7dac8e8 100644 --- a/json_examples/missions/waypoint.json +++ b/json_examples/missions/waypoint.json @@ -1,5 +1,6 @@ { "type": "WaypointPlanner", + "uuid": "550e8400-e29b-41d4-a716-446655440000", "details": { "robots": [ { @@ -26,8 +27,8 @@ "heading": 3, "subtasks": [ { - "type": 1, - "parameters": [0.5,0.5,0.5] + "type": "gazebo_gimbal", + "parameters": [0.5, 0.5, 0.5] } ] }, @@ -44,8 +45,8 @@ "heading": 3, "subtasks": [ { - "type": 0, - "parameters": "5.6" + "type": "wait", + "parameters": 5.6 } ] }, diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index c54f9b6..09389cf 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -58,6 +58,7 @@ #include #include +#include #include #include @@ -212,6 +213,7 @@ class IROCBridge : public nodelet::Nodelet { crow::response setObstacleCallback(const crow::request& req); crow::response getObstaclesCallback(const crow::request& req); crow::response missionCallback(const crow::request& req); + crow::response getMissionCallback(const crow::request& req); crow::response changeFleetMissionStateCallback(const crow::request& req, const std::string& type); crow::response changeRobotMissionStateCallback(const crow::request& req, const std::string& robot_name, const std::string& type); @@ -320,6 +322,7 @@ void IROCBridge::onInit() { CROW_ROUTE(http_srv_, "/safety-area/world-origin").methods(crow::HTTPMethod::Get)([this](const crow::request& req) { return getOriginCallback(req); }); CROW_ROUTE(http_srv_, "/safety-area/borders").methods(crow::HTTPMethod::Get)([this](const crow::request& req) { return getSafetyBorderCallback(req); }); CROW_ROUTE(http_srv_, "/safety-area/obstacles").methods(crow::HTTPMethod::Get)([this](const crow::request& req) { return getObstaclesCallback(req); }); + CROW_ROUTE(http_srv_, "/mission").methods(crow::HTTPMethod::Get)([this](const crow::request& req) { return getMissionCallback(req); }); // Missions // TODO: CROW_REGEX_ROUTE(http_srv_, R"(/fleet/mission/(start|stop|pause))") @@ -1035,14 +1038,17 @@ json resultToJson(const boost::shared_ptr& result) { /* missionGoalToJson() method //{ */ -json missionGoalToJson(std::vector &mission_goals) { +json missionGoalToJson(const iroc_fleet_manager::IROCFleetMissionGoal &mission_goal) { json robots_data = json::list(); - for (size_t i = 0; i < mission_goals.size(); i++) { - std::string robot_name = mission_goals.at(i).name; - auto points = mission_goals.at(i).points; - int frame_id = mission_goals.at(i).frame_id; - int height_id = mission_goals.at(i).height_id; + + auto robot_goals = mission_goal.robot_goals; + + for (size_t i = 0; i < robot_goals.size(); i++) { + std::string robot_name = robot_goals.at(i).name; + auto points = robot_goals.at(i).points; + int frame_id = robot_goals.at(i).frame_id; + int height_id = robot_goals.at(i).height_id; // Extract the points json points_list = json::list(); @@ -1056,7 +1062,7 @@ json missionGoalToJson(std::vector &mission_g robots_data[i] = std::move(robot_data); } - json response = {{"success", true}, {"message", "Mission uploaded successfully"}, {"robot_data", robots_data}}; + json response = {{"success", true}, {"message", "Mission uploaded successfully"}, {"type", mission_goal.type}, {"uuid", mission_goal.uuid}, {"robot_data", robots_data}}; return response; } @@ -1237,7 +1243,6 @@ crow::response IROCBridge::setSafetyBorderCallback(const crow::request& req) { int height_id = json_msg["height_id"].i(); int max_z = json_msg["max_z"].i(); int min_z = json_msg["min_z"].i(); - bool keep_obstacles = json_msg["keep_obstacles"].b(); std::vector points = json_msg["points"].lo(); std::string horizontal_frame = "latlon_origin"; @@ -1287,7 +1292,7 @@ crow::response IROCBridge::setSafetyBorderCallback(const crow::request& req) { // check that all robot names are valid and find the corresponding robot handlers mrs_msgs::SetSafetyBorderSrvRequest service_request; service_request.safety_border = safety_border; - service_request.keep_obstacles = keep_obstacles; + service_request.keep_obstacles = false; mrs_msgs::SetSafetyBorderSrv::Request req_srv = service_request; const auto result = commandAction(robot_names, "set_safety_border", req_srv); @@ -1495,6 +1500,36 @@ crow::response IROCBridge::getObstaclesCallback(const crow::request &req) { } //} +/* getMissionCallback() method //{ */ + +/** + * \brief Callback to get the loaded mission. + * + * \param req Crow request + * \return res Crow response + */ +crow::response IROCBridge::getMissionCallback(const crow::request &req) { + + ROS_INFO_STREAM("[IROCBridge]: Processing a getMissionCallback message ROS -> JSON."); + iroc_fleet_manager::GetMissionPointsSrv get_mission_points_service; + const auto resp = + callService(sc_get_mission_points_, get_mission_points_service.request, get_mission_points_service.response); + + if (!resp.success) { + json error_response; + error_response["message"] = resp.message; + error_response["success"] = false; + error_response["robot_data"] = json::list(); + ROS_WARN_STREAM("[IROCBridge]: " << error_response["message"].dump()); + return crow::response(crow::status::INTERNAL_SERVER_ERROR, error_response.dump()); + } + + auto mission_goal = get_mission_points_service.response.mission_goal; + auto json = missionGoalToJson(mission_goal); + return crow::response(crow::status::OK, json.dump()); +} +//} + /* missionCallback() method //{ */ /** @@ -1525,12 +1560,14 @@ crow::response IROCBridge::missionCallback(const crow::request &req) { // Send the action goal to the fleet manager FleetManagerGoal action_goal; std::string type = json_msg["type"].s(); + std::string uuid = json_msg["uuid"].s(); // Convert rvalue to wvalue, then dump to string crow::json::wvalue details_wvalue(json_msg["details"]); std::string details = details_wvalue.dump(); action_goal.type = type; + action_goal.uuid = uuid; action_goal.details = details; fleet_manager_action_client_->sendGoal( @@ -1563,7 +1600,7 @@ crow::response IROCBridge::missionCallback(const crow::request &req) { return crow::response(crow::status::INTERNAL_SERVER_ERROR, error_response.dump()); } - auto mission_goal = get_mission_points_service.response.mission_robots; + auto mission_goal = get_mission_points_service.response.mission_goal; auto json = missionGoalToJson(mission_goal); return crow::response(crow::status::CREATED, json.dump()); } From 0731c1297ed81f67121b6bc885589d38ab5df622 Mon Sep 17 00:00:00 2001 From: marlonriv Date: Fri, 29 Aug 2025 15:57:53 +0200 Subject: [PATCH 17/33] Update of the service naming from fleet manager\ --- launch/iroc_bridge.launch | 2 +- src/iroc_bridge.cpp | 20 ++++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/launch/iroc_bridge.launch b/launch/iroc_bridge.launch index bc4f6ec..cb71522 100644 --- a/launch/iroc_bridge.launch +++ b/launch/iroc_bridge.launch @@ -70,7 +70,7 @@ - + diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index 09389cf..a224691 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -172,7 +172,7 @@ class IROCBridge : public nodelet::Nodelet { ros::ServiceClient sc_get_world_origin_; ros::ServiceClient sc_get_safety_border_; ros::ServiceClient sc_get_obstacles_; - ros::ServiceClient sc_get_mission_points_; + ros::ServiceClient sc_get_mission_data_; // | ----------------- action client callbacks ---------------- | @@ -476,9 +476,9 @@ void IROCBridge::onInit() { ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/get_obstacles\' -> \'%s\'", sc_get_obstacles_.getService().c_str()); - sc_get_mission_points_ = nh_.serviceClient(nh_.resolveName("svc/get_mission_points")); - ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/get_mission_points\' -> \'%s\'", - sc_get_mission_points_.getService().c_str()); + sc_get_mission_data_ = nh_.serviceClient(nh_.resolveName("svc/get_mission_data")); + ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/get_mission_data\' -> \'%s\'", + sc_get_mission_data_.getService().c_str()); /* // | --------------------- action clients --------------------- | */ @@ -1511,9 +1511,9 @@ crow::response IROCBridge::getObstaclesCallback(const crow::request &req) { crow::response IROCBridge::getMissionCallback(const crow::request &req) { ROS_INFO_STREAM("[IROCBridge]: Processing a getMissionCallback message ROS -> JSON."); - iroc_fleet_manager::GetMissionPointsSrv get_mission_points_service; + iroc_fleet_manager::GetMissionPointsSrv get_mission_data_service; const auto resp = - callService(sc_get_mission_points_, get_mission_points_service.request, get_mission_points_service.response); + callService(sc_get_mission_data_, get_mission_data_service.request, get_mission_data_service.response); if (!resp.success) { json error_response; @@ -1524,7 +1524,7 @@ crow::response IROCBridge::getMissionCallback(const crow::request &req) { return crow::response(crow::status::INTERNAL_SERVER_ERROR, error_response.dump()); } - auto mission_goal = get_mission_points_service.response.mission_goal; + auto mission_goal = get_mission_data_service.response.mission_goal; auto json = missionGoalToJson(mission_goal); return crow::response(crow::status::OK, json.dump()); } @@ -1587,9 +1587,9 @@ crow::response IROCBridge::missionCallback(const crow::request &req) { } // TODO to replace with proper action response in ROS2 - iroc_fleet_manager::GetMissionPointsSrv get_mission_points_service; + iroc_fleet_manager::GetMissionPointsSrv get_mission_data_service; const auto resp = - callService(sc_get_mission_points_, get_mission_points_service.request, get_mission_points_service.response); + callService(sc_get_mission_data_, get_mission_data_service.request, get_mission_data_service.response); if (!resp.success) { json error_response; @@ -1600,7 +1600,7 @@ crow::response IROCBridge::missionCallback(const crow::request &req) { return crow::response(crow::status::INTERNAL_SERVER_ERROR, error_response.dump()); } - auto mission_goal = get_mission_points_service.response.mission_goal; + auto mission_goal = get_mission_data_service.response.mission_goal; auto json = missionGoalToJson(mission_goal); return crow::response(crow::status::CREATED, json.dump()); } From 705fa4c992f35ff77b2fa7ac5f9e5e8468ef0d46 Mon Sep 17 00:00:00 2001 From: marlonriv Date: Mon, 1 Sep 2025 17:37:31 +0200 Subject: [PATCH 18/33] Updated mission examples and included frame_id and height_id in getter of borders and obstacles --- json_examples/missions/heading.json | 1 + json_examples/missions/one_drone.json | 1 + .../missions/sequential_subtask.json | 3 +- json_examples/missions/test.json | 27 ++++++++++++++++++ json_examples/missions/two_drones.json | 1 + json_examples/missions/waypoint.json | 3 +- src/iroc_bridge.cpp | 28 +++++++++++++++++-- 7 files changed, 59 insertions(+), 5 deletions(-) create mode 100644 json_examples/missions/test.json diff --git a/json_examples/missions/heading.json b/json_examples/missions/heading.json index b6ee999..fdeaca0 100644 --- a/json_examples/missions/heading.json +++ b/json_examples/missions/heading.json @@ -1,5 +1,6 @@ { "type": "WaypointPlanner", + "uuid": "550e8400-e29b-41d4-a716-446655440000", "details": { "robots": [ { diff --git a/json_examples/missions/one_drone.json b/json_examples/missions/one_drone.json index 2cdb07e..8a714a4 100644 --- a/json_examples/missions/one_drone.json +++ b/json_examples/missions/one_drone.json @@ -1,5 +1,6 @@ { "type": "WaypointPlanner", + "uuid": "550e8400-e29b-41d4-a716-446655440000", "details": { "robots": [ { diff --git a/json_examples/missions/sequential_subtask.json b/json_examples/missions/sequential_subtask.json index 8b01918..26fdf9e 100644 --- a/json_examples/missions/sequential_subtask.json +++ b/json_examples/missions/sequential_subtask.json @@ -1,5 +1,6 @@ { "type": "WaypointPlanner", + "uuid": "550e8400-e29b-41d4-a716-446655440000", "details": { "robots": [ { @@ -26,7 +27,7 @@ "heading": 3, "subtasks": [ { - "type": "test", + "type": "gazebo_gimbal", "parameters": [ 0.5, 0.5, diff --git a/json_examples/missions/test.json b/json_examples/missions/test.json new file mode 100644 index 0000000..fbd1c3b --- /dev/null +++ b/json_examples/missions/test.json @@ -0,0 +1,27 @@ +{ + "min_z":0.499985000000000012754, + "max_z":14.9999850000000005679, + "points":[ + { + "x":-53.1132580000000018572, + "y":-52.640006999999997106 + }, + { + "y":47.673400999999998362, + "x":-45.8052520000000029654 + }, + { + "x":46.8867419999999981428, + "y":47.359993000000002894 + }, + { + "x":46.8867419999999981428, + "y":-52.640006999999997106 + }, + { + "y":-52.640006999999997106, + "x":-53.1132580000000018572 + } + ], + "message":"All robots in the fleet with the same safety border" +} diff --git a/json_examples/missions/two_drones.json b/json_examples/missions/two_drones.json index ff71627..1856c0a 100644 --- a/json_examples/missions/two_drones.json +++ b/json_examples/missions/two_drones.json @@ -1,5 +1,6 @@ { "type": "WaypointPlanner", + "uuid": "550e8400-e29b-41d4-a716-446655440000", "details": { "robots": [ { diff --git a/json_examples/missions/waypoint.json b/json_examples/missions/waypoint.json index 7dac8e8..c31822b 100644 --- a/json_examples/missions/waypoint.json +++ b/json_examples/missions/waypoint.json @@ -1,5 +1,4 @@ -{ - "type": "WaypointPlanner", +{ "type": "WaypointPlanner", "uuid": "550e8400-e29b-41d4-a716-446655440000", "details": { "robots": [ diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index a224691..bad0723 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -1309,6 +1309,21 @@ crow::response IROCBridge::setSafetyBorderCallback(const crow::request& req) { * \param req Crow request * \return res Crow response */ + +int getFrameID(const std::string frame) { + std::map height_id_map = { + {"world_origin", 0}, + {"latlon_origin", 1}, + }; + + int id; + auto it = height_id_map.find(frame); + if (it != height_id_map.end()) + id = it->second; + + return id; +} + crow::response IROCBridge::getSafetyBorderCallback(const crow::request &req) { ROS_INFO_STREAM("[IROCBridge]: Processing a getSafetyBorderCallback message ROS -> JSON."); @@ -1337,7 +1352,12 @@ crow::response IROCBridge::getSafetyBorderCallback(const crow::request &req) { points[i] = std::move(point_json); } - json json_msg = {{"message", "All robots in the fleet with the same safety border"}, {"points", points}, {"max_z", max_z}, {"min_z", min_z}}; + json json_msg = {{"message", "All robots in the fleet with the same safety border"}, + {"points", points}, + {"max_z", max_z}, + {"min_z", min_z}, + {"frame_id", getFrameID(horizontal_frame)}, + {"height_id", getFrameID(vertical_frame)}}; return crow::response(crow::status::ACCEPTED, json_msg.dump()); } } @@ -1490,7 +1510,11 @@ crow::response IROCBridge::getObstaclesCallback(const crow::request &req) { } } - json obstacle_json = {{"points", points_list},{"max_z", max_z}, {"min_z", min_z}}; + json obstacle_json = {{"points", points_list}, + {"max_z", max_z}, + {"min_z", min_z}, + {"frame_id", getFrameID(obstacles.horizontal_frame)}, + {"height_id", getFrameID(obstacles.vertical_frame)}}; obstacles_json_list[i] = std::move(obstacle_json); } From 76da829d96d94cce640d38f0a2551b20b440e123 Mon Sep 17 00:00:00 2001 From: Marlon Rivera <53498345+MarlonRiv@users.noreply.github.com> Date: Tue, 2 Sep 2025 11:59:23 +0200 Subject: [PATCH 19/33] Partially updating the signaling protocol --- docs/README.md | 603 +++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 488 insertions(+), 115 deletions(-) diff --git a/docs/README.md b/docs/README.md index 30016d7..8a1b19e 100644 --- a/docs/README.md +++ b/docs/README.md @@ -95,42 +95,107 @@ Endpoints for controlling the robot's environment. -- `POST` - **/safety-area/borders** +- `GET` + **/safety-area/origin** - Set the safety area borders. + Retrieve the world origin. -
+ + +
Body raw (json) + + Status code: **202 Accepted** + + ```json + ``` +
+ + - `POST` + **/safety-area/borders** + + Set the safety area borders. + +
+ + Body raw (json) + + + ```json + { + "points": [ + { + "x": 47.39776, + "y": 8.545254 + }, + { + "x": 47.397719, + "y": 8.545436 + }, + { + "x": 47.397601, + "y": 8.545367 + }, + { + "x": 47.397657, + "y": 8.545191 + } + ], + "height_id": 1, + "max_z": 347, + "min_z": 343 + } + ``` + +
+- `GET` + **/safety-area/borders** + + Retrieve the safety border. + + + +
+ + Body raw (json) + + + Status code: **202 Accepted** + ```json { - "points": [ + "frame_id":1, + "min_z":0, + "points":[ { - "x": 47.39776, - "y": 8.545254 + "x":47.398283001578619178, + "y":8.54231999998631863491 }, { - "x": 47.397719, - "y": 8.545436 + "y":8.5464419999864418287, + "x":47.3979670015785998771 }, { - "x": 47.397601, - "y": 8.545367 + "x":47.3972980015784983721, + "y":8.54602099998643183199 }, { - "x": 47.397657, - "y": 8.545191 + "x":47.3975810015785512519, + "y":8.5447099999863915798 + }, + { + "y":8.54231999998631863491, + "x":47.398283001578619178 } - ], - "height_id": 1, - "max_z": 347, - "min_z": 343 + ], + "height_id":0, + "max_z":15, + "message":"All robots in the fleet with the same safety border" } ``` -
- `POST` @@ -171,10 +236,130 @@ Endpoints for controlling the robot's environment.
-### Missions +- `GET` + **/safety-area/obstacles** + + Retrieve the obstacles. + + + +
+ + Body raw (json) + + + Status code: **202 Accepted** + + ```json + { + "obstacles":[ + { + "height_id":0, + "max_z":7.05999999660582489014, + "frame_id":1, + "min_z":3.05999999660582489014, + "points":[ + { + "x":47.3977600015785611731, + "y":8.54525399998640722288 + }, + { + "y":8.54543599998641312254, + "x":47.3977190015785652122 + }, + { + "x":47.3976010015785576002, + "y":8.54536699998641147147 + }, + { + "x":47.3976570015785512169, + "y":8.54519099998640641047 + }, + { + "y":8.54525399998640722288, + "x":47.3977600015785611731 + } + ] + }, + { + "points":[ + { + "y":8.54579999998642314551, + "x":47.3979000015785771893 + }, + { + "x":47.3978550015785771166, + "y":8.54594999998642634864 + }, + { + "y":8.54588999998642506739, + "x":47.3977500015785722098 + }, + { + "y":8.54573999998642364062, + "x":47.3977950015785864935 + }, + { + "x":47.3979000015785771893, + "y":8.54579999998642314551 + } + ], + "min_z":5.05999999660582489014, + "frame_id":1, + "max_z":10.0599999966058248901, + "height_id":0 + }, + { + "height_id":0, + "max_z":12.0599999966058248901, + "frame_id":1, + "min_z":6.44836723768419848974e-31, + "points":[ + { + "x":47.3981000015786122503, + "y":8.54509999998640168428 + }, + { + "y":8.54524999998640488741, + "x":47.3980500015786105905 + }, + { + "x":47.39795000157859306, + "y":8.54519999998640500394 + }, + { + "x":47.3980000015786018253, + "y":8.54504999998640002445 + }, + { + "y":8.54509999998640168428, + "x":47.3981000015786122503 + } + ] + } + ], + "message":"All robots in the fleet with the same obstacles" + } + ``` +
+ + > **NOTE** \ + > In case of any discrepancy within the robot's safety border, obstacles & origin, having different values +You will receive a Status code **409 Conflict**, and a message to let the user know about the conflict: + +```json +{"message": "Call was not successful with message: Discrepancy in the borders between the fleet, please set the safety borders!"} +``` -The missions are handled by `IROC Fleet Manager`: node responsible of sending the mission to the robots, monitoring their progress and sending the aggregated information to the `IROC Bridge`. +### Missions +The missions are handled by `IROC Fleet Manager`: a node responsible for sending the mission to the robots, monitoring their progress, and sending the aggregated information to the `IROC Bridge`. +- `POST` + **/mission** + + Set a mission + +
@@ -184,124 +369,225 @@ The missions are handled by `IROC Fleet Manager`: node responsible of sending th
Mission Sequence Diagram
-- `POST` - **/mission/waypoints** - - Set the waypoints for the mission. - + The mission request requires the following fields: + - **type key** to specify the mission type and the specific details of the mission. + - **details key** with the specific details for each mission. + - **Uuid key** for synchronization with the UI. +
- Body raw (json) + WaypointPlanner: Body raw (json) ```json { - "mission": [ - { - "robot_name": "uav1", - "frame_id": 0, - "height_id": 0, - "points": [ - { "x": 10, "y": 10, "z": 2, "heading": 1 }, - { "x": -10, "y": 10, "z": 2, "heading": 3 } - ], - "terminal_action": 0 - }, - { - "robot_name": "uav2", - "frame_id": 0, - "height_id": 0, - "points": [ - { "x": 20, "y": 5, "z": 3, "heading": 0 } - ], - "terminal_action": 0 - } - ] + "type": "WaypointPlanner", + "uuid": "550e8400-e29b-41d4-a716-446655440000", + "details": { + "robots": [ + { + "name": "uav1", + "frame_id": 0, + "height_id": 0, + "points": [ + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1 + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 3 + }, + { + "x": -20, + "y": -20, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": "gazebo_gimbal", + "parameters": [0.5, 0.5, 0.5] + } + ] + }, + { + "x": -10, + "y": 10, + "z": 5, + "heading": 3 + }, + { + "x": 10, + "y": -10, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": "wait", + "parameters": 5.6 + } + ] + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1 + } + ], + "terminal_action": 0 + } + ] + } } ```
-- `POST` - **/mission/coverage** - - Set a coverage mission. -
- Body raw (json) + Coverage Planner: Body raw (json) ```json - { - "robots": [ - "uav1", - "uav2" - ], - "search_area": [ - {"x": 47.397978, "y": 8.545299}, - {"x": 47.397848, "y": 8.545872}, - {"x": 47.397551, "y": 8.545720}, - {"x": 47.397699, "y": 8.545129} - ], - "height_id": 0, - "height": 10, - "terminal_action": 0 + { + "type": "CoveragePlanner", + "uuid": "b5aaa323-64e5-4eb8-8615-e4059fe84997", + "details": { + "robots": ["uav1", "uav2"], + "search_area": [ + { + "x": 47.397978, + "y": 8.545299 + }, + { + "x": 47.397848, + "y": 8.545872 + }, + { + "x": 47.397551, + "y": 8.54572 + }, + { + "x": 47.397699, + "y": 8.545129 + } + ], + "height_id": 0, + "height": 5, + "terminal_action": 0 + } } ```
- -- `POST` - **/mission/autonomy-test** - - Set the autonomy test mission. - +
- Body raw (json) + AutonomyTestPlanner: Body raw (json) ```json { - "robot_name": "uav1", - "segment_length": 2 + "type": "AutonomyTestPlanner", + "uuid": "20ab7a6c-231b-48ed-83cc-864041ae40bd", + "details": { + "robots": [ + { + "name": "uav1", + "segment_length": 5 + } + ] + } } ```
-#### Mission Response Examples + +### Mission Response Examples + +The result follows the following structure: +- **message**: General message about the status of the mission. +- **success**: Boolean to denote if the mission was uploaded successfully. +- **type**: Type of the mission. +- **uuid**: The UUID of the mission. +- **robot_data**: An array with details for each robot in the mission. + - **robot**: String with the name of the robot. + - **message**: Individual robot message. + - **success**: Boolean to denote the individual result of the robot. + - **mission**: The details of the mission that were loaded into the robot. 1. Successful mission upload -
- - - Body example response (json) - -- Code: 201 Created - -- Example value: - -```json -{ - "message":"Mission uploaded successfully", - "success":true, - "robot_results":[ - { - "robot_name":"uav1", - "success":true, - "message":"Robot received the mission successfully" - }, - { - "message":"Robot received the mission successfully", - "success":true, - "robot_name":"uav2" - } - ] -} -``` - -
+ +
+ + Body example response (json) + + Code: 201 Created + + ```json + { + "robot_data":[ + { + "message": "Mission loaded successfully", + "mission":{ + "frame_id":0, + "height_id":0, + "points":[ + { + "x":20, + "y":10, + "heading":1, + "z":3 + }, + { + "z":3, + "heading":3, + "y":10, + "x":20 + }, + { + "x":-20, + "y":-20, + "heading":3, + "z":4 + }, + { + "x":-10, + "y":10, + "heading":3, + "z":5 + }, + { + "z":4, + "heading":3, + "y":-10, + "x":10 + }, + { + "z":3, + "heading":1, + "y":10, + "x":20 + } + ] + }, + "success": true, + "robot" :"uav1" + } + ], + "uuid":"550e8400-e29b-41d4-a716-446655440000", + "type": "WaypointPlanner", + "message": "Mission uploaded successfully", + "success" :true + } + ``` + +
2. Uploading mission failure due to safety area validation.
@@ -346,17 +632,104 @@ The missions are handled by `IROC Fleet Manager`: node responsible of sending th ```json { - "message": "Mission is already running. Terminate the previous one, or wait until it is finished" + "message": "Mission is already running. Terminate the previous one, or wait until it is finished." } ```
-#### Mission Control Endpoints +### Mission `GET` endpoint + +- `GET` + **/mission** + + Retrieve the mission loaded in the server. + + + +
+ + Body raw (json) + + + Status code: **202 Accepted** + + ```json + { + "robot_data":[ + { + "message": "Mission loaded successfully", + "mission":{ + "frame_id":0, + "height_id":0, + "points":[ + { + "x":20, + "y":10, + "heading":1, + "z":3 + }, + { + "z":3, + "heading":3, + "y":10, + "x":20 + }, + { + "x":-20, + "y":-20, + "heading":3, + "z":4 + }, + { + "x":-10, + "y":10, + "heading":3, + "z":5 + }, + { + "z":4, + "heading":3, + "y":-10, + "x":10 + }, + { + "z":3, + "heading":1, + "y":10, + "x":20 + } + ] + }, + "success":true, + "robot":"uav1" + } + ], + "uuid":"550e8400-e29b-41d4-a716-446655440000", + "type":"WaypointPlanner", + "message":"Mission uploaded successfully", + "success":true + } + ``` +
+ +If there is no active mission, you will get an unsuccessful response, with the message that there is no active mission: + +```json +{ + "robot_data":[ + + ], + "success": false, + "message" :"No active mission." +} +``` + +### Mission Control Endpoints -We support for both fleet-wide and individual robot mission control. +We support both fleet-wide and individual robot mission control. -##### Fleet Mission Control: +#### Fleet Mission Control: These endpoints control the mission status for all assigned robots at once: \ @@ -376,7 +749,7 @@ These endpoints control the mission status for all assigned robots at once: \ Stop the mission for all robots. -##### Robot Mission Control: +#### Robot Mission Control: You can also control individual mission robots using these endpoints: @@ -387,7 +760,7 @@ You can also control individual mission robots using these endpoints: > **NOTE** \ - > Starting a mission for a single robot will activate that robot while the others remain in a waiting state. You can later use the `/mission/start` endpoint to activate the remaining robots and continue the mission. + > Starting a mission for a single robot will activate that robot while the others remain waiting. You can later use the `/mission/start` endpoint to activate the remaining robots and continue the mission. - `POST` **/robots/{_robot_name_}/mission/pause** @@ -402,9 +775,9 @@ You can also control individual mission robots using these endpoints: > **NOTE** \ > Stopping the mission for a single robot will also abort the overall mission and stop all other robots. This behavior is intentional, as the mission assumes the participation of all assigned robots. -#### Feedback +### Feedback -During an active mission, the feedback message is broadcasted to the connected clients through a WebSocket in the `/telemetry` path. +During an active mission, the feedback message is broadcast to the connected clients through a WebSocket in the `/telemetry` path. - `onmessage` **Waypoint Mission and Autonomy Test Feedback.** @@ -451,7 +824,7 @@ During an active mission, the feedback message is broadcasted to the connected c > **NOTE** \ > Autonomy test follows the same structure as the waypoint mission feedback, but it will always contain only one robot. -#### Result +### Result When a mission is finished, the result message will be sent to From e7797792b0e46c7dc9f074867bb5923b06d55c01 Mon Sep 17 00:00:00 2001 From: Marlon Rivera <53498345+MarlonRiv@users.noreply.github.com> Date: Tue, 2 Sep 2025 12:56:50 +0200 Subject: [PATCH 20/33] Diagrams for signaling protocol --- docs/img/mission_diagram.svg | 102 ++++++++++++++++++++++++++++++ docs/img/mission_diagram_dark.svg | 102 ++++++++++++++++++++++++++++++ 2 files changed, 204 insertions(+) create mode 100644 docs/img/mission_diagram.svg create mode 100644 docs/img/mission_diagram_dark.svg diff --git a/docs/img/mission_diagram.svg b/docs/img/mission_diagram.svg new file mode 100644 index 0000000..601d264 --- /dev/null +++ b/docs/img/mission_diagram.svg @@ -0,0 +1,102 @@ +ROSAPIROSAPIloop[Websocket (ROS topic)]POST /mission/201 "Mission created"POST /mission/start200 "Command received"/telemetry type: WaypointMissionFeedbackPOST /api/missions/result \ No newline at end of file diff --git a/docs/img/mission_diagram_dark.svg b/docs/img/mission_diagram_dark.svg new file mode 100644 index 0000000..32a2bf4 --- /dev/null +++ b/docs/img/mission_diagram_dark.svg @@ -0,0 +1,102 @@ +ROSAPIROSAPIloop[Websocket (ROS topic)]POST /mission/201 "Mission created"POST /mission/start200 "Command received"/telemetry type: WaypointMissionFeedbackPOST /api/missions/result \ No newline at end of file From 4a6158d30b62c815a4927f7da62e0739e14ac3ec Mon Sep 17 00:00:00 2001 From: Marlon Rivera <53498345+MarlonRiv@users.noreply.github.com> Date: Tue, 2 Sep 2025 13:14:39 +0200 Subject: [PATCH 21/33] Added subtask documentation into the signaling protocol --- docs/README.md | 350 +++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 308 insertions(+), 42 deletions(-) diff --git a/docs/README.md b/docs/README.md index 8a1b19e..9477be9 100644 --- a/docs/README.md +++ b/docs/README.md @@ -74,7 +74,7 @@ Endpoints for controlling the robot's environment. - `POST` - **/safety-area/origin** + **/safety-area/world-origin** Set the world origin. @@ -96,7 +96,7 @@ Endpoints for controlling the robot's environment. - `GET` - **/safety-area/origin** + **/safety-area/world-origin** Retrieve the world origin. @@ -108,9 +108,14 @@ Endpoints for controlling the robot's environment. Status code: **202 Accepted** - ```json + { + "frame_id": 0, + "x": 47.397978, + "y": 8.545299 + } ``` + - `POST` @@ -209,28 +214,78 @@ Endpoints for controlling the robot's environment. ```json - { - "points": [ - { - "x": 47.39776, - "y": 8.545254 - }, - { - "x": 47.397719, - "y": 8.545436 - }, - { - "x": 47.397601, - "y": 8.545367 - }, - { - "x": 47.397657, - "y": 8.545191 - } - ], - "height_id": 1, - "max_z": 347, - "min_z": 343 + { + "obstacles":[ + { + "points":[ + { + "x":47.39776, + "y":8.545254 + }, + { + "x":47.397719, + "y":8.545436 + }, + { + "x":47.397601, + "y":8.545367 + }, + { + "x":47.397657, + "y":8.545191 + } + ], + "height_id":1, + "max_z":347, + "min_z":343 + }, + { + "points":[ + { + "x":47.397900, + "y":8.545800 + }, + { + "x":47.397855, + "y":8.545950 + }, + { + "x":47.397750, + "y":8.545890 + }, + { + "x":47.397795, + "y":8.545740 + } + ], + "height_id":1, + "max_z":350, + "min_z":345 + }, + { + "points":[ + { + "x":47.398100, + "y":8.545100 + }, + { + "x":47.398050, + "y":8.545250 + }, + { + "x":47.397950, + "y":8.545200 + }, + { + "x":47.398000, + "y":8.545050 + } + ], + "height_id":1, + "max_z":352, + "min_z":348 + } + ] } ``` @@ -351,7 +406,7 @@ You will receive a Status code **409 Conflict**, and a message to let the user k {"message": "Call was not successful with message: Discrepancy in the borders between the fleet, please set the safety borders!"} ``` -### Missions +## Missions The missions are handled by `IROC Fleet Manager`: a node responsible for sending the mission to the robots, monitoring their progress, and sending the aggregated information to the `IROC Bridge`. - `POST` @@ -362,8 +417,8 @@ The missions are handled by `IROC Fleet Manager`: a node responsible for sending
- - + + Sequence Diagram
Mission Sequence Diagram
@@ -507,7 +562,8 @@ The missions are handled by `IROC Fleet Manager`: a node responsible for sending ``` - + + ### Mission Response Examples The result follows the following structure: @@ -589,7 +645,7 @@ The result follows the following structure: -2. Uploading mission failure due to safety area validation. +2. Uploading mission failure due to safety validation.
@@ -601,20 +657,34 @@ The result follows the following structure: ```json { - "message":"Failure starting robot clients.", - "success":false, - "robot_results":[ + "robot_data":[ { - "robot_name":"uav1", - "success":true, - "message":"Mission on robot: uav1 was successfully processed" - }, + "message":"The given points are valid for: uav1, however the generated trajectory seems to be outside of safety area or within an obstacle.", + "mission":[ + + ], + "success":false, + "robot":"uav1" + } + ], + "message":"Failure starting robot clients.", + "success":false +} +``` +```json +{ + "robot_data":[ { - "message":"Unvalid trajectory for uav2, trajectory is outside of safety area", + "message":"Unvalid trajectory for uav1, trajectory is outside of safety area", + "mission":[ + + ], "success":false, - "robot_name":"uav2" + "robot":"uav1" } - ] + ], + "message":"Failure starting robot clients.", + "success":false } ``` @@ -638,6 +708,8 @@ The result follows the following structure:
+ + ### Mission `GET` endpoint - `GET` @@ -725,11 +797,13 @@ If there is no active mission, you will get an unsuccessful response, with the m } ``` + + ### Mission Control Endpoints We support both fleet-wide and individual robot mission control. -#### Fleet Mission Control: +**Fleet Mission Control**: These endpoints control the mission status for all assigned robots at once: \ @@ -749,7 +823,7 @@ These endpoints control the mission status for all assigned robots at once: \ Stop the mission for all robots. -#### Robot Mission Control: +**Robot Mission Control**: You can also control individual mission robots using these endpoints: @@ -860,6 +934,198 @@ Send the result of the mission. + +### Subtasks +On the Waypoint missions, you can send a list of subtasks that will be executed by the robot when it reaches a waypoint. You just need to add a new field called `subtasks` in the waypoint message, which is an array of subtasks. Requests are retro-compatible, so you can use the feature without changing the existing missions or additional fields. + +There are two types of subtasks supported: `wait` and `gazebo_gimbal` (for simulation), but they can be extended in the future due to the ROS `plugin` architecture in `iroc_mission_handler`. + +--- + +### `wait` - Temporal Delay + +Introduces a pause in mission execution for a specified duration. + +#### Parameters + +It receives a floating-point number representing the duration in seconds. + +#### Usage Example + +```json +{ + "type": "wait", + "parameters": 5.0 +} +``` + +--- + +### `gazebo_gimbal` - Camera Orientation Control + +Controls the gimbal system to orient the camera towards specific angles. + +#### Parameters + +It receives an array of three floating-point numbers representing the camera orientation angles in radians (`roll`, `pitch`, `yaw`) + +#### Usage Example + +```json +{ + "type": "gazebo_gimbal", + "parameters": [1.0, 0.5, 0.0] +} +``` + +The subtask system allows you to define complex behaviors that are executed when a robot reaches a waypoint. + +### Waypoint Structure + +Each waypoint now supports the following structure: + +```json +{ + // Standard `x`, `y` and `z` data + "subtasks": [ + // Array of subtask objects + ], + "parallel_execution": false // Optional: Whether subtasks can be executed in parallel +} +``` + +#### Parallel Execution + +- **Default**: `false` (sequential execution) +- **When `true`**: Subtasks are executed simultaneously +- **When `false`**: Subtasks are executed one after another (sequentially) + +### Subtask Configuration + +Each subtask supports advanced execution control through the following fields: + +#### Basic Fields + +These are the fundamental fields for configuring a subtask: + +- `type`: Type of task to execute +- `parameters`: Task-specific parameters (string format) + +#### Advanced Execution Control + +- `continue_without_waiting`: If `true`, the mission continues without waiting for this subtask to complete +- `stop_on_failure`: If `true`, the entire mission stops if this task fails +- `max_retries`: Maximum number of retry attempts if the task fails (0 = no retries) +- `retry_delay`: Delay in seconds before retrying a failed task + +#### Example Subtask Structure + +```json +{ + "type": "wait", + "parameters": "5.0", + "continue_without_waiting": false, + "stop_on_failure": true, + "max_retries": 3, + "retry_delay": 2.0 +} +``` + +#### Mission with multiple subtasks example: +
+ + Body raw (json) + + +```json +{ + "type": "WaypointPlanner", + "uuid": "550e8400-e29b-41d4-a716-446655440000", + "details": { + "robots": [ + { + "name": "uav1", + "frame_id": 0, + "height_id": 0, + "points": [ + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1 + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 3 + }, + { + "x": -20, + "y": -20, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": "gazebo_gimbal", + "parameters": [ + 0.5, + 0.5, + 0.5 + ], + "continue_without_waiting": false, + "stop_on_failure": false, + "max_retries": 1, + "retry_delay": 0 + }, + { + "type": "wait", + "parameters": 5.6 + } + ], + "parallel_execution": true + }, + { + "x": -10, + "y": 10, + "z": 5, + "heading": 3 + }, + { + "x": 10, + "y": -10, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": "wait", + "parameters": 5.6 + } + ] + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1, + "subtasks": [ + { + "type": "wait", + "parameters": 5.6 + } + ] + } + ], + "terminal_action": 0 + } + ] + } +} +``` + +
+ + ## WebSocket API You can use the WebSocket API to receive robots telemetry and send requests to control the robots. From a745f493aecfa61afb8fdb917c03267a9b130a38 Mon Sep 17 00:00:00 2001 From: Marlon Rivera <53498345+MarlonRiv@users.noreply.github.com> Date: Tue, 2 Sep 2025 13:30:29 +0200 Subject: [PATCH 22/33] Better formatting in the signaling protocol --- docs/README.md | 27 +++++++++++++++++++++------ 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/docs/README.md b/docs/README.md index 9477be9..30487ff 100644 --- a/docs/README.md +++ b/docs/README.md @@ -8,6 +8,7 @@ Designed to communicate between the web client and ROS. You can use the HTTP API to send requests to interact with the robots and receive status information. The requests and responses are in JSON format. +--- ### Robot control Endpoints for controlling the robots. @@ -58,6 +59,8 @@ Endpoints for controlling the robots. Land home all robots +--- + ### Environment setup Endpoints for controlling the robot's environment. @@ -406,6 +409,8 @@ You will receive a Status code **409 Conflict**, and a message to let the user k {"message": "Call was not successful with message: Discrepancy in the borders between the fleet, please set the safety borders!"} ``` +--- + ## Missions The missions are handled by `IROC Fleet Manager`: a node responsible for sending the mission to the robots, monitoring their progress, and sending the aggregated information to the `IROC Bridge`. @@ -562,7 +567,8 @@ The missions are handled by `IROC Fleet Manager`: a node responsible for sending ``` - + +--- ### Mission Response Examples @@ -708,7 +714,7 @@ The result follows the following structure: - +--- ### Mission `GET` endpoint @@ -797,7 +803,7 @@ If there is no active mission, you will get an unsuccessful response, with the m } ``` - +--- ### Mission Control Endpoints @@ -848,7 +854,7 @@ You can also control individual mission robots using these endpoints: > **NOTE** \ > Stopping the mission for a single robot will also abort the overall mission and stop all other robots. This behavior is intentional, as the mission assumes the participation of all assigned robots. - +--- ### Feedback During an active mission, the feedback message is broadcast to the connected clients through a WebSocket in the `/telemetry` path. @@ -898,6 +904,8 @@ During an active mission, the feedback message is broadcast to the connected cli > **NOTE** \ > Autonomy test follows the same structure as the waypoint mission feedback, but it will always contain only one robot. +--- + ### Result When a mission is finished, the result message will be sent to @@ -934,8 +942,9 @@ Send the result of the mission. +--- -### Subtasks +## Subtasks On the Waypoint missions, you can send a list of subtasks that will be executed by the robot when it reaches a waypoint. You just need to add a new field called `subtasks` in the waypoint message, which is an array of subtasks. Requests are retro-compatible, so you can use the feature without changing the existing missions or additional fields. There are two types of subtasks supported: `wait` and `gazebo_gimbal` (for simulation), but they can be extended in the future due to the ROS `plugin` architecture in `iroc_mission_handler`. @@ -978,9 +987,10 @@ It receives an array of three floating-point numbers representing the camera ori } ``` -The subtask system allows you to define complex behaviors that are executed when a robot reaches a waypoint. +--- ### Waypoint Structure +The subtask system allows you to define complex behaviors that are executed when a robot reaches a waypoint. Each waypoint now supports the following structure: @@ -1000,6 +1010,8 @@ Each waypoint now supports the following structure: - **When `true`**: Subtasks are executed simultaneously - **When `false`**: Subtasks are executed one after another (sequentially) +--- + ### Subtask Configuration Each subtask supports advanced execution control through the following fields: @@ -1125,6 +1137,7 @@ These are the fundamental fields for configuring a subtask: +--- ## WebSocket API @@ -1376,6 +1389,8 @@ You can use the WebSocket API to control the robots in the `/rc` path. +--- + ## Camera stream using WebRTC The features for the camera streaming are available, and the setup can be tested by starting the simulator with the camera argument for that will start the gazebo simulator: From 77288d3c0b2bbfe5595e0381d056300227b10fff Mon Sep 17 00:00:00 2001 From: Marlon Rivera Date: Wed, 1 Oct 2025 11:18:57 +0200 Subject: [PATCH 23/33] Extra sub/pub of fleet manager action feedback Useful for mocking the action feedback for UI display using a rosbag --- launch/iroc_bridge.launch | 2 ++ src/iroc_bridge.cpp | 47 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 49 insertions(+) diff --git a/launch/iroc_bridge.launch b/launch/iroc_bridge.launch index cb71522..ee1a257 100644 --- a/launch/iroc_bridge.launch +++ b/launch/iroc_bridge.launch @@ -50,6 +50,8 @@ + + diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index bad0723..b2e2e6b 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -135,6 +135,9 @@ class IROCBridge : public nodelet::Nodelet { // | ---------------------- ROS subscribers --------------------- | + // Fleet manager feedback subscriber + mrs_lib::SubscribeHandler sch_fleet_manager_feedback_; + struct robot_handler_t { std::string robot_name; mrs_lib::SubscribeHandler sh_general_robot_info; @@ -182,6 +185,7 @@ class IROCBridge : public nodelet::Nodelet { void missionDoneCallback(const SimpleClientGoalState& state, const boost::shared_ptr& result); template void missionFeedbackCallback(const boost::shared_ptr& feedback); + void missionFeedbackCallback(iroc_fleet_manager::IROCFleetManagerActionFeedback::ConstPtr msg); // | ------------------ Additional functions ------------------ | @@ -456,6 +460,9 @@ void IROCBridge::onInit() { } } + sch_fleet_manager_feedback_ = + mrs_lib::SubscribeHandler(shopts, "in/fleet_manager_feedback"); + sc_change_fleet_mission_state_ = nh_.serviceClient(nh_.resolveName("svc/change_fleet_mission_state")); ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc_server/change_fleet_mission_state\' -> \'%s\'", sc_change_fleet_mission_state_.getService().c_str()); @@ -535,6 +542,10 @@ void IROCBridge::timerMain([[maybe_unused]] const ros::TimerEvent& event) { if (rh.sh_system_health_info.newMsg()) { parseSystemHealthInfo(rh.sh_system_health_info.getMsg(), robot_name); } + + if (sch_fleet_manager_feedback_.newMsg()) { + missionFeedbackCallback(sch_fleet_manager_feedback_.getMsg()); + } } } //} @@ -625,6 +636,42 @@ void IROCBridge::missionFeedbackCallback(const boost::shared_ptr sendTelemetryJsonMessage("MissionFeedback", json_msg); } + +void IROCBridge::missionFeedbackCallback(iroc_fleet_manager::IROCFleetManagerActionFeedback::ConstPtr msg) { + auto robot_feedbacks = msg->feedback.info.robot_feedbacks; + + // Create a list for robot feedback + json json_msgs = json::list(); + + // Collect each robot feedback and create a json for each + for (size_t i = 0; i < robot_feedbacks.size(); i++) { + const auto& rfb = robot_feedbacks[i]; + + json robot_json = {{"robot_name", rfb.name}, + {"message", rfb.message}, + {"mission_progress", rfb.mission_progress}, + {"current_goal", rfb.goal_idx}, + {"distance_to_goal", rfb.distance_to_closest_goal}, + {"goal_estimated_arrival_time", rfb.goal_estimated_arrival_time}, + {"goal_progress", rfb.goal_progress}, + {"distance_to_finish", rfb.distance_to_finish}, + {"finish_estimated_arrival_time", rfb.finish_estimated_arrival_time}}; + + ROS_DEBUG_STREAM("[IROCBridge]: Mission feedback for robot: " + << rfb.name << ", message: " << rfb.message << ", progress: " << rfb.mission_progress << ", current goal: " << rfb.goal_idx + << ", distance to goal: " << rfb.distance_to_closest_goal << ", goal estimated arrival time: " << rfb.goal_estimated_arrival_time + << ", goal progress: " << rfb.goal_progress << ", distance to finish: " << rfb.distance_to_finish + << ", finish estimated arrival time: " << rfb.finish_estimated_arrival_time); + + // Add to the list at index i + json_msgs[i] = std::move(robot_json); + } + + // Create the main JSON message + json json_msg = {{"progress", msg->feedback.info.progress}, {"mission_state", msg->feedback.info.state}, {"message", msg->feedback.info.message}, {"robots", json_msgs}}; + + sendTelemetryJsonMessage("MissionFeedback", json_msg); +} //} // -------------------------------------------------------------- From d5ce7ad240d9ac4e1bf502f6fe1081f356a8febb Mon Sep 17 00:00:00 2001 From: Marlon Rivera Date: Wed, 1 Oct 2025 15:39:05 +0200 Subject: [PATCH 24/33] Added robot type in the getter of available robots, changed message structure --- src/iroc_bridge.cpp | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index b2e2e6b..d8c1f44 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -460,6 +460,7 @@ void IROCBridge::onInit() { } } + shopts.no_message_timeout = mrs_lib::no_timeout; sch_fleet_manager_feedback_ = mrs_lib::SubscribeHandler(shopts, "in/fleet_manager_feedback"); @@ -1781,15 +1782,20 @@ crow::response IROCBridge::commandCallback(const crow::request& req, const std:: * \return res Crow response */ crow::response IROCBridge::availableRobotsCallback([[maybe_unused]] const crow::request& req) { - std::vector robot_names; - robot_names.reserve(robot_handlers_.handlers.size()); - for (const auto &rh : robot_handlers_.handlers) - robot_names.push_back(rh.robot_name); - - json json_msg; - json_msg["robot_names"] = robot_names; + if (robot_handlers_.handlers.empty()) { + ROS_WARN_STREAM("[IROCBridge]: No robots available in the fleet."); + return crow::response(crow::status::NO_CONTENT, "{\"message\": \"No robots available in the fleet.\"}"); + } - return crow::response(crow::status::ACCEPTED, json_msg.dump()); + json robots = json::list(); + for (size_t i = 0; i < robot_handlers_.handlers.size(); i++) { + if (!robot_handlers_.handlers[i].sh_general_robot_info.hasMsg()) { + ROS_WARN_STREAM("[IROCBridge]: Robot handler for robot " << robot_handlers_.handlers[i].robot_name << " does not have general robot info."); + return crow::response(crow::status::NO_CONTENT, "{\"message\": \"No robots available in the fleet.\"}"); + } + robots[i] = {{"name", robot_handlers_.handlers[i].sh_general_robot_info.getMsg()->robot_name}, + {"type", robot_handlers_.handlers[i].sh_general_robot_info.getMsg()->robot_type}}; + } } //} From c5beaa5feafb0e15963740dc2cea60ee2acfbc6f Mon Sep 17 00:00:00 2001 From: Marlon Rivera Date: Wed, 1 Oct 2025 15:50:14 +0200 Subject: [PATCH 25/33] updated documentation for available robots getter --- docs/README.md | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/docs/README.md b/docs/README.md index 30487ff..a2b7b77 100644 --- a/docs/README.md +++ b/docs/README.md @@ -18,6 +18,27 @@ Endpoints for controlling the robots. List available robots. + +
+ + Body raw (json) + + + ```json + [ + { + "name": "uav1", + "type": 0 + }, + { + "name": "uav2", + "type": 0 + + } + ] + ``` +
+ - `POST` **/robots/{_robot_name_}/takeoff** From 9543a04d53905af1d00b431ebda0efd77e134cd9 Mon Sep 17 00:00:00 2001 From: Marlon Rivera Date: Thu, 2 Oct 2025 17:06:46 +0200 Subject: [PATCH 26/33] fix: add missing response in available robots --- src/iroc_bridge.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index d8c1f44..d10441c 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -1796,6 +1796,7 @@ crow::response IROCBridge::availableRobotsCallback([[maybe_unused]] const crow:: robots[i] = {{"name", robot_handlers_.handlers[i].sh_general_robot_info.getMsg()->robot_name}, {"type", robot_handlers_.handlers[i].sh_general_robot_info.getMsg()->robot_type}}; } + return crow::response(crow::status::ACCEPTED, robots.dump()); } //} From 2d18fe0cdab0f70d7c9c220481fc3401aa3b8b6e Mon Sep 17 00:00:00 2001 From: Marlon Rivera Date: Thu, 9 Oct 2025 12:10:02 +0200 Subject: [PATCH 27/33] Fix available robots callback when network config has unavailable robot --- src/iroc_bridge.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index d10441c..993c551 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -1791,11 +1791,15 @@ crow::response IROCBridge::availableRobotsCallback([[maybe_unused]] const crow:: for (size_t i = 0; i < robot_handlers_.handlers.size(); i++) { if (!robot_handlers_.handlers[i].sh_general_robot_info.hasMsg()) { ROS_WARN_STREAM("[IROCBridge]: Robot handler for robot " << robot_handlers_.handlers[i].robot_name << " does not have general robot info."); - return crow::response(crow::status::NO_CONTENT, "{\"message\": \"No robots available in the fleet.\"}"); + continue; } - robots[i] = {{"name", robot_handlers_.handlers[i].sh_general_robot_info.getMsg()->robot_name}, - {"type", robot_handlers_.handlers[i].sh_general_robot_info.getMsg()->robot_type}}; + + robots[robots.size()] = { + {"name", robot_handlers_.handlers[i].sh_general_robot_info.getMsg()->robot_name}, + {"type", robot_handlers_.handlers[i].sh_general_robot_info.getMsg()->robot_type} + }; } + return crow::response(crow::status::ACCEPTED, robots.dump()); } //} From 2302e42c8f20586f3e38a2edb6e40928ed10b949 Mon Sep 17 00:00:00 2001 From: Marlon Rivera Date: Wed, 15 Oct 2025 17:42:56 +0200 Subject: [PATCH 28/33] Subscribing and sending of telemetry data for sensor info --- launch/iroc_bridge.launch | 1 + src/iroc_bridge.cpp | 25 +++++++++++++++++++++++++ 2 files changed, 26 insertions(+) diff --git a/launch/iroc_bridge.launch b/launch/iroc_bridge.launch index ee1a257..af94ff3 100644 --- a/launch/iroc_bridge.launch +++ b/launch/iroc_bridge.launch @@ -50,6 +50,7 @@ + diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index 993c551..2229b46 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -146,6 +147,7 @@ class IROCBridge : public nodelet::Nodelet { mrs_lib::SubscribeHandler sh_collision_avoidance_info; mrs_lib::SubscribeHandler sh_uav_info; mrs_lib::SubscribeHandler sh_system_health_info; + mrs_lib::SubscribeHandler sh_sensor_info; ros::ServiceClient sc_takeoff; ros::ServiceClient sc_land; @@ -194,6 +196,7 @@ class IROCBridge : public nodelet::Nodelet { void parseControlInfo(mrs_robot_diagnostics::ControlInfo::ConstPtr control_info, const std::string& robot_name); void parseCollisionAvoidanceInfo(mrs_robot_diagnostics::CollisionAvoidanceInfo::ConstPtr collision_avoidance_info, const std::string& robot_name); void parseUavInfo(mrs_robot_diagnostics::UavInfo::ConstPtr uav_info, const std::string& robot_name); + void parseSensorInfo(mrs_robot_diagnostics::SensorInfo::ConstPtr sensor_info, const std::string& robot_name); void parseSystemHealthInfo(mrs_robot_diagnostics::SystemHealthInfo::ConstPtr uav_info, const std::string& robot_name); void sendJsonMessage(const std::string& msg_type, json& json_msg); @@ -426,6 +429,9 @@ void IROCBridge::onInit() { const std::string system_health_info_topic_name = "/" + robot_name + nh_.resolveName("in/system_health_info"); robot_handler.sh_system_health_info = mrs_lib::SubscribeHandler(shopts, system_health_info_topic_name); + const std::string sensor_info_topic_name = "/" + robot_name + nh_.resolveName("in/sensor_info"); + robot_handler.sh_sensor_info = mrs_lib::SubscribeHandler(shopts, sensor_info_topic_name); + robot_handler.sc_takeoff = nh_.serviceClient("/" + robot_name + nh_.resolveName("svc/takeoff")); ROS_INFO("[IROCBridge]: Created ServiceClient on service \'svc/takeoff\' -> \'%s\'", robot_handler.sc_takeoff.getService().c_str()); @@ -544,6 +550,10 @@ void IROCBridge::timerMain([[maybe_unused]] const ros::TimerEvent& event) { parseSystemHealthInfo(rh.sh_system_health_info.getMsg(), robot_name); } + if (rh.sh_sensor_info.newMsg()) { + parseSensorInfo(rh.sh_sensor_info.getMsg(), robot_name); + } + if (sch_fleet_manager_feedback_.newMsg()) { missionFeedbackCallback(sch_fleet_manager_feedback_.getMsg()); } @@ -784,6 +794,21 @@ void IROCBridge::parseUavInfo(mrs_robot_diagnostics::UavInfo::ConstPtr uav_info, } //} +/* parseSensorInfo() //{ */ +void IROCBridge::parseSensorInfo(mrs_robot_diagnostics::SensorInfo::ConstPtr sensor_info, const std::string& robot_name) { + + crow::json::rvalue json_msg = crow::json::load(sensor_info->details); + if (!json_msg) { + ROS_WARN_STREAM_THROTTLE(1.0, "[IROCBridge]: Could not parse sensor details JSON string: " << sensor_info->details); + return; + } + json telemetry_msg = { + {"type", sensor_info->type}, + {"details", json_msg}}; + + sendTelemetryJsonMessage("SensorInfo", telemetry_msg); +} + /* parseSystemHealthInfo() //{ */ void IROCBridge::parseSystemHealthInfo(mrs_robot_diagnostics::SystemHealthInfo::ConstPtr system_health_info, const std::string& robot_name) { // Create arrays for node_cpu_loads From 0a6370a008f7c7f69c184f313f3edb06d0a10b67 Mon Sep 17 00:00:00 2001 From: Marlon Rivera Date: Mon, 27 Oct 2025 16:14:31 +0100 Subject: [PATCH 29/33] Include sensor info telemetry stream in documentation --- docs/README.md | 973 ++++++++++++++++++++++++++----------------------- 1 file changed, 524 insertions(+), 449 deletions(-) diff --git a/docs/README.md b/docs/README.md index a2b7b77..319628d 100644 --- a/docs/README.md +++ b/docs/README.md @@ -9,6 +9,7 @@ You can use the HTTP API to send requests to interact with the robots and receiv The requests and responses are in JSON format. --- + ### Robot control Endpoints for controlling the robots. @@ -35,10 +36,14 @@ Endpoints for controlling the robots. "type": 0 } - ] + + ] + ``` + ``` + - `POST` **/robots/{_robot_name_}/takeoff** @@ -85,9 +90,10 @@ Endpoints for controlling the robots. ### Environment setup Endpoints for controlling the robot's environment. - > **NOTE** \ - > Each step in this sequence depends on the successful completion of the previous step. Please ensure that you first initialize the origin, then the borders, and finally the obstacles, in that exact order. -> + +> **NOTE** \ +> Each step in this sequence depends on the successful completion of the previous step. Please ensure that you first initialize the origin, then the borders, and finally the obstacles, in that exact order. +
@@ -124,107 +130,107 @@ Endpoints for controlling the robot's environment. Retrieve the world origin. - - +
Body raw (json) - + Status code: **202 Accepted** + ```json { - "frame_id": 0, - "x": 47.397978, - "y": 8.545299 + "frame_id": 0, + "x": 47.397978, + "y": 8.545299 } ``` - +
- - - `POST` - **/safety-area/borders** - - Set the safety area borders. - -
- - Body raw (json) - - - ```json - { - "points": [ - { - "x": 47.39776, - "y": 8.545254 - }, - { - "x": 47.397719, - "y": 8.545436 - }, - { - "x": 47.397601, - "y": 8.545367 - }, - { - "x": 47.397657, - "y": 8.545191 - } - ], - "height_id": 1, - "max_z": 347, - "min_z": 343 - } - ``` -
+- `POST` + **/safety-area/borders** + + Set the safety area borders. + +
+ + Body raw (json) + + + ```json + { + "points": [ + { + "x": 47.39776, + "y": 8.545254 + }, + { + "x": 47.397719, + "y": 8.545436 + }, + { + "x": 47.397601, + "y": 8.545367 + }, + { + "x": 47.397657, + "y": 8.545191 + } + ], + "height_id": 1, + "max_z": 347, + "min_z": 343 + } + ``` + +
- `GET` **/safety-area/borders** Retrieve the safety border. - - +
Body raw (json) - + Status code: **202 Accepted** - + ```json { - "frame_id":1, - "min_z":0, - "points":[ + "frame_id": 1, + "min_z": 0, + "points": [ { - "x":47.398283001578619178, - "y":8.54231999998631863491 + "x": 47.398283001578619178, + "y": 8.54231999998631863491 }, { - "y":8.5464419999864418287, - "x":47.3979670015785998771 + "y": 8.5464419999864418287, + "x": 47.3979670015785998771 }, { - "x":47.3972980015784983721, - "y":8.54602099998643183199 + "x": 47.3972980015784983721, + "y": 8.54602099998643183199 }, { - "x":47.3975810015785512519, - "y":8.5447099999863915798 + "x": 47.3975810015785512519, + "y": 8.5447099999863915798 }, { - "y":8.54231999998631863491, - "x":47.398283001578619178 + "y": 8.54231999998631863491, + "x": 47.398283001578619178 } - ], - "height_id":0, - "max_z":15, - "message":"All robots in the fleet with the same safety border" + ], + "height_id": 0, + "max_z": 15, + "message": "All robots in the fleet with the same safety border" } ``` +
- `POST` @@ -238,77 +244,77 @@ Endpoints for controlling the robot's environment. ```json - { - "obstacles":[ - { - "points":[ - { - "x":47.39776, - "y":8.545254 - }, - { - "x":47.397719, - "y":8.545436 - }, - { - "x":47.397601, - "y":8.545367 - }, - { - "x":47.397657, - "y":8.545191 - } - ], - "height_id":1, - "max_z":347, - "min_z":343 - }, - { - "points":[ - { - "x":47.397900, - "y":8.545800 - }, - { - "x":47.397855, - "y":8.545950 - }, - { - "x":47.397750, - "y":8.545890 - }, - { - "x":47.397795, - "y":8.545740 - } - ], - "height_id":1, - "max_z":350, - "min_z":345 - }, - { - "points":[ - { - "x":47.398100, - "y":8.545100 - }, - { - "x":47.398050, - "y":8.545250 - }, - { - "x":47.397950, - "y":8.545200 - }, - { - "x":47.398000, - "y":8.545050 - } - ], - "height_id":1, - "max_z":352, - "min_z":348 - } + { + "obstacles": [ + { + "points": [ + { + "x": 47.39776, + "y": 8.545254 + }, + { + "x": 47.397719, + "y": 8.545436 + }, + { + "x": 47.397601, + "y": 8.545367 + }, + { + "x": 47.397657, + "y": 8.545191 + } + ], + "height_id": 1, + "max_z": 347, + "min_z": 343 + }, + { + "points": [ + { + "x": 47.3979, + "y": 8.5458 + }, + { + "x": 47.397855, + "y": 8.54595 + }, + { + "x": 47.39775, + "y": 8.54589 + }, + { + "x": 47.397795, + "y": 8.54574 + } + ], + "height_id": 1, + "max_z": 350, + "min_z": 345 + }, + { + "points": [ + { + "x": 47.3981, + "y": 8.5451 + }, + { + "x": 47.39805, + "y": 8.54525 + }, + { + "x": 47.39795, + "y": 8.5452 + }, + { + "x": 47.398, + "y": 8.54505 + } + ], + "height_id": 1, + "max_z": 352, + "min_z": 348 + } ] } ``` @@ -320,114 +326,116 @@ Endpoints for controlling the robot's environment. Retrieve the obstacles. - - +
Body raw (json) - + Status code: **202 Accepted** - + ```json { - "obstacles":[ + "obstacles": [ { - "height_id":0, - "max_z":7.05999999660582489014, - "frame_id":1, - "min_z":3.05999999660582489014, - "points":[ - { - "x":47.3977600015785611731, - "y":8.54525399998640722288 - }, - { - "y":8.54543599998641312254, - "x":47.3977190015785652122 - }, - { - "x":47.3976010015785576002, - "y":8.54536699998641147147 - }, - { - "x":47.3976570015785512169, - "y":8.54519099998640641047 - }, - { - "y":8.54525399998640722288, - "x":47.3977600015785611731 - } - ] + "height_id": 0, + "max_z": 7.05999999660582489014, + "frame_id": 1, + "min_z": 3.05999999660582489014, + "points": [ + { + "x": 47.3977600015785611731, + "y": 8.54525399998640722288 + }, + { + "y": 8.54543599998641312254, + "x": 47.3977190015785652122 + }, + { + "x": 47.3976010015785576002, + "y": 8.54536699998641147147 + }, + { + "x": 47.3976570015785512169, + "y": 8.54519099998640641047 + }, + { + "y": 8.54525399998640722288, + "x": 47.3977600015785611731 + } + ] }, { - "points":[ - { - "y":8.54579999998642314551, - "x":47.3979000015785771893 - }, - { - "x":47.3978550015785771166, - "y":8.54594999998642634864 - }, - { - "y":8.54588999998642506739, - "x":47.3977500015785722098 - }, - { - "y":8.54573999998642364062, - "x":47.3977950015785864935 - }, - { - "x":47.3979000015785771893, - "y":8.54579999998642314551 - } - ], - "min_z":5.05999999660582489014, - "frame_id":1, - "max_z":10.0599999966058248901, - "height_id":0 + "points": [ + { + "y": 8.54579999998642314551, + "x": 47.3979000015785771893 + }, + { + "x": 47.3978550015785771166, + "y": 8.54594999998642634864 + }, + { + "y": 8.54588999998642506739, + "x": 47.3977500015785722098 + }, + { + "y": 8.54573999998642364062, + "x": 47.3977950015785864935 + }, + { + "x": 47.3979000015785771893, + "y": 8.54579999998642314551 + } + ], + "min_z": 5.05999999660582489014, + "frame_id": 1, + "max_z": 10.0599999966058248901, + "height_id": 0 }, { - "height_id":0, - "max_z":12.0599999966058248901, - "frame_id":1, - "min_z":6.44836723768419848974e-31, - "points":[ - { - "x":47.3981000015786122503, - "y":8.54509999998640168428 - }, - { - "y":8.54524999998640488741, - "x":47.3980500015786105905 - }, - { - "x":47.39795000157859306, - "y":8.54519999998640500394 - }, - { - "x":47.3980000015786018253, - "y":8.54504999998640002445 - }, - { - "y":8.54509999998640168428, - "x":47.3981000015786122503 - } - ] + "height_id": 0, + "max_z": 12.0599999966058248901, + "frame_id": 1, + "min_z": 6.44836723768419848974e-31, + "points": [ + { + "x": 47.3981000015786122503, + "y": 8.54509999998640168428 + }, + { + "y": 8.54524999998640488741, + "x": 47.3980500015786105905 + }, + { + "x": 47.39795000157859306, + "y": 8.54519999998640500394 + }, + { + "x": 47.3980000015786018253, + "y": 8.54504999998640002445 + }, + { + "y": 8.54509999998640168428, + "x": 47.3981000015786122503 + } + ] } - ], - "message":"All robots in the fleet with the same obstacles" + ], + "message": "All robots in the fleet with the same obstacles" } ``` +
- - > **NOTE** \ - > In case of any discrepancy within the robot's safety border, obstacles & origin, having different values -You will receive a Status code **409 Conflict**, and a message to let the user know about the conflict: + +> **NOTE** \ +> In case of any discrepancy within the robot's safety border, obstacles & origin, having different values +> You will receive a Status code **409 Conflict**, and a message to let the user know about the conflict: ```json -{"message": "Call was not successful with message: Discrepancy in the borders between the fleet, please set the safety borders!"} +{ + "message": "Call was not successful with message: Discrepancy in the borders between the fleet, please set the safety borders!" +} ``` --- @@ -435,12 +443,13 @@ You will receive a Status code **409 Conflict**, and a message to let the user k ## Missions The missions are handled by `IROC Fleet Manager`: a node responsible for sending the mission to the robots, monitoring their progress, and sending the aggregated information to the `IROC Bridge`. + - `POST` **/mission** - Set a mission + Set a mission - +
@@ -450,82 +459,83 @@ The missions are handled by `IROC Fleet Manager`: a node responsible for sending
Mission Sequence Diagram
- The mission request requires the following fields: - - **type key** to specify the mission type and the specific details of the mission. - - **details key** with the specific details for each mission. - - **Uuid key** for synchronization with the UI. +The mission request requires the following fields: + +- **type key** to specify the mission type and the specific details of the mission. +- **details key** with the specific details for each mission. +- **Uuid key** for synchronization with the UI.
WaypointPlanner: Body raw (json) - ```json - { - "type": "WaypointPlanner", - "uuid": "550e8400-e29b-41d4-a716-446655440000", - "details": { - "robots": [ - { - "name": "uav1", - "frame_id": 0, - "height_id": 0, - "points": [ - { - "x": 20, - "y": 10, - "z": 3, - "heading": 1 - }, - { - "x": 20, - "y": 10, - "z": 3, - "heading": 3 - }, - { - "x": -20, - "y": -20, - "z": 4, - "heading": 3, - "subtasks": [ - { - "type": "gazebo_gimbal", - "parameters": [0.5, 0.5, 0.5] - } - ] - }, - { - "x": -10, - "y": 10, - "z": 5, - "heading": 3 - }, - { - "x": 10, - "y": -10, - "z": 4, - "heading": 3, - "subtasks": [ - { - "type": "wait", - "parameters": 5.6 - } - ] - }, - { - "x": 20, - "y": 10, - "z": 3, - "heading": 1 - } - ], - "terminal_action": 0 - } - ] - } +```json +{ + "type": "WaypointPlanner", + "uuid": "550e8400-e29b-41d4-a716-446655440000", + "details": { + "robots": [ + { + "name": "uav1", + "frame_id": 0, + "height_id": 0, + "points": [ + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1 + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 3 + }, + { + "x": -20, + "y": -20, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": "gazebo_gimbal", + "parameters": [0.5, 0.5, 0.5] + } + ] + }, + { + "x": -10, + "y": 10, + "z": 5, + "heading": 3 + }, + { + "x": 10, + "y": -10, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": "wait", + "parameters": 5.6 + } + ] + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1 + } + ], + "terminal_action": 0 + } + ] } - ``` +} +```
@@ -534,36 +544,36 @@ The missions are handled by `IROC Fleet Manager`: a node responsible for sending Coverage Planner: Body raw (json) - ```json - { - "type": "CoveragePlanner", - "uuid": "b5aaa323-64e5-4eb8-8615-e4059fe84997", - "details": { - "robots": ["uav1", "uav2"], - "search_area": [ - { - "x": 47.397978, - "y": 8.545299 - }, - { - "x": 47.397848, - "y": 8.545872 - }, - { - "x": 47.397551, - "y": 8.54572 - }, - { - "x": 47.397699, - "y": 8.545129 - } - ], - "height_id": 0, - "height": 5, - "terminal_action": 0 - } +```json +{ + "type": "CoveragePlanner", + "uuid": "b5aaa323-64e5-4eb8-8615-e4059fe84997", + "details": { + "robots": ["uav1", "uav2"], + "search_area": [ + { + "x": 47.397978, + "y": 8.545299 + }, + { + "x": 47.397848, + "y": 8.545872 + }, + { + "x": 47.397551, + "y": 8.54572 + }, + { + "x": 47.397699, + "y": 8.545129 + } + ], + "height_id": 0, + "height": 5, + "terminal_action": 0 } - ``` +} +``` @@ -572,20 +582,20 @@ The missions are handled by `IROC Fleet Manager`: a node responsible for sending AutonomyTestPlanner: Body raw (json) - ```json - { - "type": "AutonomyTestPlanner", - "uuid": "20ab7a6c-231b-48ed-83cc-864041ae40bd", - "details": { - "robots": [ - { - "name": "uav1", - "segment_length": 5 - } - ] - } +```json +{ + "type": "AutonomyTestPlanner", + "uuid": "20ab7a6c-231b-48ed-83cc-864041ae40bd", + "details": { + "robots": [ + { + "name": "uav1", + "segment_length": 5 + } + ] } - ``` +} +``` @@ -594,18 +604,19 @@ The missions are handled by `IROC Fleet Manager`: a node responsible for sending ### Mission Response Examples The result follows the following structure: -- **message**: General message about the status of the mission. -- **success**: Boolean to denote if the mission was uploaded successfully. -- **type**: Type of the mission. -- **uuid**: The UUID of the mission. -- **robot_data**: An array with details for each robot in the mission. + +- **message**: General message about the status of the mission. +- **success**: Boolean to denote if the mission was uploaded successfully. +- **type**: Type of the mission. +- **uuid**: The UUID of the mission. +- **robot_data**: An array with details for each robot in the mission. - **robot**: String with the name of the robot. - - **message**: Individual robot message. - - **success**: Boolean to denote the individual result of the robot. - - **mission**: The details of the mission that were loaded into the robot. + - **message**: Individual robot message. + - **success**: Boolean to denote the individual result of the robot. + - **mission**: The details of the mission that were loaded into the robot. 1. Successful mission upload - +
Body example response (json) @@ -674,7 +685,7 @@ The result follows the following structure: 2. Uploading mission failure due to safety validation.
- + Body example response (json) @@ -684,42 +695,39 @@ The result follows the following structure: ```json { - "robot_data":[ - { - "message":"The given points are valid for: uav1, however the generated trajectory seems to be outside of safety area or within an obstacle.", - "mission":[ - - ], - "success":false, - "robot":"uav1" - } - ], - "message":"Failure starting robot clients.", - "success":false + "robot_data": [ + { + "message": "The given points are valid for: uav1, however the generated trajectory seems to be outside of safety area or within an obstacle.", + "mission": [], + "success": false, + "robot": "uav1" + } + ], + "message": "Failure starting robot clients.", + "success": false } ``` + ```json { - "robot_data":[ - { - "message":"Unvalid trajectory for uav1, trajectory is outside of safety area", - "mission":[ - - ], - "success":false, - "robot":"uav1" - } - ], - "message":"Failure starting robot clients.", - "success":false + "robot_data": [ + { + "message": "Unvalid trajectory for uav1, trajectory is outside of safety area", + "mission": [], + "success": false, + "robot": "uav1" + } + ], + "message": "Failure starting robot clients.", + "success": false } ``` - +
3. Uploading mission failure due to loaded mission in server.
- + Body example response (json) @@ -732,7 +740,7 @@ The result follows the following structure: "message": "Mission is already running. Terminate the previous one, or wait until it is finished." } ``` - +
--- @@ -742,85 +750,83 @@ The result follows the following structure: - `GET` **/mission** - Retrieve the mission loaded in the server. + Retrieve the mission loaded in the server. - - +
Body raw (json) - + Status code: **202 Accepted** - + ```json { - "robot_data":[ - { - "message": "Mission loaded successfully", - "mission":{ - "frame_id":0, - "height_id":0, - "points":[ - { - "x":20, - "y":10, - "heading":1, - "z":3 - }, - { - "z":3, - "heading":3, - "y":10, - "x":20 - }, - { - "x":-20, - "y":-20, - "heading":3, - "z":4 - }, - { - "x":-10, - "y":10, - "heading":3, - "z":5 - }, - { - "z":4, - "heading":3, - "y":-10, - "x":10 - }, - { - "z":3, - "heading":1, - "y":10, - "x":20 - } - ] - }, - "success":true, - "robot":"uav1" - } - ], - "uuid":"550e8400-e29b-41d4-a716-446655440000", - "type":"WaypointPlanner", - "message":"Mission uploaded successfully", - "success":true + "robot_data": [ + { + "message": "Mission loaded successfully", + "mission": { + "frame_id": 0, + "height_id": 0, + "points": [ + { + "x": 20, + "y": 10, + "heading": 1, + "z": 3 + }, + { + "z": 3, + "heading": 3, + "y": 10, + "x": 20 + }, + { + "x": -20, + "y": -20, + "heading": 3, + "z": 4 + }, + { + "x": -10, + "y": 10, + "heading": 3, + "z": 5 + }, + { + "z": 4, + "heading": 3, + "y": -10, + "x": 10 + }, + { + "z": 3, + "heading": 1, + "y": 10, + "x": 20 + } + ] + }, + "success": true, + "robot": "uav1" + } + ], + "uuid": "550e8400-e29b-41d4-a716-446655440000", + "type": "WaypointPlanner", + "message": "Mission uploaded successfully", + "success": true } ``` +
If there is no active mission, you will get an unsuccessful response, with the message that there is no active mission: ```json { - "robot_data":[ - - ], - "success": false, - "message" :"No active mission." + "robot_data": [], + "success": false, + "message": "No active mission." } ``` @@ -875,7 +881,9 @@ You can also control individual mission robots using these endpoints: > **NOTE** \ > Stopping the mission for a single robot will also abort the overall mission and stop all other robots. This behavior is intentional, as the mission assumes the participation of all assigned robots. + --- + ### Feedback During an active mission, the feedback message is broadcast to the connected clients through a WebSocket in the `/telemetry` path. @@ -966,7 +974,8 @@ Send the result of the mission. --- ## Subtasks -On the Waypoint missions, you can send a list of subtasks that will be executed by the robot when it reaches a waypoint. You just need to add a new field called `subtasks` in the waypoint message, which is an array of subtasks. Requests are retro-compatible, so you can use the feature without changing the existing missions or additional fields. + +On the Waypoint missions, you can send a list of subtasks that will be executed by the robot when it reaches a waypoint. You just need to add a new field called `subtasks` in the waypoint message, which is an array of subtasks. Requests are retro-compatible, so you can use the feature without changing the existing missions or additional fields. There are two types of subtasks supported: `wait` and `gazebo_gimbal` (for simulation), but they can be extended in the future due to the ROS `plugin` architecture in `iroc_mission_handler`. @@ -1011,6 +1020,7 @@ It receives an array of three floating-point numbers representing the camera ori --- ### Waypoint Structure + The subtask system allows you to define complex behaviors that are executed when a robot reaches a waypoint. Each waypoint now supports the following structure: @@ -1065,6 +1075,7 @@ These are the fundamental fields for configuring a subtask: ``` #### Mission with multiple subtasks example: +
Body raw (json) @@ -1101,11 +1112,7 @@ These are the fundamental fields for configuring a subtask: "subtasks": [ { "type": "gazebo_gimbal", - "parameters": [ - 0.5, - 0.5, - 0.5 - ], + "parameters": [0.5, 0.5, 0.5], "continue_without_waiting": false, "stop_on_failure": false, "max_retries": 1, @@ -1362,6 +1369,74 @@ Robot's data and status can be received periodically in the `/telemetry` path.
+- `onmessage` + **Sensor Info** +
+ + Message raw (json) + + + ```json + { + "type": "SensorInfo" + "details": { + "type": 7, + "camera_frame_tf": { + "rotation_rpy": { + "pitch": -0.000007, + "roll": -0.038402, + "yaw": 0 + }, + "translation": { + "x": 0.09669, + "y": 0.002004, + "z": -0.060549 + } + }, + "camera_info": { + "fov_x_rad": 1.92, + "fov_y_rad": 1.353683, + "height": 1080, + "width": 1920 + }, + "camera_orientation": { + "orientation_rpy": { + "pitch": -0.000007, + "roll": -0.038402, + "yaw": 0 + } + }, + "optical_frame_tf": { + "rotation_rpy": { + "pitch": -0.038402, + "roll": -1.570789, + "yaw": -1.570796 + }, + "translation": { + "x": 0.09669, + "y": 0.002004, + "z": -0.060549 + } + } + }, + "type": "SensorInfo" + } + ``` + + >NOTE: The `type` field in the `details` object indicates the sensor type (e.g., 7 for camera). + > The available sensor types are defined in the `mrs_robot_diagnostics/SensorStatus` message. + > The available types are: + > - 0: Autopilot + > - 1: Rangefinder + > - 2: GPS + > - 3: IMU + > - 4: Barometer + > - 5: Magnetometer + > - 6: LIDAR + > - 7: camera + +
+ ### Robot remote control You can use the WebSocket API to control the robots in the `/rc` path. From 9ca50c2083db5dbe93878b7f68ef876ea1addf71 Mon Sep 17 00:00:00 2001 From: Marlon Rivera Date: Mon, 27 Oct 2025 16:23:31 +0100 Subject: [PATCH 30/33] fix: documentation --- docs/README.md | 142 ++++++++++++++++++++++++++----------------------- 1 file changed, 74 insertions(+), 68 deletions(-) diff --git a/docs/README.md b/docs/README.md index 319628d..04b971c 100644 --- a/docs/README.md +++ b/docs/README.md @@ -38,11 +38,9 @@ Endpoints for controlling the robots. } ] - ```
- ``` - `POST` **/robots/{_robot_name_}/takeoff** @@ -470,72 +468,72 @@ The mission request requires the following fields: WaypointPlanner: Body raw (json) -```json -{ - "type": "WaypointPlanner", - "uuid": "550e8400-e29b-41d4-a716-446655440000", - "details": { - "robots": [ - { - "name": "uav1", - "frame_id": 0, - "height_id": 0, - "points": [ - { - "x": 20, - "y": 10, - "z": 3, - "heading": 1 - }, - { - "x": 20, - "y": 10, - "z": 3, - "heading": 3 - }, + ```json + { + "type": "WaypointPlanner", + "uuid": "550e8400-e29b-41d4-a716-446655440000", + "details": { + "robots": [ { - "x": -20, - "y": -20, - "z": 4, - "heading": 3, - "subtasks": [ + "name": "uav1", + "frame_id": 0, + "height_id": 0, + "points": [ { - "type": "gazebo_gimbal", - "parameters": [0.5, 0.5, 0.5] - } - ] - }, - { - "x": -10, - "y": 10, - "z": 5, - "heading": 3 - }, - { - "x": 10, - "y": -10, - "z": 4, - "heading": 3, - "subtasks": [ + "x": 20, + "y": 10, + "z": 3, + "heading": 1 + }, { - "type": "wait", - "parameters": 5.6 + "x": 20, + "y": 10, + "z": 3, + "heading": 3 + }, + { + "x": -20, + "y": -20, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": "gazebo_gimbal", + "parameters": [0.5, 0.5, 0.5] + } + ] + }, + { + "x": -10, + "y": 10, + "z": 5, + "heading": 3 + }, + { + "x": 10, + "y": -10, + "z": 4, + "heading": 3, + "subtasks": [ + { + "type": "wait", + "parameters": 5.6 + } + ] + }, + { + "x": 20, + "y": 10, + "z": 3, + "heading": 1 } - ] - }, - { - "x": 20, - "y": 10, - "z": 3, - "heading": 1 + ], + "terminal_action": 0 } - ], - "terminal_action": 0 + ] } - ] } -} -``` + ``` @@ -822,13 +820,21 @@ The result follows the following structure: If there is no active mission, you will get an unsuccessful response, with the message that there is no active mission: -```json -{ - "robot_data": [], - "success": false, - "message": "No active mission." -} -``` +
+ + Body raw (json) + + + + ```json + { + "robot_data": [], + "success": false, + "message": "No active mission." + } + ``` + +
--- From fab53dac8111462b5280e03bfc310964be97af1c Mon Sep 17 00:00:00 2001 From: Marlon Rivera <53498345+MarlonRiv@users.noreply.github.com> Date: Mon, 27 Oct 2025 16:30:18 +0100 Subject: [PATCH 31/33] Update JSON examples in README.md --- docs/README.md | 84 +++++++++++++++++++++++++------------------------- 1 file changed, 42 insertions(+), 42 deletions(-) diff --git a/docs/README.md b/docs/README.md index 04b971c..525f6b4 100644 --- a/docs/README.md +++ b/docs/README.md @@ -542,36 +542,36 @@ The mission request requires the following fields: Coverage Planner: Body raw (json) -```json -{ - "type": "CoveragePlanner", - "uuid": "b5aaa323-64e5-4eb8-8615-e4059fe84997", - "details": { - "robots": ["uav1", "uav2"], - "search_area": [ - { - "x": 47.397978, - "y": 8.545299 - }, - { - "x": 47.397848, - "y": 8.545872 - }, - { - "x": 47.397551, - "y": 8.54572 - }, - { - "x": 47.397699, - "y": 8.545129 - } - ], - "height_id": 0, - "height": 5, - "terminal_action": 0 + ```json + { + "type": "CoveragePlanner", + "uuid": "b5aaa323-64e5-4eb8-8615-e4059fe84997", + "details": { + "robots": ["uav1", "uav2"], + "search_area": [ + { + "x": 47.397978, + "y": 8.545299 + }, + { + "x": 47.397848, + "y": 8.545872 + }, + { + "x": 47.397551, + "y": 8.54572 + }, + { + "x": 47.397699, + "y": 8.545129 + } + ], + "height_id": 0, + "height": 5, + "terminal_action": 0 + } } -} -``` + ``` @@ -580,20 +580,20 @@ The mission request requires the following fields: AutonomyTestPlanner: Body raw (json) -```json -{ - "type": "AutonomyTestPlanner", - "uuid": "20ab7a6c-231b-48ed-83cc-864041ae40bd", - "details": { - "robots": [ - { - "name": "uav1", - "segment_length": 5 - } - ] + ```json + { + "type": "AutonomyTestPlanner", + "uuid": "20ab7a6c-231b-48ed-83cc-864041ae40bd", + "details": { + "robots": [ + { + "name": "uav1", + "segment_length": 5 + } + ] + } } -} -``` + ``` From a10be27bef9b323f2dc657e723f64e81996e4e40 Mon Sep 17 00:00:00 2001 From: Marlon Rivera Date: Mon, 10 Nov 2025 10:01:10 +0100 Subject: [PATCH 32/33] Added missing robot_name for sensor_info topic --- src/iroc_bridge.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/iroc_bridge.cpp b/src/iroc_bridge.cpp index 2229b46..41c5dc3 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -803,6 +803,7 @@ void IROCBridge::parseSensorInfo(mrs_robot_diagnostics::SensorInfo::ConstPtr sen return; } json telemetry_msg = { + {"robot_name", robot_name}, {"type", sensor_info->type}, {"details", json_msg}}; From 3ff022b0f40cf45393a900536ae0429c3a84a9fd Mon Sep 17 00:00:00 2001 From: Marlon Rivera <53498345+MarlonRiv@users.noreply.github.com> Date: Mon, 10 Nov 2025 14:23:05 +0100 Subject: [PATCH 33/33] Update signaling protocol documentation Added "robot_name" into SensorInfo telemetry topic --- docs/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/README.md b/docs/README.md index 525f6b4..57c16f4 100644 --- a/docs/README.md +++ b/docs/README.md @@ -1385,6 +1385,7 @@ Robot's data and status can be received periodically in the `/telemetry` path. ```json { "type": "SensorInfo" + "robot_name": "uav1" "details": { "type": 7, "camera_frame_tf": {