nucho / rosserial_mbed_lib

This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial.

Dependencies:  

Dependents:   rosserial_mbed robot_S2

ros/node_output.h@1:ff0ec969dad1, 2011-10-16 (annotated)

Committer:
nucho
Date:
Sun Oct 16 07:19:36 2011 +0000
Revision:
1:ff0ec969dad1
Parent:
0:77afd7560544 
This program supported the revision of 143 of rosserial.

And the bug fix of receive of array data.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nucho 0:77afd7560544 1 /*
nucho 1:ff0ec969dad1 2 * Software License Agreement (BSD License)
nucho 1:ff0ec969dad1 3 *
nucho 1:ff0ec969dad1 4 * Copyright (c) 2011, Willow Garage, Inc.
nucho 1:ff0ec969dad1 5 * All rights reserved.
nucho 1:ff0ec969dad1 6 *
nucho 1:ff0ec969dad1 7 * Redistribution and use in source and binary forms, with or without
nucho 1:ff0ec969dad1 8 * modification, are permitted provided that the following conditions
nucho 1:ff0ec969dad1 9 * are met:
nucho 0:77afd7560544 10 *
nucho 1:ff0ec969dad1 11 * * Redistributions of source code must retain the above copyright
nucho 1:ff0ec969dad1 12 * notice, this list of conditions and the following disclaimer.
nucho 1:ff0ec969dad1 13 * * Redistributions in binary form must reproduce the above
nucho 1:ff0ec969dad1 14 * copyright notice, this list of conditions and the following
nucho 1:ff0ec969dad1 15 * disclaimer in the documentation and/or other materials provided
nucho 1:ff0ec969dad1 16 * with the distribution.
nucho 1:ff0ec969dad1 17 * * Neither the name of Willow Garage, Inc. nor the names of its
nucho 1:ff0ec969dad1 18 * contributors may be used to endorse or promote prducts derived
nucho 1:ff0ec969dad1 19 * from this software without specific prior written permission.
nucho 1:ff0ec969dad1 20 *
nucho 1:ff0ec969dad1 21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
nucho 1:ff0ec969dad1 22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
nucho 1:ff0ec969dad1 23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
nucho 1:ff0ec969dad1 24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
nucho 1:ff0ec969dad1 25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
nucho 1:ff0ec969dad1 26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
nucho 1:ff0ec969dad1 27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
nucho 1:ff0ec969dad1 28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
nucho 1:ff0ec969dad1 29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
nucho 1:ff0ec969dad1 30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
nucho 1:ff0ec969dad1 31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
nucho 1:ff0ec969dad1 32 * POSSIBILITY OF SUCH DAMAGE.
nucho 0:77afd7560544 33 */
nucho 0:77afd7560544 34
nucho 1:ff0ec969dad1 35 #ifndef ROS_NODEOUTPUT_H_
nucho 1:ff0ec969dad1 36 #define ROS_NODEOUTPUT_H_
nucho 1:ff0ec969dad1 37
nucho 1:ff0ec969dad1 38 #include "msg.h"
nucho 1:ff0ec969dad1 39
nucho 1:ff0ec969dad1 40 #include "mbed.h"
nucho 1:ff0ec969dad1 41 //static Serial debug(p9,p10);
nucho 1:ff0ec969dad1 42
nucho 1:ff0ec969dad1 43 namespace ros {
nucho 0:77afd7560544 44
nucho 0:77afd7560544 45 /*
nucho 0:77afd7560544 46 * This class is responsible for controlling the node ouput.
nucho 0:77afd7560544 47 * It it is the object that is passed to Publishers and services
nucho 0:77afd7560544 48 */
nucho 0:77afd7560544 49 class NodeOutput_ {
nucho 0:77afd7560544 50 public:
nucho 0:77afd7560544 51 virtual int publish(short id, Msg* msg)=0;
nucho 0:77afd7560544 52 };
nucho 0:77afd7560544 53
nucho 0:77afd7560544 54 template<class Hardware, int OUTSIZE =512>
nucho 0:77afd7560544 55 class NodeOutput : public NodeOutput_ {
nucho 0:77afd7560544 56
nucho 0:77afd7560544 57 private:
nucho 0:77afd7560544 58 Hardware* hardware_;
nucho 0:77afd7560544 59 bool configured_;
nucho 0:77afd7560544 60 unsigned char message_out[OUTSIZE];
nucho 0:77afd7560544 61
nucho 0:77afd7560544 62 public:
nucho 0:77afd7560544 63 NodeOutput(Hardware* h) {
nucho 0:77afd7560544 64 hardware_ = h;
nucho 0:77afd7560544 65 configured_ = false;
nucho 0:77afd7560544 66 }
nucho 0:77afd7560544 67
nucho 0:77afd7560544 68 NodeOutput() {};
nucho 1:ff0ec969dad1 69
nucho 0:77afd7560544 70 void setHardware(Hardware* h) {
nucho 0:77afd7560544 71 hardware_ = h;
nucho 0:77afd7560544 72 configured_=false;
nucho 0:77afd7560544 73 }
nucho 0:77afd7560544 74
nucho 0:77afd7560544 75 void setConfigured(bool b) {
nucho 0:77afd7560544 76 configured_ =b;
nucho 0:77afd7560544 77 }
nucho 0:77afd7560544 78 bool configured() {
nucho 0:77afd7560544 79 return configured_;
nucho 0:77afd7560544 80 };
nucho 0:77afd7560544 81
nucho 1:ff0ec969dad1 82 virtual int publish(short id, Msg * msg) {
nucho 1:ff0ec969dad1 83 wait_ms(1);
nucho 1:ff0ec969dad1 84 if (!configured_)return 0;
nucho 0:77afd7560544 85
nucho 0:77afd7560544 86 /* serialize message */
nucho 0:77afd7560544 87 short l = msg->serialize(message_out+6);
nucho 0:77afd7560544 88
nucho 0:77afd7560544 89 /* setup the header */
nucho 0:77afd7560544 90 message_out[0] = 0xff;
nucho 0:77afd7560544 91 message_out[1] = 0xff;
nucho 0:77afd7560544 92 message_out[2] = (unsigned char) id&255;
nucho 0:77afd7560544 93 message_out[3] = (unsigned char) id>>8;
nucho 0:77afd7560544 94 message_out[4] = (unsigned char) l&255;
nucho 0:77afd7560544 95 message_out[5] = ((unsigned char) l>>8);
nucho 0:77afd7560544 96
nucho 0:77afd7560544 97 /* calculate checksum */
nucho 0:77afd7560544 98 short chk = 0;
nucho 0:77afd7560544 99 for (int i =2; i<l+6; i++)
nucho 0:77afd7560544 100 chk += message_out[i];
nucho 1:ff0ec969dad1 101 l += 6;
nucho 1:ff0ec969dad1 102 message_out[l++] = 255 - (chk%256);
nucho 0:77afd7560544 103
nucho 1:ff0ec969dad1 104 hardware_->write(message_out, l);
nucho 1:ff0ec969dad1 105 return l;
nucho 0:77afd7560544 106 }
nucho 0:77afd7560544 107 };
nucho 1:ff0ec969dad1 108
nucho 0:77afd7560544 109 }
nucho 1:ff0ec969dad1 110
nucho 1:ff0ec969dad1 111 #endif