T Nara / Mbed 2 deprecated ros_lib_kinetic

hairo

Dependencies:   mbed BufferedSerial

ros/node_handle.h@4:bcdd99711326, 2018-12-04 (annotated)

Committer:
tknara
Date:
Tue Dec 04 00:14:32 2018 +0000
Revision:
4:bcdd99711326
Parent:
0:9e9b7db60fd5 
test

Who changed what in which revision?

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