ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Hi,

what you want to use is topic_tools::ShapeShifter.

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

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 );