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/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/docs/README.md b/docs/README.md index 30016d7..57c16f4 100644 --- a/docs/README.md +++ b/docs/README.md @@ -8,6 +8,8 @@ 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. @@ -17,6 +19,29 @@ 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** @@ -58,12 +83,15 @@ Endpoints for controlling the robots. Land home all 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. +
@@ -74,7 +102,7 @@ Endpoints for controlling the robot's environment.
- `POST` - **/safety-area/origin** + **/safety-area/world-origin** Set the world origin. @@ -95,6 +123,29 @@ Endpoints for controlling the robot's environment. +- `GET` + **/safety-area/world-origin** + + Retrieve the world origin. + + +
+ + Body raw (json) + + + Status code: **202 Accepted** + + ```json + { + "frame_id": 0, + "x": 47.397978, + "y": 8.545299 + } + ``` + +
+ - `POST` **/safety-area/borders** @@ -133,6 +184,53 @@ Endpoints for controlling the robot's environment. +- `GET` + **/safety-area/borders** + + Retrieve the safety border. + + +
+ + Body raw (json) + + + Status code: **202 Accepted** + + ```json + { + "frame_id": 1, + "min_z": 0, + "points": [ + { + "x": 47.398283001578619178, + "y": 8.54231999998631863491 + }, + { + "y": 8.5464419999864418287, + "x": 47.3979670015785998771 + }, + { + "x": 47.3972980015784983721, + "y": 8.54602099998643183199 + }, + { + "x": 47.3975810015785512519, + "y": 8.5447099999863915798 + }, + { + "y": 8.54231999998631863491, + "x": 47.398283001578619178 + } + ], + "height_id": 0, + "max_z": 15, + "message": "All robots in the fleet with the same safety border" + } + ``` + +
+ - `POST` **/safety-area/obstacles** @@ -145,198 +243,489 @@ Endpoints for controlling the robot's environment. ```json { - "points": [ + "obstacles": [ { - "x": 47.39776, - "y": 8.545254 + "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 }, { - "x": 47.397719, - "y": 8.545436 + "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 }, { - "x": 47.397601, - "y": 8.545367 + "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 + } + ] + } + ``` + + + +- `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 + } + ] }, { - "x": 47.397657, - "y": 8.545191 + "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": 1, - "max_z": 347, - "min_z": 343 + "message": "All robots in the fleet with the same obstacles" } ```
-### Missions +> **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 +
- - + + Sequence Diagram
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 + + ```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 validation.
- + Body example response (json) -- Code: 201 Created +- Code: 400 Bad Request - 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" - } - ] + "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 } ``` - -
- -2. Uploading mission failure due to safety area validation. -
- - - Body example response (json) - -- Code: 400 Bad Request - -- Example value: ```json { - "message":"Failure starting robot clients.", - "success":false, - "robot_results":[ - { - "robot_name":"uav1", - "success":true, - "message":"Mission on robot: uav1 was successfully processed" - }, - { - "message":"Unvalid trajectory for uav2, trajectory is outside of safety area", - "success":false, - "robot_name":"uav2" - } - ] + "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) @@ -346,17 +735,114 @@ 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 +--- -We support for both fleet-wide and individual robot mission control. +### Mission `GET` endpoint -##### Fleet Mission Control: +- `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: + +
+ + Body raw (json) + + + + ```json + { + "robot_data": [], + "success": false, + "message": "No active mission." + } + ``` + +
+ +--- + +### Mission Control Endpoints + +We support both fleet-wide and individual robot mission control. + +**Fleet Mission Control**: These endpoints control the mission status for all assigned robots at once: \ @@ -376,7 +862,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 +873,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 +888,11 @@ 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 broadcasted to the connected clients through a WebSocket in the `/telemetry` path. +### Feedback + +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 +939,9 @@ 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 @@ -487,6 +977,202 @@ 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] +} +``` + +--- + +### 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: + +```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. @@ -689,6 +1375,75 @@ Robot's data and status can be received periodically in the `/telemetry` path. +- `onmessage` + **Sensor Info** +
+ + Message raw (json) + + + ```json + { + "type": "SensorInfo" + "robot_name": "uav1" + "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. @@ -737,6 +1492,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: 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 diff --git a/json_examples/autonomy_test.json b/json_examples/autonomy_test.json deleted file mode 100644 index 46b9502..0000000 --- a/json_examples/autonomy_test.json +++ /dev/null @@ -1,4 +0,0 @@ -{ - "robot_name": "uav1", - "segment_length": 5 -} diff --git a/json_examples/coverage_mission.json b/json_examples/coverage_mission.json deleted file mode 100644 index 96c5bbe..0000000 --- a/json_examples/coverage_mission.json +++ /dev/null @@ -1,15 +0,0 @@ -{ - "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 -} diff --git a/json_examples/missions/autonomy_test.json b/json_examples/missions/autonomy_test.json new file mode 100644 index 0000000..88b533b --- /dev/null +++ b/json_examples/missions/autonomy_test.json @@ -0,0 +1,12 @@ +{ + "type": "AutonomyTestPlanner", + "uuid": "20ab7a6c-231b-48ed-83cc-864041ae40bd", + "details": { + "robots": [ + { + "name": "uav1", + "segment_length": 5 + } + ] + } +} diff --git a/json_examples/missions/coverage.json b/json_examples/missions/coverage.json new file mode 100644 index 0000000..e392f37 --- /dev/null +++ b/json_examples/missions/coverage.json @@ -0,0 +1,28 @@ +{ + "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 + } +} diff --git a/json_examples/missions/heading.json b/json_examples/missions/heading.json index dc1eedc..fdeaca0 100644 --- a/json_examples/missions/heading.json +++ b/json_examples/missions/heading.json @@ -1,18 +1,70 @@ { - "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": 0, "parameters": "5.6" }] }, - { "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", + "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": "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/one_drone.json b/json_examples/missions/one_drone.json index 65fa79b..8a714a4 100644 --- a/json_examples/missions/one_drone.json +++ b/json_examples/missions/one_drone.json @@ -1,14 +1,28 @@ { - "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", + "uuid": "550e8400-e29b-41d4-a716-446655440000", + "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/sequential_subtask.json b/json_examples/missions/sequential_subtask.json new file mode 100644 index 0000000..26fdf9e --- /dev/null +++ b/json_examples/missions/sequential_subtask.json @@ -0,0 +1,83 @@ +{ + "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 + } + ] + } +} 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 871bcda..1856c0a 100644 --- a/json_examples/missions/two_drones.json +++ b/json_examples/missions/two_drones.json @@ -1,27 +1,66 @@ { - "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", + "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": 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/json_examples/missions/waypoint.json b/json_examples/missions/waypoint.json new file mode 100644 index 0000000..c31822b --- /dev/null +++ b/json_examples/missions/waypoint.json @@ -0,0 +1,63 @@ +{ "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 + } + ] + } +} 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/launch/iroc_bridge.launch b/launch/iroc_bridge.launch index 47537c9..af94ff3 100644 --- a/launch/iroc_bridge.launch +++ b/launch/iroc_bridge.launch @@ -3,15 +3,14 @@ - - - + + @@ -31,14 +30,12 @@ + + - - - - @@ -53,6 +50,9 @@ + + + @@ -67,14 +67,13 @@ - - - + - - - + + + + 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 c0218d0..41c5dc3 100644 --- a/src/iroc_bridge.cpp +++ b/src/iroc_bridge.cpp @@ -29,6 +29,10 @@ #include #include +#include +#include +#include +#include #include #include @@ -51,71 +55,59 @@ #include #include #include +#include #include -#include -#include -#include -#include - -#include +#include +#include #include #include + //} -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; using namespace actionlib; -typedef SimpleActionClient WaypointFleetManagerClient; -typedef SimpleActionClient CoveragePlannerClient; -typedef SimpleActionClient AutonomyTestClient; -//Waypoint mission goal -typedef iroc_fleet_manager::WaypointFleetManagerGoal FleetManagerActionServerGoal; -// Autonomy test goal -typedef iroc_fleet_manager::AutonomyTestGoal AutonomyTestActionServerGoal; -// Coverage mission goal -typedef iroc_fleet_manager::CoverageMissionGoal coverageMissionActionServerGoal; - +using FleetManagerActionClient = SimpleActionClient; +// Waypoint mission goal +typedef iroc_fleet_manager::IROCFleetManagerGoal 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 { + enum class CommandType + { Takeoff, Land, Hover, @@ -126,13 +118,7 @@ class IROCBridge : public nodelet::Nodelet { 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}, @@ -144,33 +130,24 @@ 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_; // | ---------------------- 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; + // 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; + 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; + mrs_lib::SubscribeHandler sh_sensor_info; ros::ServiceClient sc_takeoff; ros::ServiceClient sc_land; @@ -184,24 +161,23 @@ 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; - 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_; + 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_; + ros::ServiceClient sc_get_mission_data_; // | ----------------- action client callbacks ---------------- | @@ -209,8 +185,9 @@ 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); + void missionFeedbackCallback(iroc_fleet_manager::IROCFleetManagerActionFeedback::ConstPtr msg); // | ------------------ Additional functions ------------------ | @@ -219,35 +196,37 @@ 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); - 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 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 waypointMissionCallback(const crow::request& req); - crow::response coverageMissionCallback(const crow::request& req); - crow::response autonomyTestCallback(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); 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); @@ -256,6 +235,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); @@ -263,16 +246,14 @@ 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 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_; + mrs_msgs::Point2D world_origin_; }; //} @@ -296,47 +277,36 @@ 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 != "") { 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::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); 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!"); @@ -349,37 +319,42 @@ 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/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_, "/set_path").methods(crow::HTTPMethod::Post)([this](const crow::request& req) { return pathCallback(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); }); + 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))") - 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") @@ -417,13 +392,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 { @@ -432,8 +407,9 @@ void IROCBridge::onInit() { robot_handlers_.handlers.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 robot_handler.robot_name = robot_name; - + // Saving only the robot names to fill up the rest during the parsing of the messages 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); @@ -441,18 +417,21 @@ 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); + 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()); @@ -474,7 +453,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 ----------------------- | @@ -486,47 +466,44 @@ void IROCBridge::onInit() { } } - sc_change_fleet_mission_state = nh_.serviceClient(nh_.resolveName("svc/change_fleet_mission_state")); + shopts.no_message_timeout = mrs_lib::no_timeout; + 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()); + 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()); - 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_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_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()); + 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()); - /* // | --------------------- action clients --------------------- | */ + 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()); - //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()); + 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()); - //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()); + /* // | --------------------- action clients --------------------- | */ + // 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); + 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(); @@ -545,26 +522,41 @@ 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 for (auto& rh : robot_handlers_.handlers) { const auto& robot_name = rh.robot_name; - if (rh.sh_general_robot_info.newMsg()) + if (rh.sh_general_robot_info.newMsg()) { parseGeneralRobotInfo(rh.sh_general_robot_info.getMsg(), robot_name); + } - if (rh.sh_state_estimation_info.newMsg()) + if (rh.sh_state_estimation_info.newMsg()) { parseStateEstimationInfo(rh.sh_state_estimation_info.getMsg(), robot_name); + } - if (rh.sh_control_info.newMsg()) + if (rh.sh_control_info.newMsg()) { parseControlInfo(rh.sh_control_info.getMsg(), robot_name); + } - if (rh.sh_collision_avoidance_info.newMsg()) + if (rh.sh_collision_avoidance_info.newMsg()) { parseCollisionAvoidanceInfo(rh.sh_collision_avoidance_info.getMsg(), robot_name); + } - if (rh.sh_uav_info.newMsg()) + if (rh.sh_uav_info.newMsg()) { parseUavInfo(rh.sh_uav_info.getMsg(), robot_name); + } - if (rh.sh_system_health_info.newMsg()) + if (rh.sh_system_health_info.newMsg()) { 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()); + } } } //} @@ -583,16 +575,15 @@ void IROCBridge::missionActiveCallback() { /* missionDoneCallback //{ */ 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!"); + 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!"); // 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); @@ -603,22 +594,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); } @@ -627,50 +612,75 @@ 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); +} + +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); } //} @@ -681,26 +691,19 @@ 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}, - {"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); + 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); } //} @@ -709,61 +712,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); } @@ -771,18 +757,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); } @@ -791,12 +771,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); @@ -805,19 +783,33 @@ 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); } //} +/* 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 = { + {"robot_name", robot_name}, + {"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 @@ -826,10 +818,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); } @@ -841,29 +830,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); } @@ -873,11 +856,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); @@ -891,7 +874,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_) { @@ -924,6 +907,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, {}); @@ -965,41 +966,39 @@ 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; } } -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; - 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(); - } -} //} /* commandAction() method //{ */ 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"; @@ -1020,7 +1019,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"; @@ -1072,7 +1072,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", result->robot_results[i].name}, + {"success", static_cast(result->robot_results[i].success)}, + {"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; } //} -/* successMissionJson() method //{ */ +/* missionGoalToJson() method //{ */ -template -json successMissionJson(std::vector mission_robots) { - json robot_results = json::list(); +json missionGoalToJson(const iroc_fleet_manager::IROCFleetMissionGoal &mission_goal) { + json robots_data = 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"} - }; + + 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(); + 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); } - // Create the main JSON object - json json_msg = { - {"success", true}, - {"message", "Mission uploaded successfully"}, - {"robot_results", robot_results} - }; + json response = {{"success", true}, {"message", "Mission uploaded successfully"}, {"type", mission_goal.type}, {"uuid", mission_goal.uuid}, {"robot_data", robots_data}}; - return json_msg; + return response; } //} + // -------------------------------------------------------------- // | REST API callbacks | // -------------------------------------------------------------- @@ -1155,8 +1155,7 @@ json successMissionJson(std::vector mission_robots) { * \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); @@ -1165,9 +1164,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\"}"); @@ -1187,10 +1186,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; @@ -1202,7 +1201,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); } @@ -1222,8 +1221,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."); @@ -1234,13 +1232,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(); @@ -1253,11 +1251,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(); } @@ -1265,6 +1263,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 //{ */ /** @@ -1273,8 +1302,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); @@ -1283,8 +1311,8 @@ 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(); @@ -1321,12 +1349,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); @@ -1337,15 +1365,78 @@ 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 = false; 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); } //} +/* getSafetyBorderCallback() method //{ */ + +/** + * \brief Callback to get the safety area border. + * + * \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."); + 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}, + {"frame_id", getFrameID(horizontal_frame)}, + {"height_id", getFrameID(vertical_frame)}}; + return crow::response(crow::status::ACCEPTED, json_msg.dump()); + } +} +//} + /* setObstacleCallback() method //{ */ /** @@ -1354,467 +1445,270 @@ 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."); - 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); + // 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); - if (points.empty()) - return crow::response(crow::status::BAD_REQUEST, "Empty points array: " + req.body); + // Process each obstacle + for (size_t i = 0; i < obstacles.size(); ++i) { + const auto &obstacle = obstacles[i]; - //Process points - std::vector border_points; - border_points.reserve(points.size()); + // 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); + } - 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); + 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(); - mrs_msgs::Point2D pt; - pt.x = el["x"].d(); - pt.y = el["y"].d(); + // 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); + } - border_points.push_back(pt); - } + if (points.empty()) { + return crow::response(crow::status::BAD_REQUEST, "Empty points array in obstacle " + std::to_string(i) + ": " + req.body); + } - // Logging - ROS_INFO("[IROCBridge]: Obstacle border points size %zu ", border_points.size()); + // 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); + } - mrs_msgs::SetObstacleSrvRequest obstacle_req; + // Logging + ROS_INFO("[IROCBridge]: Obstacle %zu border points size %zu", i, border_points.size()); - 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; + // 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::scoped_lock lck(robot_handlers_.mtx); + // Call service for this obstacle + const auto result = commandAction(robot_names, "set_obstacle", obstacle_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); - - 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"); } //} -/* waypointMissionCallback() method //{ */ +/* getObstaclesCallback() method //{ */ /** - * \brief Callback for the waypoint mission request. It receives a list of missions for each robot and sends them to the fleet manager. + * \brief Callback to get the obstacles. * * \param req Crow request * \return res Crow response */ -crow::response IROCBridge::waypointMissionCallback(const crow::request& req) -{ - ROS_INFO_STREAM("[IROCBridge]: Parsing a waypointMissionCallback message JSON -> ROS."); - latest_mission_type_ = "waypoint"; +crow::response IROCBridge::getObstaclesCallback(const crow::request &req) { - 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\"}"); - } + ROS_INFO_STREAM("[IROCBridge]: Processing a getObstaclesCallback message ROS -> JSON."); + iroc_fleet_manager::GetObstaclesSrv get_obstacles_service; - // 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; - } + const auto resp = callService(sc_get_obstacles_, get_obstacles_service.request, get_obstacles_service.response); - // 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; + 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 } - 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 (!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 (!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 + "\"}"); + 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); } - // Send the action goal to the fleet manager - FleetManagerActionServerGoal action_goal; - action_goal.robots = mission_robots; - - 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 - 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(); - 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"); - 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()) + "\"}"); + 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()); } } //} -/* coverageMissionCallback() method //{ */ +/* getMissionCallback() method //{ */ /** - * \brief Callback for the coverage mission request. It receives a list of missions for each robot and sends them to the fleet manager. + * \brief Callback to get the loaded mission. * * \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"; +crow::response IROCBridge::getMissionCallback(const crow::request &req) { - 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()); - } + ROS_INFO_STREAM("[IROCBridge]: Processing a getMissionCallback message ROS -> JSON."); + iroc_fleet_manager::GetMissionPointsSrv get_mission_data_service; + const auto resp = + callService(sc_get_mission_data_, get_mission_data_service.request, get_mission_data_service.response); - 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); - } + 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()); + } - //Get the search area points - std::vector polygon_points; - polygon_points.reserve(search_area.size()); + auto mission_goal = get_mission_data_service.response.mission_goal; + auto json = missionGoalToJson(mission_goal); + return crow::response(crow::status::OK, json.dump()); +} +//} - for (const auto& el : search_area) { - mrs_msgs::Point2D pt; - pt.x = el["x"].d(); - pt.y = el["y"].d(); +/* missionCallback() method //{ */ - polygon_points.push_back(pt); - } +/** + * \brief Callback for the waypoint 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::missionCallback(const crow::request &req) { + ROS_INFO_STREAM("[IROCBridge]: Parsing a missionCallback message JSON -> ROS."); - ROS_INFO("[IROCBridge]: Polygon points size %zu ", polygon_points.size()); + try { + crow::json::rvalue json_msg = crow::json::load(req.body); + 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(); - 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 + "\"}"); - } + // Convert rvalue to wvalue, then dump to string + crow::json::wvalue details_wvalue(json_msg["details"]); + std::string details = details_wvalue.dump(); - 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); - } - ); + action_goal.type = type; + action_goal.uuid = uuid; + 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); }); // 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(); + ros::Duration(5).sleep(); - if (coverage_action_client_ptr_->getState().isDone()) { - auto result = coverage_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; - 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(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); + return crow::response(crow::status::CREATED, json.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()) + "\"}"); } } //} -/* 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 //{ */ /** @@ -1823,8 +1717,7 @@ crow::response IROCBridge::autonomyTestCallback(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 @@ -1834,17 +1727,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 @@ -1860,47 +1743,31 @@ 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; - 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()); 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); } @@ -1914,17 +1781,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); } @@ -1943,17 +1807,26 @@ 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) -{ - 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); +crow::response IROCBridge::availableRobotsCallback([[maybe_unused]] const crow::request& req) { + 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.\"}"); + } - json json_msg; - json_msg["robot_names"] = robot_names; + 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."); + continue; + } + + 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, json_msg.dump()); + return crow::response(crow::status::ACCEPTED, robots.dump()); } //} @@ -1987,8 +1860,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")) { @@ -1998,12 +1870,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 @@ -2014,9 +1885,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; @@ -2027,34 +1897,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);