Generic ROS publisher using ShapeShifter

GenericROSPublisher.cpp

#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
#include "topic_tools/shape_shifter.h"
#include "ros/message_traits.h"
#include <sstream>

template <typename T>
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);
}


int main(int argc, char **argv)
{

  ros::init(argc, argv, "talker");

  ros::NodeHandle node_handler;

  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 joints_publisher = shape_shifter.advertise(node_handler, "my_jointstate", 1);

  ros::Rate loop_rate(10);

  sensor_msgs::JointState joint_state;

  // prepare the message
  joint_state.name.resize( 3 );
  joint_state.position.resize( 3 );
  joint_state.velocity.resize( 3 );
  joint_state.effort.resize( 3 );

  joint_state.name[0] = ("joint_1");
  joint_state.name[1] = ("joint_2");
  joint_state.name[2] = ("joint_3");

  std::vector<uint8_t> buffer;

  uint32_t count = 0;
  while (ros::ok())
  {

    // update the message
    joint_state.header.seq = count;
    joint_state.header.stamp = ros::Time::now();

    for (size_t i=0; i<3; i++)
    {
      joint_state.position[i]= count;
      joint_state.velocity[i]= count;
      joint_state.effort[i]= count;
    }

    // serialize the message inside the shape_shifter instance
    SerializeToByteArray(joint_state, buffer);
    ros::serialization::OStream stream( buffer.data(), buffer.size() );
    shape_shifter.read( stream );
    joints_publisher.publish( shape_shifter );

    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }


  return 0;
}