Devel by MarlonRiv · Pull Request #22 · fly4future/iroc_bridge

MarlonRiv · 2025-12-15T13:58:18Z

No description provided.

Replaced mission callbacks with new fleet manager action

As we are only having a centralized fleet manager, we don't need specific mission callbacks anymore.

Handlers to share with fleet manager and planners

Will be added instead into fleet manager

Following new mission request structure

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.

Possibility to add multiple obstacles at once, and possibility to keep obstacles when setting a new safety area border.

Added type and UUID useful for synchronization with the UI backend.

…r of borders and obstacles

Useful for mocking the action feedback for UI display using a rosbag

…tructure

Subscribing and sending of telemetry data for sensor info

Added "robot_name" into SensorInfo telemetry topic

Copilot

Pull request overview

This PR implements a major refactoring of the IROC Bridge mission handling system to support a unified fleet manager architecture. The changes consolidate multiple mission-specific action clients into a single generic fleet manager client and introduce new getter endpoints for retrieving configuration data.

Unified mission handling through a single IROCFleetManagerAction client replacing separate waypoint, coverage, and autonomy test clients
Added getter endpoints for world origin, safety borders, obstacles, and mission data
Enhanced obstacle handling to support multiple obstacles in a single request
Added sensor info parsing and telemetry support

Reviewed changes

Copilot reviewed 18 out of 20 changed files in this pull request and generated 14 comments.

Show a summary per file

