Colony » Colony Scout

all: scout_avr.hex

	rm -f scout_avr.elf scout_avr.hex

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

#include <math.h>

#include "ros/duration.h"

namespace ros

{

  void normalizeSecNSecSigned(long &sec, long &nsec)

  {

    long nsec_part = nsec;

    long sec_part = sec;

    while (nsec_part > 1000000000L)

    {

      nsec_part -= 1000000000L;

      ++sec_part;

    }

    while (nsec_part < 0)

    {

      nsec_part += 1000000000L;

      --sec_part;

    }

    sec = sec_part;

    nsec = nsec_part;

  }

  Duration& Duration::operator+=(const Duration &rhs)

  {

    sec += rhs.sec;

    nsec += rhs.nsec;

    normalizeSecNSecSigned(sec, nsec);

    return *this;

  }

  Duration& Duration::operator-=(const Duration &rhs){

    sec += -rhs.sec;

    nsec += -rhs.nsec;

    normalizeSecNSecSigned(sec, nsec);

    return *this;

  }

  Duration& Duration::operator*=(double scale){

    sec *= scale;

    nsec *= scale;

    normalizeSecNSecSigned(sec, nsec);

    return *this;

  }

}

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

#ifndef _ROS_DURATION_H_

#define _ROS_DURATION_H_

namespace ros {

  void normalizeSecNSecSigned(long& sec, long& nsec);

  class Duration

  {

    public:

      long sec, nsec; 

      Duration() : sec(0), nsec(0) {}

      Duration(long _sec, long _nsec) : sec(_sec), nsec(_nsec)

      {

        normalizeSecNSecSigned(sec, nsec);

      }

      Duration& operator+=(const Duration &rhs);

      Duration& operator-=(const Duration &rhs);

      Duration& operator*=(double scale);

  };

}

#endif

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

#ifndef _ROS_MSG_H_

#define _ROS_MSG_H_

namespace ros {

  /* Base Message Type */

  class Msg

  {

    public:

      virtual int serialize(unsigned char *outbuffer) const = 0;

	  virtual int deserialize(unsigned char *data) = 0;

      virtual const char * getType() = 0;

      virtual const char * getMD5() = 0;

  };

}

#endif

