liu isad
rosserial for Hydro
Fork of
rosserial_mbed_lib
by 
nucho
ros/node_output.h@0:77afd7560544, 2011-08-19 (annotated)
- Committer:
- nucho
- Date:
- Fri Aug 19 09:06:30 2011 +0000
- Revision:
- 0:77afd7560544
- Child:
- 1:ff0ec969dad1
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| nucho | 0:77afd7560544 | 1 | /* |
| nucho | 0:77afd7560544 | 2 | * NodeOutput.h |
| nucho | 0:77afd7560544 | 3 | * |
| nucho | 0:77afd7560544 | 4 | * Created on: Aug 5, 2011 |
| nucho | 0:77afd7560544 | 5 | * Author: astambler |
| nucho | 0:77afd7560544 | 6 | */ |
| nucho | 0:77afd7560544 | 7 | |
| nucho | 0:77afd7560544 | 8 | #ifndef NODEOUTPUT_H_ |
| nucho | 0:77afd7560544 | 9 | #define NODEOUTPUT_H_ |
| nucho | 0:77afd7560544 | 10 | |
| nucho | 0:77afd7560544 | 11 | /* |
| nucho | 0:77afd7560544 | 12 | * This class is responsible for controlling the node ouput. |
| nucho | 0:77afd7560544 | 13 | * It it is the object that is passed to Publishers and services |
| nucho | 0:77afd7560544 | 14 | */ |
| nucho | 0:77afd7560544 | 15 | |
| nucho | 0:77afd7560544 | 16 | #include "msg.h" |
| nucho | 0:77afd7560544 | 17 | |
| nucho | 0:77afd7560544 | 18 | namespace ros { |
| nucho | 0:77afd7560544 | 19 | |
| nucho | 0:77afd7560544 | 20 | class NodeOutput_ { |
| nucho | 0:77afd7560544 | 21 | public: |
| nucho | 0:77afd7560544 | 22 | virtual int publish(short id, Msg* msg)=0; |
| nucho | 0:77afd7560544 | 23 | }; |
| nucho | 0:77afd7560544 | 24 | |
| nucho | 0:77afd7560544 | 25 | |
| nucho | 0:77afd7560544 | 26 | template<class Hardware, int OUTSIZE =512> |
| nucho | 0:77afd7560544 | 27 | class NodeOutput : public NodeOutput_ { |
| nucho | 0:77afd7560544 | 28 | |
| nucho | 0:77afd7560544 | 29 | private: |
| nucho | 0:77afd7560544 | 30 | Hardware* hardware_; |
| nucho | 0:77afd7560544 | 31 | bool configured_; |
| nucho | 0:77afd7560544 | 32 | unsigned char message_out[OUTSIZE]; |
| nucho | 0:77afd7560544 | 33 | |
| nucho | 0:77afd7560544 | 34 | public: |
| nucho | 0:77afd7560544 | 35 | NodeOutput(Hardware* h) { |
| nucho | 0:77afd7560544 | 36 | hardware_ = h; |
| nucho | 0:77afd7560544 | 37 | configured_ = false; |
| nucho | 0:77afd7560544 | 38 | } |
| nucho | 0:77afd7560544 | 39 | |
| nucho | 0:77afd7560544 | 40 | NodeOutput() {}; |
| nucho | 0:77afd7560544 | 41 | void setHardware(Hardware* h) { |
| nucho | 0:77afd7560544 | 42 | hardware_ = h; |
| nucho | 0:77afd7560544 | 43 | configured_=false; |
| nucho | 0:77afd7560544 | 44 | } |
| nucho | 0:77afd7560544 | 45 | |
| nucho | 0:77afd7560544 | 46 | void setConfigured(bool b) { |
| nucho | 0:77afd7560544 | 47 | configured_ =b; |
| nucho | 0:77afd7560544 | 48 | } |
| nucho | 0:77afd7560544 | 49 | bool configured() { |
| nucho | 0:77afd7560544 | 50 | return configured_; |
| nucho | 0:77afd7560544 | 51 | }; |
| nucho | 0:77afd7560544 | 52 | |
| nucho | 0:77afd7560544 | 53 | virtual int publish(short id, Msg * msg) { |
| nucho | 0:77afd7560544 | 54 | if (!configured_) return 0; |
| nucho | 0:77afd7560544 | 55 | |
| nucho | 0:77afd7560544 | 56 | //short test_id,test_off; |
| nucho | 0:77afd7560544 | 57 | /* serialize message */ |
| nucho | 0:77afd7560544 | 58 | short l = msg->serialize(message_out+6); |
| nucho | 0:77afd7560544 | 59 | |
| nucho | 0:77afd7560544 | 60 | /* setup the header */ |
| nucho | 0:77afd7560544 | 61 | message_out[0] = 0xff; |
| nucho | 0:77afd7560544 | 62 | message_out[1] = 0xff; |
| nucho | 0:77afd7560544 | 63 | message_out[2] = (unsigned char) id&255; |
| nucho | 0:77afd7560544 | 64 | message_out[3] = (unsigned char) id>>8; |
| nucho | 0:77afd7560544 | 65 | message_out[4] = (unsigned char) l&255; |
| nucho | 0:77afd7560544 | 66 | message_out[5] = ((unsigned char) l>>8); |
| nucho | 0:77afd7560544 | 67 | |
| nucho | 0:77afd7560544 | 68 | /* calculate checksum */ |
| nucho | 0:77afd7560544 | 69 | short chk = 0; |
| nucho | 0:77afd7560544 | 70 | for (int i =2; i<l+6; i++) |
| nucho | 0:77afd7560544 | 71 | chk += message_out[i]; |
| nucho | 0:77afd7560544 | 72 | message_out[6+l] = 255 - (chk%256); |
| nucho | 0:77afd7560544 | 73 | |
| nucho | 0:77afd7560544 | 74 | hardware_->write(message_out, 6+l+1); |
| nucho | 0:77afd7560544 | 75 | return 1; |
| nucho | 0:77afd7560544 | 76 | } |
| nucho | 0:77afd7560544 | 77 | }; |
| nucho | 0:77afd7560544 | 78 | } |
| nucho | 0:77afd7560544 | 79 | #endif /* NODEOUTPUT_H_ */ |