ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
If you are sure that the shapeshifter contains a message with a std_msgs::Header
, you can try temporarily morphing the shapeshifter into a Header and then instantiating it. This is essentially the same as always serializing it, but because you don't have to have raw uint8_t
buffers floating around, it may be more understandable:
ros::Time readStamp(
const topic_tools::ShapeShifter::ConstPtr& shapeshifter) {
topic_tools::ShapeShifter* ss_tmp =
const_cast<topic_tools::ShapeShifter*>(shapeshifter.get());
auto md5sum = ss_tmp->getMD5Sum();
auto datatype = ss_tmp->getDataType();
auto msgdef = ss_tmp->getMessageDefinition();
ss_tmp->morph(ros::message_traits::md5sum<std_msgs::Header>(),
ros::message_traits::datatype<std_msgs::Header>(), "",
"");
auto tmp = ss_tmp->instantiate<std_msgs::Header>();
ss_tmp->morph(md5sum, datatype, msgdef, "");
return tmp->stamp;
}
Please note that if you make a copy of ShapeShifter
then your code will crash. This is because the ShapeShifter
class has a pointer free in its destructor but it does not have a deep copy constructor. So, copying it will result in a double pointer free bug when both instances go out of scope.