nucho / Mbed 2 deprecated rosserial_mbed

This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial. This program contains an example.

Dependencies:   rosserial_mbed_lib mbed Servo

ros/node_handle.h@3:dff241b66f84, 2011-11-12 (annotated)

Committer:
nucho
Date:
Sat Nov 12 23:53:04 2011 +0000
Revision:
3:dff241b66f84
Parent:
1:098e75fd5ad2 
This program supported the revision of 167 of rosserial.

Who changed what in which revision?

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