Pipe Inpection Robot / Mbed OS GPG_motor_latest

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BufferedSerial motor_sn7544

ros/node_handle.h@7:13dd93a0efe8, 2019-06-27 (annotated)

Committer:
Jeonghoon
Date:
Thu Jun 27 04:58:29 2019 +0000
Revision:
7:13dd93a0efe8
combine motor with ROS

Who changed what in which revision?

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