Jacob Hernandez / ros_lib_kinetic

This is a fork from the original, including a small change in the buffer size of the hardware interface (increased to 2048) and decreasing the number of publishers and subscribers to 5. Besides, the library about the message Adc.h was modified so as to increase the number of available Adc channels to be read ( from 6 to 7 ) For this modification, a change in checksum was required

Dependencies:   BufferedSerial

Fork of ros_lib_kinetic by Gary Servin

ros/node_handle.h@2:9114cc24ddcf, 2017-10-17 (annotated)

Committer:
jacobepfl1692
Date:
Tue Oct 17 18:49:03 2017 +0000
Revision:
2:9114cc24ddcf
Parent:
0:9e9b7db60fd5 
I increased the channels of the ADC to 6 (hence change in checksum) because my application needed it (STM32f407V6)

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