Akash Vibhute / Mbed 2 deprecated rosserial_hydro

rosserial library for mbed Inspired by nucho's rosserial library This library is still under development

Dependencies:   MODSERIAL mbed

Dependents:   mbed_roshydro_test

Library still under development!

ros/node_handle.h@0:30537dec6e0b, 2015-02-15 (annotated)

Committer:
akashvibhute
Date:
Sun Feb 15 10:53:43 2015 +0000
Revision:
0:30537dec6e0b
First commit; Library still need to be debugged, compilation issues with new mbed and modserial updates.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akashvibhute 0:30537dec6e0b 1 /*
akashvibhute 0:30537dec6e0b 2 * Software License Agreement (BSD License)
akashvibhute 0:30537dec6e0b 3 *
akashvibhute 0:30537dec6e0b 4 * Copyright (c) 2011, Willow Garage, Inc.
akashvibhute 0:30537dec6e0b 5 * All rights reserved.
akashvibhute 0:30537dec6e0b 6 *
akashvibhute 0:30537dec6e0b 7 * Redistribution and use in source and binary forms, with or without
akashvibhute 0:30537dec6e0b 8 * modification, are permitted provided that the following conditions
akashvibhute 0:30537dec6e0b 9 * are met:
akashvibhute 0:30537dec6e0b 10 *
akashvibhute 0:30537dec6e0b 11 * * Redistributions of source code must retain the above copyright
akashvibhute 0:30537dec6e0b 12 * notice, this list of conditions and the following disclaimer.
akashvibhute 0:30537dec6e0b 13 * * Redistributions in binary form must reproduce the above
akashvibhute 0:30537dec6e0b 14 * copyright notice, this list of conditions and the following
akashvibhute 0:30537dec6e0b 15 * disclaimer in the documentation and/or other materials provided
akashvibhute 0:30537dec6e0b 16 * with the distribution.
akashvibhute 0:30537dec6e0b 17 * * Neither the name of Willow Garage, Inc. nor the names of its
akashvibhute 0:30537dec6e0b 18 * contributors may be used to endorse or promote prducts derived
akashvibhute 0:30537dec6e0b 19 * from this software without specific prior written permission.
akashvibhute 0:30537dec6e0b 20 *
akashvibhute 0:30537dec6e0b 21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
akashvibhute 0:30537dec6e0b 22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
akashvibhute 0:30537dec6e0b 23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
akashvibhute 0:30537dec6e0b 24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
akashvibhute 0:30537dec6e0b 25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
akashvibhute 0:30537dec6e0b 26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
akashvibhute 0:30537dec6e0b 27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
akashvibhute 0:30537dec6e0b 28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
akashvibhute 0:30537dec6e0b 29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
akashvibhute 0:30537dec6e0b 30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
akashvibhute 0:30537dec6e0b 31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
akashvibhute 0:30537dec6e0b 32 * POSSIBILITY OF SUCH DAMAGE.
akashvibhute 0:30537dec6e0b 33 */
akashvibhute 0:30537dec6e0b 34
akashvibhute 0:30537dec6e0b 35 #ifndef ROS_NODE_HANDLE_H_
akashvibhute 0:30537dec6e0b 36 #define ROS_NODE_HANDLE_H_
akashvibhute 0:30537dec6e0b 37
akashvibhute 0:30537dec6e0b 38 #include "std_msgs/Time.h"
akashvibhute 0:30537dec6e0b 39 #include "rosserial_msgs/TopicInfo.h"
akashvibhute 0:30537dec6e0b 40 #include "rosserial_msgs/Log.h"
akashvibhute 0:30537dec6e0b 41 #include "rosserial_msgs/RequestParam.h"
akashvibhute 0:30537dec6e0b 42
akashvibhute 0:30537dec6e0b 43 #define SYNC_SECONDS 5
akashvibhute 0:30537dec6e0b 44
akashvibhute 0:30537dec6e0b 45 #define MODE_FIRST_FF 0
akashvibhute 0:30537dec6e0b 46 /*
akashvibhute 0:30537dec6e0b 47 * The second sync byte is a protocol version. It's value is 0xff for the first
akashvibhute 0:30537dec6e0b 48 * version of the rosserial protocol (used up to hydro), 0xfe for the second version
akashvibhute 0:30537dec6e0b 49 * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable
akashvibhute 0:30537dec6e0b 50 * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy
akashvibhute 0:30537dec6e0b 51 * rosserial_arduino. It must be changed in both this file and in
akashvibhute 0:30537dec6e0b 52 * rosserial_python/src/rosserial_python/SerialClient.py
akashvibhute 0:30537dec6e0b 53 */
akashvibhute 0:30537dec6e0b 54 #define MODE_PROTOCOL_VER 1
akashvibhute 0:30537dec6e0b 55 #define PROTOCOL_VER1 0xff // through groovy
akashvibhute 0:30537dec6e0b 56 #define PROTOCOL_VER2 0xfe // in hydro
akashvibhute 0:30537dec6e0b 57 #define PROTOCOL_VER PROTOCOL_VER2
akashvibhute 0:30537dec6e0b 58 #define MODE_SIZE_L 2
akashvibhute 0:30537dec6e0b 59 #define MODE_SIZE_H 3
akashvibhute 0:30537dec6e0b 60 #define MODE_SIZE_CHECKSUM 4 // checksum for msg size received from size L and H
akashvibhute 0:30537dec6e0b 61 #define MODE_TOPIC_L 5 // waiting for topic id
akashvibhute 0:30537dec6e0b 62 #define MODE_TOPIC_H 6
akashvibhute 0:30537dec6e0b 63 #define MODE_MESSAGE 7
akashvibhute 0:30537dec6e0b 64 #define MODE_MSG_CHECKSUM 8 // checksum for msg and topic id
akashvibhute 0:30537dec6e0b 65
akashvibhute 0:30537dec6e0b 66
akashvibhute 0:30537dec6e0b 67 #define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data
akashvibhute 0:30537dec6e0b 68
akashvibhute 0:30537dec6e0b 69 #include "msg.h"
akashvibhute 0:30537dec6e0b 70
akashvibhute 0:30537dec6e0b 71 namespace ros {
akashvibhute 0:30537dec6e0b 72
akashvibhute 0:30537dec6e0b 73 class NodeHandleBase_{
akashvibhute 0:30537dec6e0b 74 public:
akashvibhute 0:30537dec6e0b 75 virtual int publish(int id, const Msg* msg)=0;
akashvibhute 0:30537dec6e0b 76 virtual int spinOnce()=0;
akashvibhute 0:30537dec6e0b 77 virtual bool connected()=0;
akashvibhute 0:30537dec6e0b 78 };
akashvibhute 0:30537dec6e0b 79 }
akashvibhute 0:30537dec6e0b 80
akashvibhute 0:30537dec6e0b 81 #include "publisher.h"
akashvibhute 0:30537dec6e0b 82 #include "subscriber.h"
akashvibhute 0:30537dec6e0b 83 #include "service_server.h"
akashvibhute 0:30537dec6e0b 84 #include "service_client.h"
akashvibhute 0:30537dec6e0b 85
akashvibhute 0:30537dec6e0b 86 namespace ros {
akashvibhute 0:30537dec6e0b 87
akashvibhute 0:30537dec6e0b 88 using rosserial_msgs::TopicInfo;
akashvibhute 0:30537dec6e0b 89
akashvibhute 0:30537dec6e0b 90 /* Node Handle */
akashvibhute 0:30537dec6e0b 91 template<class Hardware,
akashvibhute 0:30537dec6e0b 92 int MAX_SUBSCRIBERS=25,
akashvibhute 0:30537dec6e0b 93 int MAX_PUBLISHERS=25,
akashvibhute 0:30537dec6e0b 94 int INPUT_SIZE=512,
akashvibhute 0:30537dec6e0b 95 int OUTPUT_SIZE=512>
akashvibhute 0:30537dec6e0b 96 class NodeHandle_ : public NodeHandleBase_
akashvibhute 0:30537dec6e0b 97 {
akashvibhute 0:30537dec6e0b 98 protected:
akashvibhute 0:30537dec6e0b 99 Hardware hardware_;
akashvibhute 0:30537dec6e0b 100
akashvibhute 0:30537dec6e0b 101 /* time used for syncing */
akashvibhute 0:30537dec6e0b 102 unsigned long rt_time;
akashvibhute 0:30537dec6e0b 103
akashvibhute 0:30537dec6e0b 104 /* used for computing current time */
akashvibhute 0:30537dec6e0b 105 unsigned long sec_offset, nsec_offset;
akashvibhute 0:30537dec6e0b 106
akashvibhute 0:30537dec6e0b 107 unsigned char message_in[INPUT_SIZE];
akashvibhute 0:30537dec6e0b 108 unsigned char message_out[OUTPUT_SIZE];
akashvibhute 0:30537dec6e0b 109
akashvibhute 0:30537dec6e0b 110 Publisher * publishers[MAX_PUBLISHERS];
akashvibhute 0:30537dec6e0b 111 Subscriber_ * subscribers[MAX_SUBSCRIBERS];
akashvibhute 0:30537dec6e0b 112
akashvibhute 0:30537dec6e0b 113 /*
akashvibhute 0:30537dec6e0b 114 * Setup Functions
akashvibhute 0:30537dec6e0b 115 */
akashvibhute 0:30537dec6e0b 116 public:
akashvibhute 0:30537dec6e0b 117 NodeHandle_() : configured_(false) {
akashvibhute 0:30537dec6e0b 118
akashvibhute 0:30537dec6e0b 119 for(unsigned int i=0; i< MAX_PUBLISHERS; i++)
akashvibhute 0:30537dec6e0b 120 publishers[i] = 0;
akashvibhute 0:30537dec6e0b 121
akashvibhute 0:30537dec6e0b 122 for(unsigned int i=0; i< MAX_SUBSCRIBERS; i++)
akashvibhute 0:30537dec6e0b 123 subscribers[i] = 0;
akashvibhute 0:30537dec6e0b 124
akashvibhute 0:30537dec6e0b 125 for(unsigned int i=0; i< INPUT_SIZE; i++)
akashvibhute 0:30537dec6e0b 126 message_in[i] = 0;
akashvibhute 0:30537dec6e0b 127
akashvibhute 0:30537dec6e0b 128 for(unsigned int i=0; i< OUTPUT_SIZE; i++)
akashvibhute 0:30537dec6e0b 129 message_out[i] = 0;
akashvibhute 0:30537dec6e0b 130
akashvibhute 0:30537dec6e0b 131 req_param_resp.ints_length = 0;
akashvibhute 0:30537dec6e0b 132 req_param_resp.ints = NULL;
akashvibhute 0:30537dec6e0b 133 req_param_resp.floats_length = 0;
akashvibhute 0:30537dec6e0b 134 req_param_resp.floats = NULL;
akashvibhute 0:30537dec6e0b 135 req_param_resp.ints_length = 0;
akashvibhute 0:30537dec6e0b 136 req_param_resp.ints = NULL;
akashvibhute 0:30537dec6e0b 137 }
akashvibhute 0:30537dec6e0b 138
akashvibhute 0:30537dec6e0b 139 Hardware* getHardware(){
akashvibhute 0:30537dec6e0b 140 return &hardware_;
akashvibhute 0:30537dec6e0b 141 }
akashvibhute 0:30537dec6e0b 142
akashvibhute 0:30537dec6e0b 143 /* Start serial, initialize buffers */
akashvibhute 0:30537dec6e0b 144 void initNode(){
akashvibhute 0:30537dec6e0b 145 hardware_.init();
akashvibhute 0:30537dec6e0b 146 mode_ = 0;
akashvibhute 0:30537dec6e0b 147 bytes_ = 0;
akashvibhute 0:30537dec6e0b 148 index_ = 0;
akashvibhute 0:30537dec6e0b 149 topic_ = 0;
akashvibhute 0:30537dec6e0b 150 };
akashvibhute 0:30537dec6e0b 151
akashvibhute 0:30537dec6e0b 152 /* Start a named port, which may be network server IP, initialize buffers */
akashvibhute 0:30537dec6e0b 153 void initNode(char *portName){
akashvibhute 0:30537dec6e0b 154 hardware_.init(portName);
akashvibhute 0:30537dec6e0b 155 mode_ = 0;
akashvibhute 0:30537dec6e0b 156 bytes_ = 0;
akashvibhute 0:30537dec6e0b 157 index_ = 0;
akashvibhute 0:30537dec6e0b 158 topic_ = 0;
akashvibhute 0:30537dec6e0b 159 };
akashvibhute 0:30537dec6e0b 160
akashvibhute 0:30537dec6e0b 161 protected:
akashvibhute 0:30537dec6e0b 162 //State machine variables for spinOnce
akashvibhute 0:30537dec6e0b 163 int mode_;
akashvibhute 0:30537dec6e0b 164 int bytes_;
akashvibhute 0:30537dec6e0b 165 int topic_;
akashvibhute 0:30537dec6e0b 166 int index_;
akashvibhute 0:30537dec6e0b 167 int checksum_;
akashvibhute 0:30537dec6e0b 168
akashvibhute 0:30537dec6e0b 169 bool configured_;
akashvibhute 0:30537dec6e0b 170
akashvibhute 0:30537dec6e0b 171 /* used for syncing the time */
akashvibhute 0:30537dec6e0b 172 unsigned long last_sync_time;
akashvibhute 0:30537dec6e0b 173 unsigned long last_sync_receive_time;
akashvibhute 0:30537dec6e0b 174 unsigned long last_msg_timeout_time;
akashvibhute 0:30537dec6e0b 175
akashvibhute 0:30537dec6e0b 176 public:
akashvibhute 0:30537dec6e0b 177 /* This function goes in your loop() function, it handles
akashvibhute 0:30537dec6e0b 178 * serial input and callbacks for subscribers.
akashvibhute 0:30537dec6e0b 179 */
akashvibhute 0:30537dec6e0b 180
akashvibhute 0:30537dec6e0b 181
akashvibhute 0:30537dec6e0b 182 virtual int spinOnce(){
akashvibhute 0:30537dec6e0b 183
akashvibhute 0:30537dec6e0b 184 /* restart if timed out */
akashvibhute 0:30537dec6e0b 185 unsigned long c_time = hardware_.time();
akashvibhute 0:30537dec6e0b 186 if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){
akashvibhute 0:30537dec6e0b 187 configured_ = false;
akashvibhute 0:30537dec6e0b 188 }
akashvibhute 0:30537dec6e0b 189
akashvibhute 0:30537dec6e0b 190 /* reset if message has timed out */
akashvibhute 0:30537dec6e0b 191 if ( mode_ != MODE_FIRST_FF){
akashvibhute 0:30537dec6e0b 192 if (c_time > last_msg_timeout_time){
akashvibhute 0:30537dec6e0b 193 mode_ = MODE_FIRST_FF;
akashvibhute 0:30537dec6e0b 194 }
akashvibhute 0:30537dec6e0b 195 }
akashvibhute 0:30537dec6e0b 196
akashvibhute 0:30537dec6e0b 197 /* while available buffer, read data */
akashvibhute 0:30537dec6e0b 198 while( true )
akashvibhute 0:30537dec6e0b 199 {
akashvibhute 0:30537dec6e0b 200 int data = hardware_.read();
akashvibhute 0:30537dec6e0b 201 if( data < 0 )
akashvibhute 0:30537dec6e0b 202 break;
akashvibhute 0:30537dec6e0b 203 checksum_ += data;
akashvibhute 0:30537dec6e0b 204 if( mode_ == MODE_MESSAGE ){ /* message data being recieved */
akashvibhute 0:30537dec6e0b 205 message_in[index_++] = data;
akashvibhute 0:30537dec6e0b 206 bytes_--;
akashvibhute 0:30537dec6e0b 207 if(bytes_ == 0) /* is message complete? if so, checksum */
akashvibhute 0:30537dec6e0b 208 mode_ = MODE_MSG_CHECKSUM;
akashvibhute 0:30537dec6e0b 209 }else if( mode_ == MODE_FIRST_FF ){
akashvibhute 0:30537dec6e0b 210 if(data == 0xff){
akashvibhute 0:30537dec6e0b 211 mode_++;
akashvibhute 0:30537dec6e0b 212 last_msg_timeout_time = c_time + MSG_TIMEOUT;
akashvibhute 0:30537dec6e0b 213 }
akashvibhute 0:30537dec6e0b 214 else if( hardware_.time() - c_time > (SYNC_SECONDS)){
akashvibhute 0:30537dec6e0b 215 /* We have been stuck in spinOnce too long, return error */
akashvibhute 0:30537dec6e0b 216 configured_=false;
akashvibhute 0:30537dec6e0b 217 return -2;
akashvibhute 0:30537dec6e0b 218 }
akashvibhute 0:30537dec6e0b 219 }else if( mode_ == MODE_PROTOCOL_VER ){
akashvibhute 0:30537dec6e0b 220 if(data == PROTOCOL_VER){
akashvibhute 0:30537dec6e0b 221 mode_++;
akashvibhute 0:30537dec6e0b 222 }else{
akashvibhute 0:30537dec6e0b 223 mode_ = MODE_FIRST_FF;
akashvibhute 0:30537dec6e0b 224 if (configured_ == false)
akashvibhute 0:30537dec6e0b 225 requestSyncTime(); /* send a msg back showing our protocol version */
akashvibhute 0:30537dec6e0b 226 }
akashvibhute 0:30537dec6e0b 227 }else if( mode_ == MODE_SIZE_L ){ /* bottom half of message size */
akashvibhute 0:30537dec6e0b 228 bytes_ = data;
akashvibhute 0:30537dec6e0b 229 index_ = 0;
akashvibhute 0:30537dec6e0b 230 mode_++;
akashvibhute 0:30537dec6e0b 231 checksum_ = data; /* first byte for calculating size checksum */
akashvibhute 0:30537dec6e0b 232 }else if( mode_ == MODE_SIZE_H ){ /* top half of message size */
akashvibhute 0:30537dec6e0b 233 bytes_ += data<<8;
akashvibhute 0:30537dec6e0b 234 mode_++;
akashvibhute 0:30537dec6e0b 235 }else if( mode_ == MODE_SIZE_CHECKSUM ){
akashvibhute 0:30537dec6e0b 236 if( (checksum_%256) == 255)
akashvibhute 0:30537dec6e0b 237 mode_++;
akashvibhute 0:30537dec6e0b 238 else
akashvibhute 0:30537dec6e0b 239 mode_ = MODE_FIRST_FF; /* Abandon the frame if the msg len is wrong */
akashvibhute 0:30537dec6e0b 240 }else if( mode_ == MODE_TOPIC_L ){ /* bottom half of topic id */
akashvibhute 0:30537dec6e0b 241 topic_ = data;
akashvibhute 0:30537dec6e0b 242 mode_++;
akashvibhute 0:30537dec6e0b 243 checksum_ = data; /* first byte included in checksum */
akashvibhute 0:30537dec6e0b 244 }else if( mode_ == MODE_TOPIC_H ){ /* top half of topic id */
akashvibhute 0:30537dec6e0b 245 topic_ += data<<8;
akashvibhute 0:30537dec6e0b 246 mode_ = MODE_MESSAGE;
akashvibhute 0:30537dec6e0b 247 if(bytes_ == 0)
akashvibhute 0:30537dec6e0b 248 mode_ = MODE_MSG_CHECKSUM;
akashvibhute 0:30537dec6e0b 249 }else if( mode_ == MODE_MSG_CHECKSUM ){ /* do checksum */
akashvibhute 0:30537dec6e0b 250 mode_ = MODE_FIRST_FF;
akashvibhute 0:30537dec6e0b 251 if( (checksum_%256) == 255){
akashvibhute 0:30537dec6e0b 252 if(topic_ == TopicInfo::ID_PUBLISHER){
akashvibhute 0:30537dec6e0b 253 requestSyncTime();
akashvibhute 0:30537dec6e0b 254 negotiateTopics();
akashvibhute 0:30537dec6e0b 255 last_sync_time = c_time;
akashvibhute 0:30537dec6e0b 256 last_sync_receive_time = c_time;
akashvibhute 0:30537dec6e0b 257 return -1;
akashvibhute 0:30537dec6e0b 258 }else if(topic_ == TopicInfo::ID_TIME){
akashvibhute 0:30537dec6e0b 259 syncTime(message_in);
akashvibhute 0:30537dec6e0b 260 }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){
akashvibhute 0:30537dec6e0b 261 req_param_resp.deserialize(message_in);
akashvibhute 0:30537dec6e0b 262 param_recieved= true;
akashvibhute 0:30537dec6e0b 263 }else if(topic_ == TopicInfo::ID_TX_STOP){
akashvibhute 0:30537dec6e0b 264 configured_ = false;
akashvibhute 0:30537dec6e0b 265 }else{
akashvibhute 0:30537dec6e0b 266 if(subscribers[topic_-100])
akashvibhute 0:30537dec6e0b 267 subscribers[topic_-100]->callback( message_in );
akashvibhute 0:30537dec6e0b 268 }
akashvibhute 0:30537dec6e0b 269 }
akashvibhute 0:30537dec6e0b 270 }
akashvibhute 0:30537dec6e0b 271 }
akashvibhute 0:30537dec6e0b 272
akashvibhute 0:30537dec6e0b 273 /* occasionally sync time */
akashvibhute 0:30537dec6e0b 274 if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){
akashvibhute 0:30537dec6e0b 275 requestSyncTime();
akashvibhute 0:30537dec6e0b 276 last_sync_time = c_time;
akashvibhute 0:30537dec6e0b 277 }
akashvibhute 0:30537dec6e0b 278
akashvibhute 0:30537dec6e0b 279 return 0;
akashvibhute 0:30537dec6e0b 280 }
akashvibhute 0:30537dec6e0b 281
akashvibhute 0:30537dec6e0b 282
akashvibhute 0:30537dec6e0b 283 /* Are we connected to the PC? */
akashvibhute 0:30537dec6e0b 284 virtual bool connected() {
akashvibhute 0:30537dec6e0b 285 return configured_;
akashvibhute 0:30537dec6e0b 286 };
akashvibhute 0:30537dec6e0b 287
akashvibhute 0:30537dec6e0b 288 /********************************************************************
akashvibhute 0:30537dec6e0b 289 * Time functions
akashvibhute 0:30537dec6e0b 290 */
akashvibhute 0:30537dec6e0b 291
akashvibhute 0:30537dec6e0b 292 void requestSyncTime()
akashvibhute 0:30537dec6e0b 293 {
akashvibhute 0:30537dec6e0b 294 std_msgs::Time t;
akashvibhute 0:30537dec6e0b 295 publish(TopicInfo::ID_TIME, &t);
akashvibhute 0:30537dec6e0b 296 rt_time = hardware_.time();
akashvibhute 0:30537dec6e0b 297 }
akashvibhute 0:30537dec6e0b 298
akashvibhute 0:30537dec6e0b 299 void syncTime( unsigned char * data )
akashvibhute 0:30537dec6e0b 300 {
akashvibhute 0:30537dec6e0b 301 std_msgs::Time t;
akashvibhute 0:30537dec6e0b 302 unsigned long offset = hardware_.time() - rt_time;
akashvibhute 0:30537dec6e0b 303
akashvibhute 0:30537dec6e0b 304 t.deserialize(data);
akashvibhute 0:30537dec6e0b 305 t.data.sec += offset/1000;
akashvibhute 0:30537dec6e0b 306 t.data.nsec += (offset%1000)*1000000UL;
akashvibhute 0:30537dec6e0b 307
akashvibhute 0:30537dec6e0b 308 this->setNow(t.data);
akashvibhute 0:30537dec6e0b 309 last_sync_receive_time = hardware_.time();
akashvibhute 0:30537dec6e0b 310 }
akashvibhute 0:30537dec6e0b 311
akashvibhute 0:30537dec6e0b 312 Time now(){
akashvibhute 0:30537dec6e0b 313 unsigned long ms = hardware_.time();
akashvibhute 0:30537dec6e0b 314 Time current_time;
akashvibhute 0:30537dec6e0b 315 current_time.sec = ms/1000 + sec_offset;
akashvibhute 0:30537dec6e0b 316 current_time.nsec = (ms%1000)*1000000UL + nsec_offset;
akashvibhute 0:30537dec6e0b 317 normalizeSecNSec(current_time.sec, current_time.nsec);
akashvibhute 0:30537dec6e0b 318 return current_time;
akashvibhute 0:30537dec6e0b 319 }
akashvibhute 0:30537dec6e0b 320
akashvibhute 0:30537dec6e0b 321 void setNow( Time & new_now )
akashvibhute 0:30537dec6e0b 322 {
akashvibhute 0:30537dec6e0b 323 unsigned long ms = hardware_.time();
akashvibhute 0:30537dec6e0b 324 sec_offset = new_now.sec - ms/1000 - 1;
akashvibhute 0:30537dec6e0b 325 nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
akashvibhute 0:30537dec6e0b 326 normalizeSecNSec(sec_offset, nsec_offset);
akashvibhute 0:30537dec6e0b 327 }
akashvibhute 0:30537dec6e0b 328
akashvibhute 0:30537dec6e0b 329 /********************************************************************
akashvibhute 0:30537dec6e0b 330 * Topic Management
akashvibhute 0:30537dec6e0b 331 */
akashvibhute 0:30537dec6e0b 332
akashvibhute 0:30537dec6e0b 333 /* Register a new publisher */
akashvibhute 0:30537dec6e0b 334 bool advertise(Publisher & p)
akashvibhute 0:30537dec6e0b 335 {
akashvibhute 0:30537dec6e0b 336 for(int i = 0; i < MAX_PUBLISHERS; i++){
akashvibhute 0:30537dec6e0b 337 if(publishers[i] == 0){ // empty slot
akashvibhute 0:30537dec6e0b 338 publishers[i] = &p;
akashvibhute 0:30537dec6e0b 339 p.id_ = i+100+MAX_SUBSCRIBERS;
akashvibhute 0:30537dec6e0b 340 p.nh_ = this;
akashvibhute 0:30537dec6e0b 341 return true;
akashvibhute 0:30537dec6e0b 342 }
akashvibhute 0:30537dec6e0b 343 }
akashvibhute 0:30537dec6e0b 344 return false;
akashvibhute 0:30537dec6e0b 345 }
akashvibhute 0:30537dec6e0b 346
akashvibhute 0:30537dec6e0b 347 /* Register a new subscriber */
akashvibhute 0:30537dec6e0b 348 template<typename MsgT>
akashvibhute 0:30537dec6e0b 349 bool subscribe(Subscriber< MsgT> & s){
akashvibhute 0:30537dec6e0b 350 for(int i = 0; i < MAX_SUBSCRIBERS; i++){
akashvibhute 0:30537dec6e0b 351 if(subscribers[i] == 0){ // empty slot
akashvibhute 0:30537dec6e0b 352 subscribers[i] = (Subscriber_*) &s;
akashvibhute 0:30537dec6e0b 353 s.id_ = i+100;
akashvibhute 0:30537dec6e0b 354 return true;
akashvibhute 0:30537dec6e0b 355 }
akashvibhute 0:30537dec6e0b 356 }
akashvibhute 0:30537dec6e0b 357 return false;
akashvibhute 0:30537dec6e0b 358 }
akashvibhute 0:30537dec6e0b 359
akashvibhute 0:30537dec6e0b 360 /* Register a new Service Server */
akashvibhute 0:30537dec6e0b 361 template<typename MReq, typename MRes>
akashvibhute 0:30537dec6e0b 362 bool advertiseService(ServiceServer<MReq,MRes>& srv){
akashvibhute 0:30537dec6e0b 363 bool v = advertise(srv.pub);
akashvibhute 0:30537dec6e0b 364 for(int i = 0; i < MAX_SUBSCRIBERS; i++){
akashvibhute 0:30537dec6e0b 365 if(subscribers[i] == 0){ // empty slot
akashvibhute 0:30537dec6e0b 366 subscribers[i] = (Subscriber_*) &srv;
akashvibhute 0:30537dec6e0b 367 srv.id_ = i+100;
akashvibhute 0:30537dec6e0b 368 return v;
akashvibhute 0:30537dec6e0b 369 }
akashvibhute 0:30537dec6e0b 370 }
akashvibhute 0:30537dec6e0b 371 return false;
akashvibhute 0:30537dec6e0b 372 }
akashvibhute 0:30537dec6e0b 373
akashvibhute 0:30537dec6e0b 374 /* Register a new Service Client */
akashvibhute 0:30537dec6e0b 375 template<typename MReq, typename MRes>
akashvibhute 0:30537dec6e0b 376 bool serviceClient(ServiceClient<MReq, MRes>& srv){
akashvibhute 0:30537dec6e0b 377 bool v = advertise(srv.pub);
akashvibhute 0:30537dec6e0b 378 for(int i = 0; i < MAX_SUBSCRIBERS; i++){
akashvibhute 0:30537dec6e0b 379 if(subscribers[i] == 0){ // empty slot
akashvibhute 0:30537dec6e0b 380 subscribers[i] = (Subscriber_*) &srv;
akashvibhute 0:30537dec6e0b 381 srv.id_ = i+100;
akashvibhute 0:30537dec6e0b 382 return v;
akashvibhute 0:30537dec6e0b 383 }
akashvibhute 0:30537dec6e0b 384 }
akashvibhute 0:30537dec6e0b 385 return false;
akashvibhute 0:30537dec6e0b 386 }
akashvibhute 0:30537dec6e0b 387
akashvibhute 0:30537dec6e0b 388 void negotiateTopics()
akashvibhute 0:30537dec6e0b 389 {
akashvibhute 0:30537dec6e0b 390 rosserial_msgs::TopicInfo ti;
akashvibhute 0:30537dec6e0b 391 int i;
akashvibhute 0:30537dec6e0b 392 for(i = 0; i < MAX_PUBLISHERS; i++)
akashvibhute 0:30537dec6e0b 393 {
akashvibhute 0:30537dec6e0b 394 if(publishers[i] != 0) // non-empty slot
akashvibhute 0:30537dec6e0b 395 {
akashvibhute 0:30537dec6e0b 396 ti.topic_id = publishers[i]->id_;
akashvibhute 0:30537dec6e0b 397 ti.topic_name = (char *) publishers[i]->topic_;
akashvibhute 0:30537dec6e0b 398 ti.message_type = (char *) publishers[i]->msg_->getType();
akashvibhute 0:30537dec6e0b 399 ti.md5sum = (char *) publishers[i]->msg_->getMD5();
akashvibhute 0:30537dec6e0b 400 ti.buffer_size = OUTPUT_SIZE;
akashvibhute 0:30537dec6e0b 401 publish( publishers[i]->getEndpointType(), &ti );
akashvibhute 0:30537dec6e0b 402 }
akashvibhute 0:30537dec6e0b 403 }
akashvibhute 0:30537dec6e0b 404 for(i = 0; i < MAX_SUBSCRIBERS; i++)
akashvibhute 0:30537dec6e0b 405 {
akashvibhute 0:30537dec6e0b 406 if(subscribers[i] != 0) // non-empty slot
akashvibhute 0:30537dec6e0b 407 {
akashvibhute 0:30537dec6e0b 408 ti.topic_id = subscribers[i]->id_;
akashvibhute 0:30537dec6e0b 409 ti.topic_name = (char *) subscribers[i]->topic_;
akashvibhute 0:30537dec6e0b 410 ti.message_type = (char *) subscribers[i]->getMsgType();
akashvibhute 0:30537dec6e0b 411 ti.md5sum = (char *) subscribers[i]->getMsgMD5();
akashvibhute 0:30537dec6e0b 412 ti.buffer_size = INPUT_SIZE;
akashvibhute 0:30537dec6e0b 413 publish( subscribers[i]->getEndpointType(), &ti );
akashvibhute 0:30537dec6e0b 414 }
akashvibhute 0:30537dec6e0b 415 }
akashvibhute 0:30537dec6e0b 416 configured_ = true;
akashvibhute 0:30537dec6e0b 417 }
akashvibhute 0:30537dec6e0b 418
akashvibhute 0:30537dec6e0b 419 virtual int publish(int id, const Msg * msg)
akashvibhute 0:30537dec6e0b 420 {
akashvibhute 0:30537dec6e0b 421 if(id >= 100 && !configured_)
akashvibhute 0:30537dec6e0b 422 return 0;
akashvibhute 0:30537dec6e0b 423
akashvibhute 0:30537dec6e0b 424 /* serialize message */
akashvibhute 0:30537dec6e0b 425 unsigned int l = msg->serialize(message_out+7);
akashvibhute 0:30537dec6e0b 426
akashvibhute 0:30537dec6e0b 427 /* setup the header */
akashvibhute 0:30537dec6e0b 428 message_out[0] = 0xff;
akashvibhute 0:30537dec6e0b 429 message_out[1] = PROTOCOL_VER;
akashvibhute 0:30537dec6e0b 430 message_out[2] = (unsigned char) ((unsigned int)l&255);
akashvibhute 0:30537dec6e0b 431 message_out[3] = (unsigned char) ((unsigned int)l>>8);
akashvibhute 0:30537dec6e0b 432 message_out[4] = 255 - ((message_out[2] + message_out[3])%256);
akashvibhute 0:30537dec6e0b 433 message_out[5] = (unsigned char) ((int)id&255);
akashvibhute 0:30537dec6e0b 434 message_out[6] = (unsigned char) ((int)id>>8);
akashvibhute 0:30537dec6e0b 435
akashvibhute 0:30537dec6e0b 436 /* calculate checksum */
akashvibhute 0:30537dec6e0b 437 int chk = 0;
akashvibhute 0:30537dec6e0b 438 for(int i =5; i<l+7; i++)
akashvibhute 0:30537dec6e0b 439 chk += message_out[i];
akashvibhute 0:30537dec6e0b 440 l += 7;
akashvibhute 0:30537dec6e0b 441 message_out[l++] = 255 - (chk%256);
akashvibhute 0:30537dec6e0b 442
akashvibhute 0:30537dec6e0b 443 if( l <= OUTPUT_SIZE ){
akashvibhute 0:30537dec6e0b 444 hardware_.write(message_out, l);
akashvibhute 0:30537dec6e0b 445 return l;
akashvibhute 0:30537dec6e0b 446 }else{
akashvibhute 0:30537dec6e0b 447 logerror("Message from device dropped: message larger than buffer.");
akashvibhute 0:30537dec6e0b 448 return -1;
akashvibhute 0:30537dec6e0b 449 }
akashvibhute 0:30537dec6e0b 450 }
akashvibhute 0:30537dec6e0b 451
akashvibhute 0:30537dec6e0b 452 /********************************************************************
akashvibhute 0:30537dec6e0b 453 * Logging
akashvibhute 0:30537dec6e0b 454 */
akashvibhute 0:30537dec6e0b 455
akashvibhute 0:30537dec6e0b 456 private:
akashvibhute 0:30537dec6e0b 457 void log(char byte, const char * msg){
akashvibhute 0:30537dec6e0b 458 rosserial_msgs::Log l;
akashvibhute 0:30537dec6e0b 459 l.level= byte;
akashvibhute 0:30537dec6e0b 460 l.msg = (char*)msg;
akashvibhute 0:30537dec6e0b 461 publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
akashvibhute 0:30537dec6e0b 462 }
akashvibhute 0:30537dec6e0b 463
akashvibhute 0:30537dec6e0b 464 public:
akashvibhute 0:30537dec6e0b 465 void logdebug(const char* msg){
akashvibhute 0:30537dec6e0b 466 log(rosserial_msgs::Log::ROSDEBUG, msg);
akashvibhute 0:30537dec6e0b 467 }
akashvibhute 0:30537dec6e0b 468 void loginfo(const char * msg){
akashvibhute 0:30537dec6e0b 469 log(rosserial_msgs::Log::INFO, msg);
akashvibhute 0:30537dec6e0b 470 }
akashvibhute 0:30537dec6e0b 471 void logwarn(const char *msg){
akashvibhute 0:30537dec6e0b 472 log(rosserial_msgs::Log::WARN, msg);
akashvibhute 0:30537dec6e0b 473 }
akashvibhute 0:30537dec6e0b 474 void logerror(const char*msg){
akashvibhute 0:30537dec6e0b 475 log(rosserial_msgs::Log::ERROR, msg);
akashvibhute 0:30537dec6e0b 476 }
akashvibhute 0:30537dec6e0b 477 void logfatal(const char*msg){
akashvibhute 0:30537dec6e0b 478 log(rosserial_msgs::Log::FATAL, msg);
akashvibhute 0:30537dec6e0b 479 }
akashvibhute 0:30537dec6e0b 480
akashvibhute 0:30537dec6e0b 481 /********************************************************************
akashvibhute 0:30537dec6e0b 482 * Parameters
akashvibhute 0:30537dec6e0b 483 */
akashvibhute 0:30537dec6e0b 484
akashvibhute 0:30537dec6e0b 485 private:
akashvibhute 0:30537dec6e0b 486 bool param_recieved;
akashvibhute 0:30537dec6e0b 487 rosserial_msgs::RequestParamResponse req_param_resp;
akashvibhute 0:30537dec6e0b 488
akashvibhute 0:30537dec6e0b 489 bool requestParam(const char * name, int time_out = 1000){
akashvibhute 0:30537dec6e0b 490 param_recieved = false;
akashvibhute 0:30537dec6e0b 491 rosserial_msgs::RequestParamRequest req;
akashvibhute 0:30537dec6e0b 492 req.name = (char*)name;
akashvibhute 0:30537dec6e0b 493 publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
akashvibhute 0:30537dec6e0b 494 unsigned int end_time = hardware_.time() + time_out;
akashvibhute 0:30537dec6e0b 495 while(!param_recieved ){
akashvibhute 0:30537dec6e0b 496 spinOnce();
akashvibhute 0:30537dec6e0b 497 if (hardware_.time() > end_time) return false;
akashvibhute 0:30537dec6e0b 498 }
akashvibhute 0:30537dec6e0b 499 return true;
akashvibhute 0:30537dec6e0b 500 }
akashvibhute 0:30537dec6e0b 501
akashvibhute 0:30537dec6e0b 502 public:
akashvibhute 0:30537dec6e0b 503 bool getParam(const char* name, int* param, int length =1){
akashvibhute 0:30537dec6e0b 504 if (requestParam(name) ){
akashvibhute 0:30537dec6e0b 505 if (length == req_param_resp.ints_length){
akashvibhute 0:30537dec6e0b 506 //copy it over
akashvibhute 0:30537dec6e0b 507 for(int i=0; i<length; i++)
akashvibhute 0:30537dec6e0b 508 param[i] = req_param_resp.ints[i];
akashvibhute 0:30537dec6e0b 509 return true;
akashvibhute 0:30537dec6e0b 510 }
akashvibhute 0:30537dec6e0b 511 }
akashvibhute 0:30537dec6e0b 512 return false;
akashvibhute 0:30537dec6e0b 513 }
akashvibhute 0:30537dec6e0b 514 bool getParam(const char* name, float* param, int length=1){
akashvibhute 0:30537dec6e0b 515 if (requestParam(name) ){
akashvibhute 0:30537dec6e0b 516 if (length == req_param_resp.floats_length){
akashvibhute 0:30537dec6e0b 517 //copy it over
akashvibhute 0:30537dec6e0b 518 for(int i=0; i<length; i++)
akashvibhute 0:30537dec6e0b 519 param[i] = req_param_resp.floats[i];
akashvibhute 0:30537dec6e0b 520 return true;
akashvibhute 0:30537dec6e0b 521 }
akashvibhute 0:30537dec6e0b 522 }
akashvibhute 0:30537dec6e0b 523 return false;
akashvibhute 0:30537dec6e0b 524 }
akashvibhute 0:30537dec6e0b 525 bool getParam(const char* name, char** param, int length=1){
akashvibhute 0:30537dec6e0b 526 if (requestParam(name) ){
akashvibhute 0:30537dec6e0b 527 if (length == req_param_resp.strings_length){
akashvibhute 0:30537dec6e0b 528 //copy it over
akashvibhute 0:30537dec6e0b 529 for(int i=0; i<length; i++)
akashvibhute 0:30537dec6e0b 530 strcpy(param[i],req_param_resp.strings[i]);
akashvibhute 0:30537dec6e0b 531 return true;
akashvibhute 0:30537dec6e0b 532 }
akashvibhute 0:30537dec6e0b 533 }
akashvibhute 0:30537dec6e0b 534 return false;
akashvibhute 0:30537dec6e0b 535 }
akashvibhute 0:30537dec6e0b 536 };
akashvibhute 0:30537dec6e0b 537
akashvibhute 0:30537dec6e0b 538 }
akashvibhute 0:30537dec6e0b 539
akashvibhute 0:30537dec6e0b 540 #endif
akashvibhute 0:30537dec6e0b 541