35 #ifndef ROSSERIAL_SERVER_TOPIC_HANDLERS_H 36 #define ROSSERIAL_SERVER_TOPIC_HANDLERS_H 39 #include <rosserial_msgs/TopicInfo.h> 40 #include <rosserial_msgs/RequestMessageInfo.h> 41 #include <rosserial_msgs/RequestServiceInfo.h> 54 ROS_WARN(
"Timed out waiting for message_info service to become available.");
58 rosserial_msgs::RequestMessageInfo info;
59 info.request.type = topic_info.message_type;
61 if (info.response.md5 != topic_info.md5sum) {
62 ROS_WARN_STREAM(
"Message" << topic_info.message_type <<
"MD5 sum from client does not match that in system. Will avoid using system's message definition.");
63 info.response.definition =
"";
66 ROS_WARN(
"Failed to call message_info service. Proceeding without full message definition.");
69 message_.
morph(topic_info.md5sum, topic_info.message_type, info.response.definition,
"false");
96 boost::function<
void(std::vector<uint8_t>& buffer)> write_fn)
97 : write_fn_(write_fn) {
101 opts.
md5sum = topic_info.md5sum;
102 opts.
datatype = topic_info.message_type;
107 return subscriber_.getTopic();
113 std::vector<uint8_t> buffer(length);
122 boost::function<void(std::vector<uint8_t>& buffer)> write_fn_;
130 boost::function<
void(std::vector<uint8_t>& buffer,
const uint16_t topic_id)> write_fn)
131 : write_fn_(write_fn) {
133 if (!service_info_service_.isValid()) {
135 service_info_service_ = nh.
serviceClient<rosserial_msgs::RequestServiceInfo>(
"service_info");
136 if (!service_info_service_.waitForExistence(
ros::Duration(5.0))) {
137 ROS_WARN(
"Timed out waiting for service_info service to become available.");
141 rosserial_msgs::RequestServiceInfo info;
142 info.request.service = topic_info.message_type;
143 ROS_DEBUG(
"Calling service_info service for topic name %s",topic_info.topic_name.c_str());
144 if (service_info_service_.call(info)) {
145 request_message_md5_ = info.response.request_md5;
146 response_message_md5_ = info.response.response_md5;
148 ROS_WARN(
"Failed to call service_info service. The service client will be created with blank md5sum.");
151 opts.
service = topic_info.topic_name;
152 opts.
md5sum = service_md5_ = info.response.service_md5;
157 topic_id_ = topic_id;
160 return request_message_md5_;
163 return response_message_md5_;
173 service_client_.call(request_message_, response_message_, service_md5_);
177 std::vector<uint8_t> buffer(length);
180 write_fn_(buffer,topic_id_);
188 boost::function<void(std::vector<uint8_t>& buffer,
const uint16_t topic_id)> write_fn_;
200 #endif // ROSSERIAL_SERVER_TOPIC_HANDLERS_H ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
Subscriber(ros::NodeHandle &nh, rosserial_msgs::TopicInfo &topic_info, boost::function< void(std::vector< uint8_t > &buffer)> write_fn)
static void write(Stream &stream, typename boost::call_traits< T >::param_type t)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void handle(const boost::shared_ptr< topic_tools::ShapeShifter const > &msg)
boost::shared_ptr< Publisher > PublisherPtr
bool call(MReq &req, MRes &res)
ServiceClient(ros::NodeHandle &nh, rosserial_msgs::TopicInfo &topic_info, boost::function< void(std::vector< uint8_t > &buffer, const uint16_t topic_id)> write_fn)
ros::Subscriber subscriber_
ros::ServiceClient service_client_
boost::shared_ptr< Subscriber > SubscriberPtr
Publisher(ros::NodeHandle &nh, const rosserial_msgs::TopicInfo &topic_info)
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
std::string request_message_md5_
topic_tools::ShapeShifter request_message_
topic_tools::ShapeShifter response_message_
std::string response_message_md5_
static void read(Stream &stream, typename boost::call_traits< T >::reference t)
std::string getRequestMessageMD5()
static ros::ServiceClient message_service_
void setTopicId(uint16_t topic_id)
#define ROS_WARN_STREAM(args)
boost::shared_ptr< ServiceClient > ServiceClientPtr
void handle(ros::serialization::IStream stream)
topic_tools::ShapeShifter message_
static ros::ServiceClient service_info_service_
uint32_t serializationLength(const T &t)
ros::Publisher publisher_
void init(const std::string &_topic, uint32_t _queue_size, const boost::function< void(const boost::shared_ptr< M const > &)> &_callback, const boost::function< boost::shared_ptr< M >(void)> &factory_fn=DefaultMessageCreator< M >())
std::string getTopic() const
void handle(ros::serialization::IStream stream)
std::string getResponseMessageMD5()