File	Description
src/iroc_bridge.cpp	Major refactoring consolidating action clients, adding getter endpoints, sensor info parsing, and multiple obstacle support
package.xml	Removed dependency on iroc_mission_handler
CMakeLists.txt	Removed iroc_mission_handler from catkin dependencies
launch/iroc_bridge.launch	Updated remappings to use unified mission action client and added new service remappings
config/config.yaml	Restructured configuration under 'iroc_bridge' namespace
json_examples/*.json	Updated JSON examples to match new mission format with type, uuid, and details fields
docs/README.md	Comprehensive documentation updates covering new endpoints, mission format, and subtask system

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot · 2025-12-15T14:10:45Z

src/iroc_bridge.cpp

+    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
        }
-        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);
      }


The variable 'point_index' could potentially exceed the size of 'obstacles.data' if the sum of all 'number_of_vertices' values is greater than the actual data size. Consider adding bounds checking to prevent out-of-bounds access.

Copilot · 2025-12-15T14:10:45Z

src/iroc_bridge.cpp

+    json_msg["message"] = get_safety_border_service.response.message;
+
+    json points                   = json::list();
+    auto vector_points            = get_safety_border_service.response.border.points; 


There's a trailing whitespace at the end of line 1417. Please remove it for consistency.

Suggested change

auto vector_points = get_safety_border_service.response.border.points;

auto vector_points = get_safety_border_service.response.border.points;

Copilot · 2025-12-15T14:10:46Z

launch/iroc_bridge.launch

+

There's a trailing whitespace at the end of line 55 in the launch file. Please remove it for consistency.

Suggested change

Copilot · 2025-12-15T14:10:46Z

src/iroc_bridge.cpp

+    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();
+    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);


The loop at line 1129 uses 'points.size()' directly in the condition, but 'points' is obtained from 'robot_goals.at(i).points'. If this throws an exception or returns an invalid reference, the loop could fail. Consider adding validation that 'i' is within bounds of 'robot_goals' before accessing.

Suggested change

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();

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);

try {

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();

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);

} catch (const std::out_of_range& e) {

// Handle out-of-range error gracefully

// Optionally log the error or add an error entry

continue;

}

Copilot · 2025-12-15T14:10:47Z

src/iroc_bridge.cpp

+  json telemetry_msg = {
+      {"robot_name", robot_name},
+      {"type", sensor_info->type},
+      {"details", json_msg}};


The parseSensorInfo function attempts to create a wvalue JSON object from an rvalue, which may lead to issues. The 'json_msg' loaded from crow::json::load is an rvalue and cannot be directly assigned to a wvalue. Consider converting the rvalue to a string and then assigning it, or use the rvalue directly in the telemetry message construction.

Suggested change

{"details", json_msg}};

{"details", json_msg.dump()}};

Copilot · 2025-12-15T14:10:48Z

src/iroc_bridge.cpp

+  const auto url = param_loader.loadParam2<std::string>("url");
+  const auto client_port = param_loader.loadParam2<int>("client_port");
+  const auto server_port = param_loader.loadParam2<int>("server_port");
+


There's a trailing whitespace at the end of line 303. Please remove it for consistency.

Suggested change

Copilot · 2025-12-15T14:10:49Z

src/iroc_bridge.cpp

+    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(); 


There's a trailing whitespace at the end of line 1568. Please remove it for consistency.

Suggested change

json obstacles_json_list = json::list();

json obstacles_json_list = json::list();

Copilot · 2025-12-15T14:10:49Z

src/iroc_bridge.cpp

+      auto json          = resultToJson(result);
      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);
+
+    // TODO to replace with proper action response in ROS2
+    iroc_fleet_manager::GetMissionPointsSrv get_mission_data_service;
+    const auto resp =
+        callService<iroc_fleet_manager::GetMissionPointsSrv>(sc_get_mission_data_, get_mission_data_service.request, get_mission_data_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_data_service.response.mission_goal;
+    auto json = missionGoalToJson(mission_goal);


The variable 'json' is declared twice in this function scope (lines 1681 and 1702), which causes a shadowing issue. The second declaration shadows the first one. Consider renaming one of them (e.g., 'error_json' and 'success_json' or 'result_json' and 'mission_json') to avoid confusion.

Copilot · 2025-12-15T14:10:49Z

src/iroc_bridge.cpp

+    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\"}");

    // Validate if the action client is connected and if the action is already running
-    if (!coverage_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 (!coverage_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
-    coverageMissionActionServerGoal action_goal;
+    FleetManagerGoal action_goal;
+    std::string type = json_msg["type"].s();
+    std::string uuid = json_msg["uuid"].s();


Missing validation for the 'uuid' field in the JSON message. If the 'uuid' field is missing from the JSON, calling json_msg["uuid"].s() will throw an exception. Consider adding a check similar to the one for 'type' and 'details' fields.

Copilot · 2025-12-15T14:10:50Z

src/iroc_bridge.cpp

-    mrs_lib::SubscribeHandler<mrs_robot_diagnostics::StateEstimationInfo>    sh_state_estimation_info;
-    mrs_lib::SubscribeHandler<mrs_robot_diagnostics::ControlInfo>            sh_control_info;
+  // Fleet manager feedback subscriber
+  mrs_lib::SubscribeHandler<iroc_fleet_manager::IROCFleetManagerActionFeedback> sch_fleet_manager_feedback_; 


The line 140 has a trailing whitespace after the subscriber initialization. This should be removed for consistency with the project's coding style.

Suggested change

mrs_lib::SubscribeHandler<iroc_fleet_manager::IROCFleetManagerActionFeedback> sch_fleet_manager_feedback_;

mrs_lib::SubscribeHandler<iroc_fleet_manager::IROCFleetManagerActionFeedback> sch_fleet_manager_feedback_;

MarlonRiv and others added 30 commits July 31, 2025 17:42

Removing specific mission callbacks

1c69761

Replaced mission callbacks with new fleet manager action

Removed coverage and autonomy stuff, update with new mission structure

cf12ee8

Removed unnecessary routines

04b01fc

As we are only having a centralized fleet manager, we don't need specific mission callbacks anymore.

Added common handlers

8630e7f

Handlers to share with fleet manager and planners

Removed common handlers

d1fa46d

Will be added instead into fleet manager

Formatting

565dd65

update of json examples

ee1f907

Following new mission request structure

Fixed custom config loading, and cleaning

c1bd308

Fix of action name from fleet manager

766e3c5

remove unused dependency and include

cf62666

Getters for safety area_borders and world origin

a4b19a9

Obtain the mission points from fleet manager

0b8ccce

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.

Cleaning and improve consistency with the mission response

e3e3a15

Replace NULL to nullptr

4b1e93d

Obstacles improvement

acb19b0

Possibility to add multiple obstacles at once, and possibility to keep obstacles when setting a new safety area border.

Getter of loaded mission

4f5f77f

Added type and UUID useful for synchronization with the UI backend.

Update of the service naming from fleet manager\

0731c12

Updated mission examples and included frame_id and height_id in gette…

705fa4c

…r of borders and obstacles

Merge branch 'devel' into pluginize

487fb87

Merge pull request #18 from fly4future/pluginize

ebb63e7

Partially updating the signaling protocol

76da829

Diagrams for signaling protocol

e779779

Added subtask documentation into the signaling protocol

4a6158d

Better formatting in the signaling protocol

a745f49

Extra sub/pub of fleet manager action feedback

77288d3

Useful for mocking the action feedback for UI display using a rosbag

Added robot type in the getter of available robots, changed message s…

d5ce7ad

…tructure

updated documentation for available robots getter

c5beaa5

fix: add missing response in available robots

9543a04

Fix available robots callback when network config has unavailable robot

2d18fe0

Subscribing and sending of telemetry data for sensor info

2302e42

MarlonRiv and others added 6 commits October 27, 2025 14:46

Merge pull request #21 from fly4future/sensor-info

fa4f115

Subscribing and sending of telemetry data for sensor info

Include sensor info telemetry stream in documentation

0a6370a

fix: documentation

9ca50c2

Update JSON examples in README.md

fab53da

Added missing robot_name for sensor_info topic

a10be27

Update signaling protocol documentation

3ff022b

Added "robot_name" into SensorInfo telemetry topic

MarlonRiv requested a review from Copilot December 15, 2025 13:58

Copilot started reviewing on behalf of MarlonRiv December 15, 2025 13:58 View session

Copilot AI reviewed Dec 15, 2025

View reviewed changes

MarlonRiv merged commit cd501ef into main Dec 15, 2025
9 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Devel#22

Devel#22
MarlonRiv merged 36 commits intomainfrom
devel

MarlonRiv commented Dec 15, 2025

Uh oh!

Copilot AI left a comment

Uh oh!

Copilot AI Dec 15, 2025

Uh oh!

Copilot AI Dec 15, 2025

Uh oh!

Copilot AI Dec 15, 2025

Uh oh!

Copilot AI Dec 15, 2025

Uh oh!

Copilot AI Dec 15, 2025

Uh oh!

Copilot AI Dec 15, 2025

Uh oh!

Copilot AI Dec 15, 2025

Uh oh!

Copilot AI Dec 15, 2025

Uh oh!

Copilot AI Dec 15, 2025

Uh oh!

Copilot AI Dec 15, 2025

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

	auto vector_points = get_safety_border_service.response.border.points;
	auto vector_points = get_safety_border_service.response.border.points;

	json obstacles_json_list = json::list();
	json obstacles_json_list = json::list();

	mrs_lib::SubscribeHandler<iroc_fleet_manager::IROCFleetManagerActionFeedback> sch_fleet_manager_feedback_;
	mrs_lib::SubscribeHandler<iroc_fleet_manager::IROCFleetManagerActionFeedback> sch_fleet_manager_feedback_;

Conversation

MarlonRiv commented Dec 15, 2025

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Dec 15, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Dec 15, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Dec 15, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Dec 15, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Dec 15, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Dec 15, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Dec 15, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Dec 15, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Dec 15, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Dec 15, 2025

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants