Ask Your Question
0

Transform data to a buffer and publish it

asked 2017-09-12 05:09:30 -0500

Mehdi. gravatar image

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.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2017-09-12 09:50:00 -0500

Davide Faconti gravatar image

Hi,

what you want to use is topic_tools::ShapeShifter.

Look at the following GISt for a complete example https://gist.github.com/facontidavide...

The previous answer is correct. Use a function like this

void SerializeToByteArray(const T& msg,  std::vector<uint8_t>& destination_buffer)
{
  const uint32_t length = ros::serialization::serializationLength(msg);
  destination_buffer.resize( length );
  //copy into your own buffer
  ros::serialization::OStream stream(destination_buffer.data(), length);
  ros::serialization::serialize(stream, msg);
}

Furthermore, to publish this message in a "generic way, you need to create an instance of ShapeShifter like this (lets suppose you want to serialize sensor_msgs::JointState

topic_tools::ShapeShifter shape_shifter;
shape_shifter.morph( 
    ros::message_traits::MD5Sum<sensor_msgs::JointState>::value(),
    ros::message_traits::DataType<sensor_msgs::JointState>::value(),
    ros::message_traits::Definition<sensor_msgs::JointState>::value(),
    "" );

ros::Publisher ss_publisher = shape_shifter.advertise(node_handler, "my_jointstate", 1);

Now you have a generic publisher. Every time you have an instance of sensor_msgs::JointState that you want to publish, just do this

sensor_msgs::JointState msg; 
// prepare the message to send 

static std::vector<uint8_t> buffer; 
SerializeToByteArray(msg, buffer); 
ros::serialization::OStream stream( buffer.data(), buffer.size() ); 
shape_shifter.read( stream );  
ss_publisher.publish( shape_shifter );
edit flag offensive delete link more
1

answered 2017-09-12 05:40:34 -0500

gvdhoorn gravatar image

I think you're looking for message traits.

See roscpp/Overview/Messages/SerializationAndAdaptingTypes for some more information.

edit flag offensive delete link more

Comments

I still don't see how I can go from having an array of uint8_t to running publish(something_built_from_that_array). The array I get it from the message received by doing

buffer.resize(msg->size()); 
ros::serialization::OStream stream(buffer.data(), buffer.size());
msg->write(stream);
Mehdi. gravatar image Mehdi.  ( 2017-09-12 05:56:26 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-09-12 05:09:30 -0500

Seen: 1,741 times

Last updated: Sep 12 '17