Pipe Inpection Robot / Mbed OS capstone_finish

capstone_finish

Dependencies:   BufferedSerial motor_sn7544

ros/node_handle.h@10:ca4e4062701a, 2019-11-26 (annotated)

Committer:
Jeonghoon
Date:
Tue Nov 26 15:30:27 2019 +0000
Revision:
10:ca4e4062701a
Parent:
3:2a3664dc6634 
capstone finish

Who changed what in which revision?

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