/*

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

#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"

#define SYNC_SECONDS        5

#define MODE_FIRST_FF       0

#define MODE_SECOND_FF      1

#define MODE_TOPIC_L        2   // waiting for topic id

#define MODE_TOPIC_H        3

#define MODE_SIZE_L         4   // waiting for message size

#define MODE_SIZE_H         5

#define MODE_MESSAGE        6

#define MODE_CHECKSUM       7

#define MSG_TIMEOUT 20  //20 milliseconds to recieve all of message data

#include "msg.h"

namespace ros {

  class NodeHandleBase_{

    public:

      virtual int publish(int id, const Msg* msg)=0;

      virtual int spinOnce()=0;

      virtual bool connected()=0;

    };

}

#include "publisher.h"

#include "subscriber.h"

#include "service_server.h"

#include "service_client.h"

namespace ros {

  using rosserial_msgs::TopicInfo;

  /* Node Handle */

  template<class Hardware,

           int MAX_SUBSCRIBERS=25,

           int MAX_PUBLISHERS=25,

           int INPUT_SIZE=512,

           int OUTPUT_SIZE=512>

  class NodeHandle_ : public NodeHandleBase_

  {

    protected:

      Hardware hardware_;

      /* time used for syncing */

      unsigned long rt_time;

      /* used for computing current time */

      unsigned long sec_offset, nsec_offset;

      unsigned char message_in[INPUT_SIZE];

      unsigned char message_out[OUTPUT_SIZE];

      Publisher * publishers[MAX_PUBLISHERS];

      Subscriber_ * subscribers[MAX_SUBSCRIBERS];

      /*

       * Setup Functions

       */

    public:

      NodeHandle_() : configured_(false) {}

      Hardware* getHardware(){

        return &hardware_;

      }

      /* Start serial, initialize buffers */

      void initNode(){

        hardware_.init();

        mode_ = 0;

        bytes_ = 0;

        index_ = 0;

        topic_ = 0;

      };

    protected:

      //State machine variables for spinOnce

      int mode_;

      int bytes_;

      int topic_;

      int index_;

      int checksum_;

      bool configured_;

      /* used for syncing the time */

      unsigned long last_sync_time;

      unsigned long last_sync_receive_time;

      unsigned long last_msg_timeout_time;

    public:

      /* This function goes in your loop() function, it handles

       *  serial input and callbacks for subscribers.

       */

      virtual int spinOnce(){

        /* restart if timed out */

        unsigned long c_time = hardware_.time();

        if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){

            configured_ = false;

         }

        /* reset if message has timed out */

        if ( mode_ != MODE_FIRST_FF){ 

          if (c_time > last_msg_timeout_time){

            mode_ = MODE_FIRST_FF;

          }

        }

        /* while available buffer, read data */

        while( true )

        {

          int data = hardware_.read();

          if( data < 0 )

            break;

          checksum_ += data;

          if( mode_ == MODE_MESSAGE ){        /* message data being recieved */

            message_in[index_++] = data;

            bytes_--;

            if(bytes_ == 0)                   /* is message complete? if so, checksum */

              mode_ = MODE_CHECKSUM;

          }else if( mode_ == MODE_FIRST_FF ){

            if(data == 0xff){

              mode_++;

              last_msg_timeout_time = c_time + MSG_TIMEOUT;

            }

          }else if( mode_ == MODE_SECOND_FF ){

            if(data == 0xff){

              mode_++;

            }else{

              mode_ = MODE_FIRST_FF;

            }

          }else if( mode_ == MODE_TOPIC_L ){  /* bottom half of topic id */

            topic_ = data;

            mode_++;

            checksum_ = data;                 /* first byte included in checksum */

          }else if( mode_ == MODE_TOPIC_H ){  /* top half of topic id */

            topic_ += data<<8;

            mode_++;

          }else if( mode_ == MODE_SIZE_L ){   /* bottom half of message size */

            bytes_ = data;

            index_ = 0;

            mode_++;

          }else if( mode_ == MODE_SIZE_H ){   /* top half of message size */

            bytes_ += data<<8;

            mode_ = MODE_MESSAGE;

            if(bytes_ == 0)

              mode_ = MODE_CHECKSUM;

          }else if( mode_ == MODE_CHECKSUM ){ /* do checksum */

            mode_ = MODE_FIRST_FF;

            if( (checksum_%256) == 255){

              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(subscribers[topic_-100])

                  subscribers[topic_-100]->callback( message_in );

              }

            }

          }

        }

        /* occasionally sync time */

        if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){

          requestSyncTime();

          last_sync_time = c_time;

        }

        return 0;

      }

      /* Are we connected to the PC? */

      virtual bool connected() {

        return configured_;

      };

      /********************************************************************

       * Time functions

       */

      void requestSyncTime()

      {

        std_msgs::Time t;

        publish(TopicInfo::ID_TIME, &t);

        rt_time = hardware_.time();

      }

      void syncTime( unsigned char * data )

      {

        std_msgs::Time t;

        unsigned long offset = hardware_.time() - rt_time;

        t.deserialize(data);

        t.data.sec += offset/1000;

        t.data.nsec += (offset%1000)*1000000UL;

        this->setNow(t.data);

        last_sync_receive_time = hardware_.time();

      }

      Time now(){

        unsigned long ms = hardware_.time();

        Time current_time;

        current_time.sec = ms/1000 + sec_offset;

        current_time.nsec = (ms%1000)*1000000UL + nsec_offset;

        normalizeSecNSec(current_time.sec, current_time.nsec);

        return current_time;

      }

      void setNow( Time & new_now )

      {

        unsigned long ms = hardware_.time();

        sec_offset = new_now.sec - ms/1000 - 1;

        nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;

        normalizeSecNSec(sec_offset, nsec_offset);

      }

      /********************************************************************

       * Topic Management 

       */

      /* Register a new publisher */    

      bool advertise(Publisher & p)

      {

        for(int i = 0; i < MAX_PUBLISHERS; i++){

          if(publishers[i] == 0){ // empty slot

            publishers[i] = &p;

            p.id_ = i+100+MAX_SUBSCRIBERS;

            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 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;

      }

      /* 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()

      {

        configured_ = true;

        rosserial_msgs::TopicInfo ti;

        int i;

        for(i = 0; i < MAX_PUBLISHERS; i++)

        {

          if(publishers[i] != 0) // non-empty slot

          {

            ti.topic_id = publishers[i]->id_;

            ti.topic_name = (char *) publishers[i]->topic_;

            ti.message_type = (char *) publishers[i]->msg_->getType();

            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(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(int id, const Msg * msg)

      {

        if(!configured_) return 0;

        /* serialize message */

        int 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 0;

        }

      }

      /********************************************************************

       * Logging

       */

    private:

      void log(char byte, const char * msg){

        rosserial_msgs::Log l;

        l.level= byte;

        l.msg = (char*)msg;

        publish(rosserial_msgs::TopicInfo::ID_LOG, &l);

      }

    public:

      void logdebug(const char* msg){

        log(rosserial_msgs::Log::DEBUG, msg);

      }

      void loginfo(const char * msg){

        log(rosserial_msgs::Log::INFO, msg);

      }

      void logwarn(const char *msg){

        log(rosserial_msgs::Log::WARN, msg);

      }

      void logerror(const char*msg){

        log(rosserial_msgs::Log::ERROR, msg);

      }

      void logfatal(const char*msg){

        log(rosserial_msgs::Log::FATAL, msg);

      }

      /********************************************************************

       * Parameters

       */

    private:

      bool param_recieved;

      rosserial_msgs::RequestParamResponse req_param_resp;

      bool requestParam(const char * name, int time_out =  1000){

        param_recieved = false;

        rosserial_msgs::RequestParamRequest req;

        req.name  = (char*)name;

        publish(TopicInfo::ID_PARAMETER_REQUEST, &req);

        int end_time = hardware_.time();

        while(!param_recieved ){

          spinOnce();

          if (end_time > hardware_.time()) return false;

        }

        return true;

      }

    public:

      bool getParam(const char* name, int* param, int length =1){

        if (requestParam(name) ){

          if (length == req_param_resp.ints_length){

            //copy it over

            for(int i=0; i<length; i++)

              param[i] = req_param_resp.ints[i];

            return true;

          }

        }

        return false;

      }

      bool getParam(const char* name, float* param, int length=1){

        if (requestParam(name) ){

          if (length == req_param_resp.floats_length){

            //copy it over

            for(int i=0; i<length; i++) 

              param[i] = req_param_resp.floats[i];

            return true;

          }

        }

        return false;

      }

      bool getParam(const char* name, char** param, int length=1){

        if (requestParam(name) ){

          if (length == req_param_resp.strings_length){

            //copy it over

            for(int i=0; i<length; i++)

              strcpy(param[i],req_param_resp.strings[i]);

            return true;

          }

        }

        return false;

      }  

  };

}

