string doQueue(const ros::MessageEvent<topic_tools::ShapeShifter const>& msg_event, string const& topic, shared_ptr<ros::Subscriber> subscriber, shared_ptr<int> count) {
static int once = 0;
ros::Time rectime = ros::Time::now();
rosbag::OutgoingMessage out(topic, msg_event.getMessage(), msg_event.getConnectionHeaderPtr(), rectime);
std::cout<<"topic MD5: "<<out.msg->getMD5Sum()<<" dataType: "<<out.msg->getDataType()<<
" getMessageDefinition "<<out.msg->getMessageDefinition()<<
" size: "<<out.connection_header->size()
<<std::endl;
if(once==0)
{
ros::NodeHandle node;
if(static_cast<std::string>(ros::message_traits::MD5Sum<msgs_ht::Track_Control>::value()) ==out.msg->getMD5Sum())
{
Track_sub = node.subscribe("/track_ctrl", 1000, &RosCommunication::track_listen_back<msgs_ht::Track_Control::ConstPtr>,this);
}
else if(static_cast<std::string>(ros::message_traits::MD5Sum<msgs_ht1::Track_Control>::value()) ==out.msg->getMD5Sum())
{
Track_sub = node.subscribe("/track_ctrl", 1000, &RosCommunication::track_listen_back<msgs_ht1::Track_Control::ConstPtr>,this);
}
ReadMD5.insert(map<string,string>::value_type (out.msg->getMD5Sum(),"/track_ctrl"));
once=1;
}
return out.msg->getMD5Sum();
}
template<typename T>
double getTopicInfo(string const &topic,T t){
ROS_INFO("Subscribing to %s", topic.c_str());
ros::NodeHandle nh;
shared_ptr<int> count(make_shared<int>(2));
shared_ptr<ros::Subscriber> sub(make_shared<ros::Subscriber>());
ros::SubscribeOptions ops;
ops.topic = topic;
//ops.queue_size = 100;
ops.md5sum = ros::message_traits::md5sum<topic_tools::ShapeShifter>();
ops.datatype = ros::message_traits::datatype<topic_tools::ShapeShifter>();
ops.helper = boost::make_shared<ros::SubscriptionCallbackHelperT<
const ros::MessageEvent<topic_tools::ShapeShifter const> &> >(
boost::bind(t, this, _1, topic, sub, count));
ros::TransportHints transport_hints;
ops.transport_hints = transport_hints;
*sub = nh.subscribe(ops);
}
getTopicInfo("/track_ctrl",&RosCommunication::doQueue);
只贴主要代码,感兴趣的人去查找相关文档。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)