modify for Hydro version
Fork of rosserial_mbed_lib by
Diff: ros/node_output.h
- Revision:
- 0:77afd7560544
- Child:
- 1:ff0ec969dad1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros/node_output.h Fri Aug 19 09:06:30 2011 +0000 @@ -0,0 +1,79 @@ +/* + * NodeOutput.h + * + * Created on: Aug 5, 2011 + * Author: astambler + */ + +#ifndef NODEOUTPUT_H_ +#define NODEOUTPUT_H_ + +/* + * This class is responsible for controlling the node ouput. + * It it is the object that is passed to Publishers and services + */ + +#include "msg.h" + +namespace ros { + +class NodeOutput_ { +public: + virtual int publish(short id, Msg* msg)=0; +}; + + +template<class Hardware, int OUTSIZE =512> +class NodeOutput : public NodeOutput_ { + +private: + Hardware* hardware_; + bool configured_; + unsigned char message_out[OUTSIZE]; + +public: + NodeOutput(Hardware* h) { + hardware_ = h; + configured_ = false; + } + + NodeOutput() {}; + void setHardware(Hardware* h) { + hardware_ = h; + configured_=false; + } + + void setConfigured(bool b) { + configured_ =b; + } + bool configured() { + return configured_; + }; + + virtual int publish(short id, Msg * msg) { + if (!configured_) return 0; + + //short test_id,test_off; + /* serialize message */ + short 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 */ + short chk = 0; + for (int i =2; i<l+6; i++) + chk += message_out[i]; + message_out[6+l] = 255 - (chk%256); + + hardware_->write(message_out, 6+l+1); + return 1; + } +}; +} +#endif /* NODEOUTPUT_H_ */