diff --git a/README.md b/README.md index a82bb89..bf7f86f 100644 --- a/README.md +++ b/README.md @@ -267,7 +267,7 @@ bridge: As seen in the [Quick Start](#quick-start), the *mqtt_client* can not only exchange arbitrary ROS messages with other *mqtt_clients*, but it can also exchange primitive message data with other non-*mqtt_client* MQTT clients. This allows ROS-based devices to exchange primitive messages with devices not based on ROS. The `primitive` parameter can be set for both ROS-to-MQTT (`bridge/ros2mqtt`) and for MQTT-to-ROS (`bridge/mqtt2ros`) transmissions. -If a ROS-to-MQTT transmission is configured as `primitive` and the ROS message type is one of the supported primitive ROS message types, the raw data is published as a string. The supported primitive ROS message types are [`std_msgs/String`](http://docs.ros.org/en/api/std_msgs/html/msg/String.html), [`std_msgs/Bool`](http://docs.ros.org/en/api/std_msgs/html/msg/Bool.html), [`std_msgs/Char`](http://docs.ros.org/en/api/std_msgs/html/msg/Char.html), [`std_msgs/UInt8`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt8.html), [`std_msgs/UInt16`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt16.html), [`std_msgs/UInt32`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt32.html), [`std_msgs/UInt64`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt64.html), [`std_msgs/Int8`](http://docs.ros.org/en/api/std_msgs/html/msg/Int8.html), [`std_msgs/Int16`](http://docs.ros.org/en/api/std_msgs/html/msg/Int16.html), [`std_msgs/Int32`](http://docs.ros.org/en/api/std_msgs/html/msg/Int32.html), [`std_msgs/Int64`](http://docs.ros.org/en/api/std_msgs/html/msg/Int64.html), [`std_msgs/Float32`](http://docs.ros.org/en/api/std_msgs/html/msg/Float32.html), [`std_msgs/Float64`](http://docs.ros.org/en/api/std_msgs/html/msg/Float64.html). +If a ROS-to-MQTT transmission is configured as `primitive` and the ROS message type is one of the supported primitive ROS message types, the raw data is published as a string. The supported primitive ROS message types are [`std_msgs/String`](http://docs.ros.org/en/api/std_msgs/html/msg/String.html), [`std_msgs/Bool`](http://docs.ros.org/en/api/std_msgs/html/msg/Bool.html), [`std_msgs/Char`](http://docs.ros.org/en/api/std_msgs/html/msg/Char.html), [`std_msgs/UInt8`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt8.html), [`std_msgs/UInt16`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt16.html), [`std_msgs/UInt32`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt32.html), [`std_msgs/UInt64`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt64.html), [`std_msgs/Int8`](http://docs.ros.org/en/api/std_msgs/html/msg/Int8.html), [`std_msgs/Int16`](http://docs.ros.org/en/api/std_msgs/html/msg/Int16.html), [`std_msgs/Int32`](http://docs.ros.org/en/api/std_msgs/html/msg/Int32.html), [`std_msgs/Int64`](http://docs.ros.org/en/api/std_msgs/html/msg/Int64.html), [`std_msgs/Float32`](http://docs.ros.org/en/api/std_msgs/html/msg/Float32.html), [`std_msgs/Float64`](http://docs.ros.org/en/api/std_msgs/html/msg/Float64.html), [`std_msgs/UInt8MultiArray`](http://docs.ros.org/en/api/std_msgs/html/msg/UInt8MultiArray.html). If an MQTT-to-ROS transmission is configured as `primitive`, the MQTT message is interpreted and published as a primitive data type, if possible. The message is probed in the following order: `bool` ([`std_msgs/Bool`](http://docs.ros.org/en/api/std_msgs/html/msg/Bool.html)), `int` ([`std_msgs/Int32`](http://docs.ros.org/en/api/std_msgs/html/msg/Int32.html)), `float` ([`std_msgs/Float32`](http://docs.ros.org/en/api/std_msgs/html/msg/Float32.html)), `string` ([`std_msgs/String`](http://docs.ros.org/en/api/std_msgs/html/msg/String.html)). diff --git a/mqtt_client/src/MqttClient.cpp b/mqtt_client/src/MqttClient.cpp index de2bf6c..771a628 100644 --- a/mqtt_client/src/MqttClient.cpp +++ b/mqtt_client/src/MqttClient.cpp @@ -48,6 +48,7 @@ SOFTWARE. #include #include #include +#include #include RCLCPP_COMPONENTS_REGISTER_NODE(mqtt_client::MqttClient) @@ -164,6 +165,12 @@ bool fixedMqtt2PrimitiveRos(mqtt::const_message_ptr mqtt_msg, std_msgs::msg::Float64 msg; msg.data = mqtt2float(mqtt_msg); + serializeRosMessage(msg, serialized_msg); + } else if (msg_type == "std_msgs/msg/UInt8MultiArray") { + std_msgs::msg::UInt8MultiArray msg; + const std::string& str_msg = mqtt_msg->to_string(); + msg.data = std::vector(str_msg.begin(), str_msg.end()); + serializeRosMessage(msg, serialized_msg); } else { throw std::domain_error("Unhandled message type (" + msg_type + ")"); @@ -251,6 +258,10 @@ bool primitiveRosMessageToString( std_msgs::msg::Float64 msg; deserializeRosMessage(*serialized_msg, msg); primitive = std::to_string(msg.data); + } else if (msg_type == "std_msgs/msg/UInt8MultiArray") { + std_msgs::msg::UInt8MultiArray msg; + deserializeRosMessage(*serialized_msg, msg); + primitive = std::string(msg.data.begin(), msg.data.end()); } else { found_primitive = false; }