#endif

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

#ifndef _ROS_PUBLISHER_H_

#define _ROS_PUBLISHER_H_

#include "rosserial_msgs/TopicInfo.h"

#include "node_handle.h"

namespace ros {

  /* Generic Publisher */

  class Publisher

  {

    public:

      Publisher( const char * topic_name, Msg * msg, int endpoint=rosserial_msgs::TopicInfo::ID_PUBLISHER) :

        topic_(topic_name), 

        msg_(msg),

        endpoint_(endpoint) {};

      int publish( const Msg * msg ) { return nh_->publish(id_, msg); };

      int getEndpointType(){ return endpoint_; }

      const char * topic_;

      Msg *msg_;

      // id_ and no_ are set by NodeHandle when we advertise 

      int id_;

      NodeHandleBase_* nh_;

    private:

      int endpoint_;

  };

}

#endif

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

#ifndef _ROS_SERVICE_CLIENT_H_

#define _ROS_SERVICE_CLIENT_H_

#include "rosserial_msgs/TopicInfo.h"

#include "publisher.h"

#include "subscriber.h"

namespace ros {

  template<typename MReq , typename MRes>

  class ServiceClient : public Subscriber_  {

    public:

      ServiceClient(const char* topic_name) : 

        pub(topic_name, &req, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER)

