Colony » Colony Scout » Colony Scout OS

MCU=atmega128rfa1

F_CPU=16000000

SRC=src/*.cpp src/ros_lib/*.cpp

FLAGS=-Isrc/ros_lib -Isrc -mmcu=$(MCU) -DF_CPU=$(F_CPU)UL -funsigned-char -Os -fpack-struct -Wall

all: scout_avr.hex

%.hex: %.elf

	avr-objcopy -j .text -j .data -O ihex $< $@

scout_avr.elf: $(SRC)

	avr-g++ $(FLAGS) $(SRC) -o scout_avr.elf

clean:

	rm -f scout_avr.elf scout_avr.hex

/**

\mainpage

\htmlinclude manifest.html

\b scout_avr is ... 

<!-- 

Provide an overview of your package.

-->

\section codeapi Code API

<!--

Provide links to specific auto-generated API documentation within your

package that is of particular interest to a reader. Doxygen will

document pretty much every part of your code, so do your best here to

point the reader to the actual API.

If your codebase is fairly large or has different sets of APIs, you

should use the doxygen 'group' tag to keep these APIs together. For

example, the roscpp documentation has 'libros' group.

-->

*/

<package>

  <description brief="scout_avr">

    scout_avr contains all of the code which will go on the avr, including

    the rosserial_client implementation

  </description>

  <author>Tom Mullins</author>

  <author>CMU Robotics Club</author>

  <depend package="rosserial_client" />

</package>

#include "Atmega128rfa1.h"

extern "C"

{

#include <avr/io.h>

#include <avr/interrupt.h>

}

unsigned long millis;

Atmega128rfa1::Atmega128rfa1()

{

}

ISR(TIMER0_COMPA_vect)

{

  millis++;

}

void Atmega128rfa1::init()

{

  // === init serial ===

  // for 16 MHz clock, 76800 baud

  uint16_t baud = 12;

  UBRR0H = baud >> 8;

  UBRR0L = baud;

  // UMSEL0 = 0, asynchronous usart

  // UPM0 = 0, parity check disabled

  // USBS0 = 0, 1 stop bit

  // UCSZ0 = 3, 8-bit

  UCSR0B = _BV(RXEN0) | _BV(TXEN0);

  UCSR0C = _BV(UCSZ01) | _BV(UCSZ00);

  // === init time ===

  // COM0x = 0, pin OC0x not used

  // WGM0 = 2, clear timer on compare match, TOP = OCRA

  // CS0 = 3, 64 prescaler

  TCCR0A = _BV(WGM01);

  TCCR0B = _BV(CS01) | _BV(CS00);

  // enable interrupt on compare match A

  TIMSK0 = _BV(OCIE0A);

  // (1 ms) * 16 MHz / 64 prescaler = 250

  OCR0A = 250;

  millis = 0;

}

int Atmega128rfa1::read()

{

  // TODO make a rx buffer

  if (UCSR0A & _BV(RXC0)) return UDR0;

  else return -1;

}

void Atmega128rfa1::write(uint8_t* data, int length)

{

  // TODO make this non-blocking with a tx buffer

  int i;

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

  {

    while (!(UCSR0A & _BV(UDRE0)));

    UDR0 = data[i];

  }

}

unsigned long Atmega128rfa1::time()

{

  unsigned long ret;

  cli();

  ret = millis;

  sei();

  return ret;

}

#ifndef _ATMEGA128RFA1_H_

#define _ATMEGA128RFA1_H_

#include "ros/node_handle.h"

#define MAX_SUBSCRIBERS 25

#define MAX_PUBLISHERS 25

#define INPUT_SIZE 512

#define OUTPUT_SIZE 512

class Atmega128rfa1

{

public:

  Atmega128rfa1();

  void init();

  int read();

  void write(uint8_t* data, int length);

  unsigned long time();

};

#endif

#include "Atmega128rfa1.h"

#include <std_msgs/UInt16.h>

/*int main()

{

  std_msgs::UInt16 counter;

  ros::NodeHandle nh;

  ros::Publisher test("rosserial_test", &counter);

  nh.initNode();

  nh.advertise(test);

  counter.data = 0;

  while (1)

  {

    test.publish(&counter);

    counter.data++;

    nh.spinOnce();

  }

  return 0;

}*/

extern "C"

{

#include <stdlib.h>

#include <string.h>

}

int main()

{

  char time[20];

  Atmega128rfa1 avr;

  avr.init();

  while (1)

  {

    ultoa(avr.time(), time, 10);

    avr.write((uint8_t*) time, strlen(time));

  }

  return 0;

}

#ifndef _ROS_H_

#define _ROS_H_

#include "Atmega128rfa1.h"

namespace ros

{

  typedef ros::NodeHandle_<Atmega128rfa1, MAX_SUBSCRIBERS, MAX_PUBLISHERS,

          INPUT_SIZE, OUTPUT_SIZE> NodeHandle;

}

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

 */

#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.");

        }

      }

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

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