Skip to content

Commit bcdc45f

Browse files
mfaferek93Michał Fąferek
andauthored
[#40] feat: implement PUT /components/{component_id}/data/{topic_name} endpoint (#45)
- Add publish_to_topic() method to DataAccessManager using ros2 topic pub --once - Implement PUT handler with input validation: - Required fields: type (message type), data (payload) - Message type format validation (package/msg/Type) - Component existence check - Add 6 integration tests for publish endpoint (REQ_INTEROP_020) - Update Postman collection with PUT examples - Standardize endpoint list format with HTTP method prefixes Co-authored-by: Michał Fąferek <michal.faferek@42dot.ai>
1 parent 9984654 commit bcdc45f

File tree

7 files changed

+488
-19
lines changed

7 files changed

+488
-19
lines changed

postman/collections/ros2-medkit-gateway.postman_collection.json

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -187,6 +187,66 @@
187187
"description": "Read brake pressure topic data from the pressure_sensor component."
188188
},
189189
"response": []
190+
},
191+
{
192+
"name": "PUT Publish Brake Command",
193+
"request": {
194+
"method": "PUT",
195+
"header": [
196+
{
197+
"key": "Content-Type",
198+
"value": "application/json"
199+
}
200+
],
201+
"body": {
202+
"mode": "raw",
203+
"raw": "{\n \"type\": \"std_msgs/msg/Float32\",\n \"data\": {\n \"data\": 50.0\n }\n}"
204+
},
205+
"url": {
206+
"raw": "{{base_url}}/components/actuator/data/command",
207+
"host": [
208+
"{{base_url}}"
209+
],
210+
"path": [
211+
"components",
212+
"actuator",
213+
"data",
214+
"command"
215+
]
216+
},
217+
"description": "Publish data to a component topic (REQ_INTEROP_020). Request body must contain 'type' (ROS 2 message type) and 'data' (message payload). Returns publish confirmation with topic, type, status, and timestamp."
218+
},
219+
"response": []
220+
},
221+
{
222+
"name": "PUT Publish Temperature Value",
223+
"request": {
224+
"method": "PUT",
225+
"header": [
226+
{
227+
"key": "Content-Type",
228+
"value": "application/json"
229+
}
230+
],
231+
"body": {
232+
"mode": "raw",
233+
"raw": "{\n \"type\": \"sensor_msgs/msg/Temperature\",\n \"data\": {\n \"temperature\": 85.5,\n \"variance\": 0.1\n }\n}"
234+
},
235+
"url": {
236+
"raw": "{{base_url}}/components/temp_sensor/data/temperature",
237+
"host": [
238+
"{{base_url}}"
239+
],
240+
"path": [
241+
"components",
242+
"temp_sensor",
243+
"data",
244+
"temperature"
245+
]
246+
},
247+
"description": "Publish temperature data to the temp_sensor component. Example of publishing sensor_msgs/msg/Temperature type."
248+
},
249+
"response": []
190250
}
191251
]
192252
}

src/ros2_medkit_gateway/include/ros2_medkit_gateway/data_access_manager.hpp

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,21 @@ class DataAccessManager {
4848
double timeout_sec = 3.0
4949
);
5050