      {

        this->topic_ = topic_name;

        this->waiting = true;

      }

      virtual void call(const MReq & request, MRes & response)

      {

        if(!pub.nh_->connected()) return;

        ret = &response;

        waiting = true;

        pub.publish(&request);

        while(waiting && pub.nh_->connected())

          if(pub.nh_->spinOnce() < 0) break;

      }

      // these refer to the subscriber

      virtual void callback(unsigned char *data){

        ret->deserialize(data);

        waiting = false;

      }

      virtual const char * getMsgType(){ return this->resp.getType(); }

      virtual const char * getMsgMD5(){ return this->resp.getMD5(); }

      virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; }

      MReq req;

      MRes resp;

      MRes * ret;

      bool waiting;

      Publisher pub;

  };

}

#endif

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

#ifndef _ROS_SERVICE_SERVER_H_

#define _ROS_SERVICE_SERVER_H_

#include "rosserial_msgs/TopicInfo.h"

#include "publisher.h"

#include "subscriber.h"

namespace ros {

  template<typename MReq , typename MRes>

  class ServiceServer : public Subscriber_ {

    public:

      typedef void(*CallbackT)(const MReq&,  MRes&);

      ServiceServer(const char* topic_name, CallbackT cb) :

        pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER)

      {

        this->topic_ = topic_name;

        this->cb_ = cb;

      }

      // these refer to the subscriber

      virtual void callback(unsigned char *data){

        req.deserialize(data);

        cb_(req,resp);

        pub.publish(&resp);

      }

      virtual const char * getMsgType(){ return this->req.getType(); }

      virtual const char * getMsgMD5(){ return this->req.getMD5(); }

      virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; }

      MReq req;

      MRes resp;

      Publisher pub;

    private:

      CallbackT cb_;

  };

}

#endif

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

#ifndef ROS_SUBSCRIBER_H_

#define ROS_SUBSCRIBER_H_

#include "rosserial_msgs/TopicInfo.h"

namespace ros {

  /* Base class for objects subscribers. */

  class Subscriber_

  {

    public:

      virtual void callback(unsigned char *data)=0;

      virtual int getEndpointType()=0;

      // id_ is set by NodeHandle when we advertise 

      int id_;

      virtual const char * getMsgType()=0;

      virtual const char * getMsgMD5()=0;

      const char * topic_;

  };

  /* Actual subscriber, templated on message type. */

  template<typename MsgT>

  class Subscriber: public Subscriber_{

    public:

      typedef void(*CallbackT)(const MsgT&);

      MsgT msg;

      Subscriber(const char * topic_name, CallbackT cb, int endpoint=rosserial_msgs::TopicInfo::ID_SUBSCRIBER) :

        cb_(cb),

        endpoint_(endpoint)

      {

        topic_ = topic_name;

      };

      virtual void callback(unsigned char* data){

        msg.deserialize(data);

        this->cb_(msg);

      }

      virtual const char * getMsgType(){ return this->msg.getType(); }

      virtual const char * getMsgMD5(){ return this->msg.getMD5(); }

      virtual int getEndpointType(){ return endpoint_; }

    private:

      CallbackT cb_;

      int endpoint_;

  };

}

#endif

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

#ifndef ROS_TIME_H_

#define ROS_TIME_H_

#include <ros/duration.h>

#include <math.h>

namespace ros

{

  void normalizeSecNSec(unsigned long &sec, unsigned long &nsec);

  class Time

  {

    public:

      unsigned long sec, nsec;

      Time() : sec(0), nsec(0) {}

      Time(unsigned long _sec, unsigned long _nsec) : sec(_sec), nsec(_nsec)

      {

        normalizeSecNSec(sec, nsec);  

      } 

      double toSec() const { return (double)sec + 1e-9*(double)nsec; };

      void fromSec(double t) { sec = (unsigned long) floor(t); nsec = (unsigned long) round((t-sec) * 1e9); };

      unsigned long toNsec() { return (unsigned long)sec*1000000000ull + (unsigned long)nsec; };

