This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial. This program contains an example.
Dependencies: rosserial_mbed_lib mbed Servo
Diff: ros/node_handle.h
- Revision:
- 3:dff241b66f84
- Parent:
- 1:098e75fd5ad2
diff -r 094e5153a559 -r dff241b66f84 ros/node_handle.h --- a/ros/node_handle.h Sun Oct 16 09:33:53 2011 +0000 +++ b/ros/node_handle.h Sat Nov 12 23:53:04 2011 +0000 @@ -35,10 +35,10 @@ #ifndef ROS_NODE_HANDLE_H_ #define ROS_NODE_HANDLE_H_ -#include "../std_msgs/Time.h" -#include "../rosserial_msgs/TopicInfo.h" -#include "../rosserial_msgs/Log.h" -#include "../rosserial_msgs/RequestParam.h" +#include "std_msgs/Time.h" +#include "rosserial_msgs/TopicInfo.h" +#include "rosserial_msgs/Log.h" +#include "rosserial_msgs/RequestParam.h" #define SYNC_SECONDS 5 @@ -53,13 +53,24 @@ #define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data -#include "node_output.h" +#include "msg.h" + +namespace ros { + +class NodeHandleBase_ { +public: + virtual int publish(int16_t id, const Msg* msg)=0; + virtual int spinOnce()=0; + virtual bool connected()=0; +}; + +} #include "publisher.h" -#include "msg_receiver.h" #include "subscriber.h" -#include "rosserial_ids.h" #include "service_server.h" +#include "service_client.h" + namespace ros { @@ -71,10 +82,9 @@ int MAX_PUBLISHERS=25, int INPUT_SIZE=512, int OUTPUT_SIZE=512> -class NodeHandle_ { +class NodeHandle_ : public NodeHandleBase_ { protected: Hardware hardware_; - NodeOutput<Hardware, OUTPUT_SIZE> no_; /* time used for syncing */ unsigned long rt_time; @@ -83,15 +93,16 @@ unsigned long sec_offset, nsec_offset; unsigned char message_in[INPUT_SIZE]; + unsigned char message_out[OUTPUT_SIZE]; Publisher * publishers[MAX_PUBLISHERS]; - MsgReceiver * receivers[MAX_SUBSCRIBERS]; + Subscriber_ * subscribers[MAX_SUBSCRIBERS]; /* * Setup Functions */ public: - NodeHandle_() : no_(&hardware_) {} + NodeHandle_() : configured_(false) {} Hardware* getHardware() { return &hardware_; @@ -105,18 +116,18 @@ index_ = 0; topic_ = 0; checksum_=0; - - total_receivers=0; }; protected: - //State machine variables for spinOnce +//State machine variables for spinOnce int mode_; int bytes_; int topic_; int index_; int checksum_; + bool configured_; + int total_receivers; /* used for syncing the time */ @@ -124,26 +135,17 @@ unsigned long last_sync_receive_time; unsigned long last_msg_timeout_time; - bool registerReceiver(MsgReceiver* rcv) { - if (total_receivers >= MAX_SUBSCRIBERS) - return false; - receivers[total_receivers] = rcv; - rcv->id_ = 100+total_receivers; - total_receivers++; - return true; - } - public: /* This function goes in your loop() function, it handles * serial input and callbacks for subscribers. */ - virtual void spinOnce() { + virtual int spinOnce() { /* restart if timed out */ unsigned long c_time = hardware_.time(); if ( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ) { - no_.setConfigured(false); + configured_ = false; } /* reset if message has timed out */ @@ -192,45 +194,48 @@ if (bytes_ == 0) mode_ = MODE_CHECKSUM; } else if ( mode_ == MODE_CHECKSUM ) { /* do checksum */ + mode_ = MODE_FIRST_FF; if ( (checksum_%256) == 255) { - if (topic_ == TOPIC_NEGOTIATION) { + if (topic_ == TopicInfo::ID_PUBLISHER) { requestSyncTime(); negotiateTopics(); last_sync_time = c_time; last_sync_receive_time = c_time; + return -1; } else if (topic_ == TopicInfo::ID_TIME) { syncTime(message_in); } else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST) { req_param_resp.deserialize(message_in); param_recieved= true; } else { - if (receivers[topic_-100]) - receivers[topic_-100]->receive( message_in ); + if (subscribers[topic_-100]) + subscribers[topic_-100]->callback( message_in ); } } - mode_ = MODE_FIRST_FF; } } /* occasionally sync time */ - if ( no_.configured() && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )) { + if ( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )) { requestSyncTime(); last_sync_time = c_time; } + + return 0; } /* Are we connected to the PC? */ - bool connected() { - return no_.configured(); + virtual bool connected() { + return configured_; }; - /* + /******************************************************************** * Time functions */ void requestSyncTime() { std_msgs::Time t; - no_.publish( rosserial_msgs::TopicInfo::ID_TIME, &t); + publish(TopicInfo::ID_TIME, &t); rt_time = hardware_.time(); } @@ -263,37 +268,66 @@ normalizeSecNSec(sec_offset, nsec_offset); } - /* - * Registration + /******************************************************************** + * Topic Management */ + /* Register a new publisher */ bool advertise(Publisher & p) { - int i; - for (i = 0; i < MAX_PUBLISHERS; i++) { + for (int i = 0; i < MAX_PUBLISHERS; i++) { if (publishers[i] == 0) { // empty slot publishers[i] = &p; p.id_ = i+100+MAX_SUBSCRIBERS; - p.no_ = &this->no_; + p.nh_ = this; + return true; + } + } + return false; + } + + /* Register a new subscriber */ + template<typename MsgT> + bool subscribe(Subscriber< MsgT> & s) { + for (int i = 0; i < MAX_SUBSCRIBERS; i++) { + if (subscribers[i] == 0) { // empty slot + subscribers[i] = (Subscriber_*) &s; + s.id_ = i+100; return true; } } return false; } - /* Register a subscriber with the node */ - template<typename MsgT> - bool subscribe(Subscriber< MsgT> &s) { - return registerReceiver((MsgReceiver*) &s); + /* Register a new Service Server */ + template<typename MReq, typename MRes> + bool advertiseService(ServiceServer<MReq,MRes>& srv) { + bool v = advertise(srv.pub); + for (int i = 0; i < MAX_SUBSCRIBERS; i++) { + if (subscribers[i] == 0) { // empty slot + subscribers[i] = (Subscriber_*) &srv; + srv.id_ = i+100; + return v; + } + } + return false; } - template<typename SrvReq, typename SrvResp> - bool advertiseService(ServiceServer<SrvReq,SrvResp>& srv) { - srv.no_ = &no_; - return registerReceiver((MsgReceiver*) &srv); + /* Register a new Service Client */ + template<typename MReq, typename MRes> + bool serviceClient(ServiceClient<MReq, MRes>& srv) { + bool v = advertise(srv.pub); + for (int i = 0; i < MAX_SUBSCRIBERS; i++) { + if (subscribers[i] == 0) { // empty slot + subscribers[i] = (Subscriber_*) &srv; + srv.id_ = i+100; + return v; + } + } + return false; } void negotiateTopics() { - no_.setConfigured(true); + configured_ = true; rosserial_msgs::TopicInfo ti; int i; for (i = 0; i < MAX_PUBLISHERS; i++) { @@ -301,20 +335,57 @@ ti.topic_id = publishers[i]->id_; ti.topic_name = (char *) publishers[i]->topic_; ti.message_type = (char *) publishers[i]->msg_->getType(); - no_.publish( TOPIC_PUBLISHERS, &ti ); + ti.md5sum = (char *) publishers[i]->msg_->getMD5(); + ti.buffer_size = OUTPUT_SIZE; + + publish( publishers[i]->getEndpointType(), &ti ); } } for (i = 0; i < MAX_SUBSCRIBERS; i++) { - if (receivers[i] != 0) { // non-empty slot - ti.topic_id = receivers[i]->id_; - ti.topic_name = (char *) receivers[i]->topic_; - ti.message_type = (char *) receivers[i]->getMsgType(); - no_.publish( TOPIC_SUBSCRIBERS, &ti ); + if (subscribers[i] != 0) { // non-empty slot + ti.topic_id = subscribers[i]->id_; + ti.topic_name = (char *) subscribers[i]->topic_; + ti.message_type = (char *) subscribers[i]->getMsgType(); + ti.md5sum = (char *) subscribers[i]->getMsgMD5(); + ti.buffer_size = INPUT_SIZE; + publish( subscribers[i]->getEndpointType(), &ti ); } } } - /* + virtual int publish(int16_t id, const Msg * msg) + { + if (!configured_) return 0; + + /* serialize message */ + int16_t l = msg->serialize(message_out+6); + + /* setup the header */ + message_out[0] = 0xff; + message_out[1] = 0xff; + message_out[2] = (unsigned char) id&255; + message_out[3] = (unsigned char) id>>8; + message_out[4] = (unsigned char) l&255; + message_out[5] = ((unsigned char) l>>8); + + /* calculate checksum */ + int chk = 0; + for (int i =2; i<l+6; i++) + chk += message_out[i]; + l += 6; + message_out[l++] = 255 - (chk%256); + + if ( l <= OUTPUT_SIZE ) { + hardware_.write(message_out, l); + return l; + } else { + logerror("Message from device dropped: message larger than buffer."); + return 1; + } + + } + + /******************************************************************** * Logging */ @@ -323,7 +394,7 @@ rosserial_msgs::Log l; l.level= byte; l.msg = (char*)msg; - this->no_.publish(rosserial_msgs::TopicInfo::ID_LOG, &l); + publish(rosserial_msgs::TopicInfo::ID_LOG, &l); } public: @@ -344,8 +415,8 @@ } - /* - * Retrieve Parameters + /******************************************************************** + * Parameters */ private: @@ -356,7 +427,7 @@ param_recieved = false; rosserial_msgs::RequestParamRequest req; req.name = (char*)name; - no_.publish(TopicInfo::ID_PARAMETER_REQUEST, &req); + publish(TopicInfo::ID_PARAMETER_REQUEST, &req); int end_time = hardware_.time(); while (!param_recieved ) { spinOnce();