modify for Hydro version

Dependencies:   MODSERIAL

Fork of rosserial_mbed_lib by nucho

Committer:
nucho
Date:
Sun Oct 16 07:19:36 2011 +0000
Revision:
1:ff0ec969dad1
Parent:
0:77afd7560544
Child:
3:1cf99502f396
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 1:ff0ec969dad1 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_SUBSCRIBER_H_
nucho 1:ff0ec969dad1 36 #define ROS_SUBSCRIBER_H_
nucho 0:77afd7560544 37
nucho 0:77afd7560544 38 #include "rosserial_ids.h"
nucho 0:77afd7560544 39 #include "msg_receiver.h"
nucho 1:ff0ec969dad1 40
nucho 1:ff0ec969dad1 41 namespace ros {
nucho 0:77afd7560544 42
nucho 1:ff0ec969dad1 43 /* ROS Subscriber
nucho 1:ff0ec969dad1 44 * This class handles holding the msg so that
nucho 1:ff0ec969dad1 45 * it is not continously reallocated. It is also used by the
nucho 1:ff0ec969dad1 46 * node handle to keep track of callback functions and IDs.
nucho 1:ff0ec969dad1 47 */
nucho 1:ff0ec969dad1 48 template<typename MsgT>
nucho 1:ff0ec969dad1 49 class Subscriber: public MsgReceiver{
nucho 1:ff0ec969dad1 50 public:
nucho 0:77afd7560544 51 typedef void(*CallbackT)(const MsgT&);
nucho 1:ff0ec969dad1 52 MsgT msg;
nucho 0:77afd7560544 53
nucho 0:77afd7560544 54 Subscriber(const char * topic_name, CallbackT msgCB){
nucho 1:ff0ec969dad1 55 topic_ = topic_name;
nucho 1:ff0ec969dad1 56 cb_= msgCB;
nucho 1:ff0ec969dad1 57 }
nucho 1:ff0ec969dad1 58
nucho 1:ff0ec969dad1 59 virtual void receive(unsigned char* data){
nucho 1:ff0ec969dad1 60 msg.deserialize(data);
nucho 1:ff0ec969dad1 61 this->cb_(msg);
nucho 0:77afd7560544 62 }
nucho 1:ff0ec969dad1 63
nucho 1:ff0ec969dad1 64 virtual const char * getMsgType(){return this->msg.getType();}
nucho 1:ff0ec969dad1 65 virtual int _getType(){return TOPIC_SUBSCRIBERS;}
nucho 1:ff0ec969dad1 66
nucho 1:ff0ec969dad1 67 private:
nucho 1:ff0ec969dad1 68 CallbackT cb_;
nucho 1:ff0ec969dad1 69 };
nucho 0:77afd7560544 70
nucho 0:77afd7560544 71 }
nucho 1:ff0ec969dad1 72
nucho 1:ff0ec969dad1 73 #endif