      Time& fromNSec(long t);

      Time& operator +=(const Duration &rhs);

      Time& operator -=(const Duration &rhs);

      static Time now();

      static void setNow( Time & new_now);

  };

}

#endif

#ifndef _ROS_rosserial_msgs_Log_h

#define _ROS_rosserial_msgs_Log_h

#include <stdint.h>

#include <string.h>

#include <stdlib.h>

#include "ros/msg.h"

namespace rosserial_msgs

{

  class Log : public ros::Msg

  {

    public:

      uint8_t level;

      char * msg;

      enum { DEBUG = 0 };

      enum { INFO = 1 };

      enum { WARN = 2 };

      enum { ERROR = 3 };

      enum { FATAL = 4 };

    virtual int serialize(unsigned char *outbuffer) const

    {

      int offset = 0;

      *(outbuffer + offset + 0) = (this->level >> (8 * 0)) & 0xFF;

      offset += sizeof(this->level);

      uint32_t * length_msg = (uint32_t *)(outbuffer + offset);

      *length_msg = strlen( (const char*) this->msg);

      offset += 4;

      memcpy(outbuffer + offset, this->msg, *length_msg);

      offset += *length_msg;

      return offset;

    }

    virtual int deserialize(unsigned char *inbuffer)

    {

      int offset = 0;

      this->level |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);

      offset += sizeof(this->level);

      uint32_t length_msg = *(uint32_t *)(inbuffer + offset);

      offset += 4;

      for(unsigned int k= offset; k< offset+length_msg; ++k){

          inbuffer[k-1]=inbuffer[k];

      }

      inbuffer[offset+length_msg-1]=0;

      this->msg = (char *)(inbuffer + offset-1);

      offset += length_msg;

     return offset;

    }

    const char * getType(){ return "rosserial_msgs/Log"; };

    const char * getMD5(){ return "7170d5aec999754ba0d9f762bf49b913"; };

  };

}

#endif

#ifndef _ROS_SERVICE_RequestParam_h

#define _ROS_SERVICE_RequestParam_h

#include <stdint.h>

#include <string.h>

#include <stdlib.h>

#include "ros/msg.h"

namespace rosserial_msgs

{

static const char REQUESTPARAM[] = "rosserial_msgs/RequestParam";

  class RequestParamRequest : public ros::Msg

  {

    public:

      char * name;

    virtual int serialize(unsigned char *outbuffer) const

    {

      int offset = 0;

      uint32_t * length_name = (uint32_t *)(outbuffer + offset);

      *length_name = strlen( (const char*) this->name);

      offset += 4;

      memcpy(outbuffer + offset, this->name, *length_name);

      offset += *length_name;

      return offset;

    }

    virtual int deserialize(unsigned char *inbuffer)

    {

      int offset = 0;

      uint32_t length_name = *(uint32_t *)(inbuffer + offset);

      offset += 4;

      for(unsigned int k= offset; k< offset+length_name; ++k){

          inbuffer[k-1]=inbuffer[k];

      }

      inbuffer[offset+length_name-1]=0;

      this->name = (char *)(inbuffer + offset-1);

      offset += length_name;

     return offset;

    }

    const char * getType(){ return REQUESTPARAM; };

    const char * getMD5(){ return "c1f3d28f1b044c871e6eff2e9fc3c667"; };

  };

  class RequestParamResponse : public ros::Msg

  {

    public:

      uint8_t ints_length;

      int32_t st_ints;

      int32_t * ints;

      uint8_t floats_length;

      float st_floats;

      float * floats;

      uint8_t strings_length;

      char* st_strings;

      char* * strings;

    virtual int serialize(unsigned char *outbuffer) const