51+
/**
52+
* @brief Publish data to a specific topic
53+
* @param topic_path Full topic path (e.g., /chassis/brakes/command)
54+
* @param msg_type ROS 2 message type (e.g., std_msgs/msg/Float32)
55+
* @param data JSON data to publish
56+
* @param timeout_sec Timeout for the publish operation
57+
* @return JSON with publish status
58+
*/
59+
json publish_to_topic(
60+
const std::string& topic_path,
61+
const std::string& msg_type,
62+
const json& data,
63+
double timeout_sec = 5.0
64+
);
65+
5166
private:
5267
/**
5368
* @brief Find all topics under a given namespace

src/ros2_medkit_gateway/include/ros2_medkit_gateway/rest_server.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ class RESTServer {
4646
void handle_area_components(const httplib::Request& req, httplib::Response& res);
4747
void handle_component_data(const httplib::Request& req, httplib::Response& res);
4848
void handle_component_topic_data(const httplib::Request& req, httplib::Response& res);
49+
void handle_component_topic_publish(const httplib::Request& req, httplib::Response& res);
4950

5051
// Helper methods
5152
std::expected<void, std::string> validate_entity_id(const std::string& entity_id) const;

src/ros2_medkit_gateway/src/data_access_manager.cpp

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -207,4 +207,56 @@ json DataAccessManager::get_component_data(
207207
return result;
208208
}
209209

210+
json DataAccessManager::publish_to_topic(
211+
const std::string& topic_path,
212+
const std::string& msg_type,
213+
const json& data,
214+
double timeout_sec
215+
) {
216+
try {
217+
// Convert JSON data to string for ros2 topic pub
218+
// Note: data.dump() produces JSON format, but ROS 2 CLI accepts JSON
219+
// as valid YAML (JSON is a subset of YAML 1.2)
220+
std::string yaml_data = data.dump();
221+
222+
// TODO(mfaferek93) #32: Check timeout command availability
223+
// GNU coreutils 'timeout' may not be available on all systems (BSD, containers)
224+
// Should check in constructor or provide fallback mechanism
225+
std::ostringstream cmd;
226+
cmd << "timeout " << static_cast<int>(std::ceil(timeout_sec)) << "s "
227+
<< "ros2 topic pub --once -w 0 "
228+
<< ROS2CLIWrapper::escape_shell_arg(topic_path) << " "
229+
<< ROS2CLIWrapper::escape_shell_arg(msg_type) << " "
230+
<< ROS2CLIWrapper::escape_shell_arg(yaml_data);
231+
232+
RCLCPP_INFO(node_->get_logger(), "Executing: %s", cmd.str().c_str());
233+
234+
std::string output = cli_wrapper_->exec(cmd.str());
235+
236+
RCLCPP_INFO(node_->get_logger(),
237+
"Published to topic '%s' with type '%s'",
238+
topic_path.c_str(),
239+
msg_type.c_str());
240+
241+
json result = {
242+
{"topic", topic_path},
243+
{"type", msg_type},
244+
{"status", "published"},
245+
{"timestamp", std::chrono::duration_cast<std::chrono::nanoseconds>(
246+
std::chrono::system_clock::now().time_since_epoch()
247+
).count()}
248+
};
249+
250+
return result;
251+
} catch (const std::exception& e) {
252+
RCLCPP_ERROR(node_->get_logger(),
253+
"Failed to publish to topic '%s': %s",
254+
topic_path.c_str(),
255+
e.what());
256+
throw std::runtime_error(
257+
"Failed to publish to topic '" + topic_path + "': " + e.what()
258+
);
259+
}
260+
}
261+
210262
} // namespace ros2_medkit_gateway

src/ros2_medkit_gateway/src/rest_server.cpp

Lines changed: 191 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414

1515
#include "ros2_medkit_gateway/rest_server.hpp"
1616

17+
#include <algorithm>
1718
#include <iomanip>
1819
#include <sstream>
1920

@@ -78,6 +79,11 @@ void RESTServer::setup_routes() {
7879
server_->Get(R"(/components/([^/]+)/data$)", [this](const httplib::Request& req, httplib::Response& res) {
7980
handle_component_data(req, res);
8081
});
82+
83+
// Component topic publish (PUT)
84+
server_->Put(R"(/components/([^/]+)/data/([^/]+)$)", [this](const httplib::Request& req, httplib::Response& res) {
85+
handle_component_topic_publish(req, res);
86+
});
8187
}
8288

8389
void RESTServer::start() {
@@ -170,13 +176,14 @@ void RESTServer::handle_root(const httplib::Request& req, httplib::Response& res
170176
{"name", "ROS 2 Medkit Gateway"},
171177
{"version", "0.1.0"},
172178
{"endpoints", json::array({
173-
"/health",
174-
"/version-info",
175-
"/areas",
176-
"/components",
177-
"/areas/{area_id}/components",
178-
"/components/{component_id}/data",
179-
"/components/{component_id}/data/{topic_name}"
179+
"GET /health",
180+
"GET /version-info",
181+
"GET /areas",
182+
"GET /components",
183+
"GET /areas/{area_id}/components",
184+
"GET /components/{component_id}/data",
185+
"GET /components/{component_id}/data/{topic_name}",
186+
"PUT /components/{component_id}/data/{topic_name}"
180187
})},
181188
{"capabilities", {
182189
{"discovery", true},
@@ -546,4 +553,181 @@ void RESTServer::handle_component_topic_data(const httplib::Request& req, httpli
546553
}
547554
}
548555

556+
void RESTServer::handle_component_topic_publish(
557+
const httplib::Request& req,
558+
httplib::Response& res
559+
) {
560+
std::string component_id;
561+
std::string topic_name;
562+
try {
563+
// Extract component_id and topic_name from URL path
564+
if (req.matches.size() < 3) {
565+
res.status = StatusCode::BadRequest_400;
566+
res.set_content(
567+
json{{"error", "Invalid request"}}.dump(2),
568+
"application/json"
569+
);
570+
return;
571+
}
572+
573+
component_id = req.matches[1];
574+
topic_name = req.matches[2];
575+
576+
// Validate component_id
577+
auto component_validation = validate_entity_id(component_id);
578+
if (!component_validation) {
579+
res.status = StatusCode::BadRequest_400;
580+
res.set_content(
581+
json{
582+
{"error", "Invalid component ID"},
583+
{"details", component_validation.error()},
584+
{"component_id", component_id}
585+
}.dump(2),
586+
"application/json"
587+
);
588+
return;
589+
}
590+
591+
// Validate topic_name
592+
auto topic_validation = validate_entity_id(topic_name);
593+
if (!topic_validation) {
594+
res.status = StatusCode::BadRequest_400;
595+
res.set_content(
596+
json{
597+
{"error", "Invalid topic name"},
598+
{"details", topic_validation.error()},
599+
{"topic_name", topic_name}
600+
}.dump(2),
601+
"application/json"
602+
);
603+
return;
604+
}
605+
606+
// Parse request body
607+
json body;
608+
try {
609+
body = json::parse(req.body);
610+
} catch (const json::parse_error& e) {
611+
res.status = StatusCode::BadRequest_400;
612+
res.set_content(
613+
json{
614+
{"error", "Invalid JSON in request body"},
615+
{"details", e.what()}
616+
}.dump(2),
617+
"application/json"
618+
);
619+
return;
620+
}
621+
622+
// Validate required fields: type and data
623+
if (!body.contains("type") || !body["type"].is_string()) {
624+
res.status = StatusCode::BadRequest_400;
625+
res.set_content(
626+
json{
627+
{"error", "Missing or invalid 'type' field"},
628+
{"details", "Request body must contain 'type' string field"}
629+
}.dump(2),
630+
"application/json"
631+
);
632+
return;
633+
}
634+
635+
if (!body.contains("data")) {
636+
res.status = StatusCode::BadRequest_400;
637+
res.set_content(
638+
json{
639+
{"error", "Missing 'data' field"},
640+
{"details", "Request body must contain 'data' field"}
641+
}.dump(2),
642+
"application/json"
643+
);
644+
return;
645+
}
646+
647+
std::string msg_type = body["type"].get<std::string>();
648+
json data = body["data"];
649+
650+
// Validate message type format (e.g., std_msgs/msg/Float32)
651+
// Expected format: package/msg/Type (exactly 2 slashes)
652+
size_t slash_count = std::count(msg_type.begin(), msg_type.end(), '/');
653+
size_t msg_pos = msg_type.find("/msg/");
654+
bool valid_format = (slash_count == 2) &&
655+
(msg_pos != std::string::npos) &&
656+
(msg_pos > 0) && // package before /msg/
657+
(msg_pos + 5 < msg_type.length()); // Type after /msg/
658+
659+
if (!valid_format) {
660+
res.status = StatusCode::BadRequest_400;
661+
res.set_content(
662+
json{
663+
{"error", "Invalid message type format"},
664+
{"details", "Message type should be in format: package/msg/Type"},
665+
{"type", msg_type}
666+
}.dump(2),
667+
"application/json"
668+
);
669+
return;
670+
}
671+
672+
const auto cache = node_->get_entity_cache();
673+
674+
// Find component in cache
675+
std::string component_namespace;
676+
bool component_found = false;
677+
678+
for (const auto& component : cache.components) {
679+
if (component.id == component_id) {
680+
component_namespace = component.namespace_path;
681+
component_found = true;
682+
break;
683+
}
684+
}
685+
686+
if (!component_found) {
687+
res.status = StatusCode::NotFound_404;
688+
res.set_content(
689+
json{
690+
{"error", "Component not found"},
691+
{"component_id", component_id}
692+
}.dump(2),
693+
"application/json"
694+
);
695+
return;
696+
}
697+
698+
// Construct full topic path
699+
std::string full_topic_path = (component_namespace == "/")
700+
? "/" + topic_name
701+
: component_namespace + "/" + topic_name;
702+
703+
// Publish data using DataAccessManager
704+
auto data_access_mgr = node_->get_data_access_manager();
705+
json result = data_access_mgr->publish_to_topic(full_topic_path, msg_type, data);
706+
707+
// Add component info to result
708+
result["component_id"] = component_id;
709+
result["topic_name"] = topic_name;
710+
711+
res.set_content(result.dump(2), "application/json");
712+
} catch (const std::exception& e) {
713+
res.status = StatusCode::InternalServerError_500;
714+
res.set_content(
715+
json{
716+
{"error", "Failed to publish to topic"},
717+
{"details", e.what()},
718+
{"component_id", component_id},
719+
{"topic_name", topic_name}
720+
}.dump(2),
721+
"application/json"
722+
);
723+
RCLCPP_ERROR(
724+
rclcpp::get_logger("rest_server"),
725+
"Error in handle_component_topic_publish for component '%s', topic '%s': %s",
726+
component_id.c_str(),
727+
topic_name.c_str(),
728+
e.what()
729+
);
730+
}
731+
}
732+
549733
} // namespace ros2_medkit_gateway

0 commit comments

Comments
 (0)