Transform data to a buffer and publish it
lets say I have an std_msgs/String or a sensor_msgs/JointState and I want to publish it without using the normal ros publisher. for the std_msgs/String for example all I have is an std::string variable. Assuming I know how the message definition but don't have a class implementing that message in c++, how could I transform the data I have into a byte array buffer and how could I publish it to a topic?
What I tried until now results in crashing my program. I ve found some code in ros answers but didn't get it to work yet.
class ByteArrayMessage
{
public:
ByteArrayMessage(uint8_t* buffer, uint32_t length, std::string dataType, std::string md5sum, std::string messageDefinition)
: __serialized_length(length),
m_dataType(dataType),
m_md5sum(md5sum),
m_messageDefinition(messageDefinition),
buffer_(buffer), length_(length)
{}
const std::string __getDataType() const { return m_dataType; }
const std::string __getMD5Sum() const { return m_md5sum; }
const std::string __getMessageDefinition() const { return m_messageDefinition; }
uint32_t serializationLength() const { return __serialized_length; }
const uint8_t *serialize(uint8_t *write_ptr, uint32_t seq) const;
uint8_t byte(size_t i) { return buffer_[i]; }
uint32_t length() { return length_; }
uint32_t __serialized_length;
private:
std::string m_dataType;
std::string m_md5sum;
std::string m_messageDefinition;
uint8_t *buffer_;
uint32_t length_;
};
const uint8_t* ByteArrayMessage::serialize(uint8_t *write_ptr, uint32_t seq) const
{
ros::serialization::OStream stream(write_ptr, 1000000000);
for (size_t i = 0; i < length_; i++)
{
stream.next(buffer_[i]);
}
return stream.getData();
}
using this class, and assuming I already have a buffer containing my uint8_t data (an std::vector<uint8_t>) I do:
ByteArrayMessage msg_out(buffer.data(), buffer.size(), datatype, msg->getMD5Sum(), definition);
ShapeShifter sh;
sh.morph(msg->getMD5Sum(), datatype, definition, "");
ros::Publisher pub = sh.advertise(node, topic_name,10);
std::cout << "Advertised" << std::endl;
pub.publish(msg_out);
In this case I subscribe to a message normally, get the message's content and then try to resend it back as a byte array usning ShapeShifter. When I run this code, it results in an error
[/talker FATAL 1505210753.217723556]: ASSERTION FAILED
file = /opt/ros/kinetic/include/ros/publisher.h
line = 115
cond = impl_->md5sum_ == "*" || std::string(mt::md5sum<M>(message)) == "*" || impl_->md5sum_ == mt::md5sum<M>(message)
message =
[/talker FATAL 1505210753.217776260]: Trying to publish message of type [?@d/ @d] on a publisher with type [std_msgs/Float32/73fcbf46b49191e672908e50842a83d4]
[/talker FATAL 1505210753.217792578]:
(In this specific case, I published a Float32 because it is very simple to understand it in bytes) I double checked the md5 sum inside the ByteArrayMessage and the message definition and they all seem to be correct.