Gary Servin / rosserial_mbed_lib

Personal fork

Dependencies:   MODSERIAL

Dependents:   rosserial_mbed

Fork of rosserial_mbed_lib by nucho

ros/node_handle.h@5:19c5437ed9fe, 2014-05-01 (annotated)

Committer:
garyservin
Date:
Thu May 01 06:01:31 2014 +0000
Revision:
5:19c5437ed9fe
Parent:
4:684f39d0c346 
Updated to hydro

Who changed what in which revision?

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