    {

      int offset = 0;

      *(outbuffer + offset++) = ints_length;

      *(outbuffer + offset++) = 0;

      *(outbuffer + offset++) = 0;

      *(outbuffer + offset++) = 0;

      for( uint8_t i = 0; i < ints_length; i++){

      union {

        int32_t real;

        uint32_t base;

      } u_intsi;

      u_intsi.real = this->ints[i];

      *(outbuffer + offset + 0) = (u_intsi.base >> (8 * 0)) & 0xFF;

      *(outbuffer + offset + 1) = (u_intsi.base >> (8 * 1)) & 0xFF;

      *(outbuffer + offset + 2) = (u_intsi.base >> (8 * 2)) & 0xFF;

      *(outbuffer + offset + 3) = (u_intsi.base >> (8 * 3)) & 0xFF;

      offset += sizeof(this->ints[i]);

      }

      *(outbuffer + offset++) = floats_length;

      *(outbuffer + offset++) = 0;

      *(outbuffer + offset++) = 0;

      *(outbuffer + offset++) = 0;

      for( uint8_t i = 0; i < floats_length; i++){

      union {

        float real;

        uint32_t base;

      } u_floatsi;

      u_floatsi.real = this->floats[i];

      *(outbuffer + offset + 0) = (u_floatsi.base >> (8 * 0)) & 0xFF;

      *(outbuffer + offset + 1) = (u_floatsi.base >> (8 * 1)) & 0xFF;

      *(outbuffer + offset + 2) = (u_floatsi.base >> (8 * 2)) & 0xFF;

      *(outbuffer + offset + 3) = (u_floatsi.base >> (8 * 3)) & 0xFF;

      offset += sizeof(this->floats[i]);

      }

      *(outbuffer + offset++) = strings_length;

      *(outbuffer + offset++) = 0;

      *(outbuffer + offset++) = 0;

      *(outbuffer + offset++) = 0;

      for( uint8_t i = 0; i < strings_length; i++){

      uint32_t * length_stringsi = (uint32_t *)(outbuffer + offset);

      *length_stringsi = strlen( (const char*) this->strings[i]);

      offset += 4;

      memcpy(outbuffer + offset, this->strings[i], *length_stringsi);

      offset += *length_stringsi;

      }

      return offset;

    }

    virtual int deserialize(unsigned char *inbuffer)

    {

      int offset = 0;

      uint8_t ints_lengthT = *(inbuffer + offset++);

      if(ints_lengthT > ints_length)

        this->ints = (int32_t*)realloc(this->ints, ints_lengthT * sizeof(int32_t));

      offset += 3;

      ints_length = ints_lengthT;

      for( uint8_t i = 0; i < ints_length; i++){

      union {

        int32_t real;

        uint32_t base;

      } u_st_ints;

      u_st_ints.base = 0;

      u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);

      u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);

      u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);

      u_st_ints.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);

      this->st_ints = u_st_ints.real;

      offset += sizeof(this->st_ints);

        memcpy( &(this->ints[i]), &(this->st_ints), sizeof(int32_t));

      }

      uint8_t floats_lengthT = *(inbuffer + offset++);

      if(floats_lengthT > floats_length)

        this->floats = (float*)realloc(this->floats, floats_lengthT * sizeof(float));

      offset += 3;

      floats_length = floats_lengthT;

      for( uint8_t i = 0; i < floats_length; i++){

      union {

        float real;

        uint32_t base;

      } u_st_floats;

      u_st_floats.base = 0;

      u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);

      u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);

      u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);

      u_st_floats.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);

      this->st_floats = u_st_floats.real;

      offset += sizeof(this->st_floats);

        memcpy( &(this->floats[i]), &(this->st_floats), sizeof(float));

      }

      uint8_t strings_lengthT = *(inbuffer + offset++);

      if(strings_lengthT > strings_length)

        this->strings = (char**)realloc(this->strings, strings_lengthT * sizeof(char*));

      offset += 3;

      strings_length = strings_lengthT;

      for( uint8_t i = 0; i < strings_length; i++){

      uint32_t length_st_strings = *(uint32_t *)(inbuffer + offset);

      offset += 4;

      for(unsigned int k= offset; k< offset+length_st_strings; ++k){

          inbuffer[k-1]=inbuffer[k];

      }

      inbuffer[offset+length_st_strings-1]=0;

      this->st_strings = (char *)(inbuffer + offset-1);

      offset += length_st_strings;

        memcpy( &(this->strings[i]), &(this->st_strings), sizeof(char*));

      }

     return offset;

    }

    const char * getType(){ return REQUESTPARAM; };

    const char * getMD5(){ return "9f0e98bda65981986ddf53afa7a40e49"; };

  };

  class RequestParam {

    public:

    typedef RequestParamRequest Request;

    typedef RequestParamResponse Response;

  };

}

#endif

#ifndef _ROS_rosserial_msgs_TopicInfo_h

#define _ROS_rosserial_msgs_TopicInfo_h

#include <stdint.h>

#include <string.h>

#include <stdlib.h>

#include "ros/msg.h"

namespace rosserial_msgs

{

  class TopicInfo : public ros::Msg

  {

    public:

      uint16_t topic_id;

      char * topic_name;

      char * message_type;

      char * md5sum;

      int32_t buffer_size;

      enum { ID_PUBLISHER = 0 };

      enum { ID_SUBSCRIBER = 1 };

      enum { ID_SERVICE_SERVER = 2 };

      enum { ID_SERVICE_CLIENT = 4 };

      enum { ID_PARAMETER_REQUEST = 6 };

      enum { ID_LOG = 7 };

      enum { ID_TIME = 10 };

    virtual int serialize(unsigned char *outbuffer) const

    {

      int offset = 0;

      *(outbuffer + offset + 0) = (this->topic_id >> (8 * 0)) & 0xFF;

      *(outbuffer + offset + 1) = (this->topic_id >> (8 * 1)) & 0xFF;

      offset += sizeof(this->topic_id);

      uint32_t * length_topic_name = (uint32_t *)(outbuffer + offset);

      *length_topic_name = strlen( (const char*) this->topic_name);

      offset += 4;

      memcpy(outbuffer + offset, this->topic_name, *length_topic_name);

      offset += *length_topic_name;

      uint32_t * length_message_type = (uint32_t *)(outbuffer + offset);

      *length_message_type = strlen( (const char*) this->message_type);

      offset += 4;

      memcpy(outbuffer + offset, this->message_type, *length_message_type);

      offset += *length_message_type;

      uint32_t * length_md5sum = (uint32_t *)(outbuffer + offset);

      *length_md5sum = strlen( (const char*) this->md5sum);

      offset += 4;

      memcpy(outbuffer + offset, this->md5sum, *length_md5sum);

      offset += *length_md5sum;

      union {

        int32_t real;

        uint32_t base;

      } u_buffer_size;

      u_buffer_size.real = this->buffer_size;

      *(outbuffer + offset + 0) = (u_buffer_size.base >> (8 * 0)) & 0xFF;

      *(outbuffer + offset + 1) = (u_buffer_size.base >> (8 * 1)) & 0xFF;

      *(outbuffer + offset + 2) = (u_buffer_size.base >> (8 * 2)) & 0xFF;

      *(outbuffer + offset + 3) = (u_buffer_size.base >> (8 * 3)) & 0xFF;

      offset += sizeof(this->buffer_size);

      return offset;

    }

    virtual int deserialize(unsigned char *inbuffer)

    {

      int offset = 0;

      this->topic_id |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);

      this->topic_id |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);

      offset += sizeof(this->topic_id);

      uint32_t length_topic_name = *(uint32_t *)(inbuffer + offset);

      offset += 4;

      for(unsigned int k= offset; k< offset+length_topic_name; ++k){

          inbuffer[k-1]=inbuffer[k];

      }

      inbuffer[offset+length_topic_name-1]=0;

      this->topic_name = (char *)(inbuffer + offset-1);

      offset += length_topic_name;

      uint32_t length_message_type = *(uint32_t *)(inbuffer + offset);

      offset += 4;

      for(unsigned int k= offset; k< offset+length_message_type; ++k){

          inbuffer[k-1]=inbuffer[k];

      }

      inbuffer[offset+length_message_type-1]=0;

      this->message_type = (char *)(inbuffer + offset-1);

      offset += length_message_type;

      uint32_t length_md5sum = *(uint32_t *)(inbuffer + offset);

      offset += 4;

      for(unsigned int k= offset; k< offset+length_md5sum; ++k){

          inbuffer[k-1]=inbuffer[k];

      }