nucho / rosserial_mbed_lib

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

Dependencies:  

Dependents:   rosserial_mbed robot_S2

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

Committer:
nucho
Date:
Sat Nov 12 23:54:45 2011 +0000
Revision:
3:1cf99502f396
Parent:
1:ff0ec969dad1
Child:
4:684f39d0c346
This program supported the revision of 167 of rosserial.

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
nucho 0:77afd7560544 46 #define MODE_SECOND_FF 1
nucho 0:77afd7560544 47 #define MODE_TOPIC_L 2 // waiting for topic id
nucho 0:77afd7560544 48 #define MODE_TOPIC_H 3
nucho 0:77afd7560544 49 #define MODE_SIZE_L 4 // waiting for message size
nucho 0:77afd7560544 50 #define MODE_SIZE_H 5
nucho 0:77afd7560544 51 #define MODE_MESSAGE 6
nucho 0:77afd7560544 52 #define MODE_CHECKSUM 7
nucho 0:77afd7560544 53
nucho 0:77afd7560544 54 #define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data
nucho 0:77afd7560544 55
nucho 3:1cf99502f396 56 #include "msg.h"
nucho 3:1cf99502f396 57
nucho 3:1cf99502f396 58 namespace ros {
nucho 3:1cf99502f396 59
nucho 3:1cf99502f396 60 class NodeHandleBase_ {
nucho 3:1cf99502f396 61 public:
nucho 3:1cf99502f396 62 virtual int publish(int16_t id, const Msg* msg)=0;
nucho 3:1cf99502f396 63 virtual int spinOnce()=0;
nucho 3:1cf99502f396 64 virtual bool connected()=0;
nucho 3:1cf99502f396 65 };
nucho 3:1cf99502f396 66
nucho 3:1cf99502f396 67 }
nucho 0:77afd7560544 68
nucho 0:77afd7560544 69 #include "publisher.h"
nucho 0:77afd7560544 70 #include "subscriber.h"
nucho 0:77afd7560544 71 #include "service_server.h"
nucho 3:1cf99502f396 72 #include "service_client.h"
nucho 3:1cf99502f396 73
nucho 0:77afd7560544 74
nucho 0:77afd7560544 75 namespace ros {
nucho 0:77afd7560544 76
nucho 0:77afd7560544 77 using rosserial_msgs::TopicInfo;
nucho 0:77afd7560544 78
nucho 0:77afd7560544 79 /* Node Handle */
nucho 1:ff0ec969dad1 80 template<class Hardware,
nucho 1:ff0ec969dad1 81 int MAX_SUBSCRIBERS=25,
nucho 1:ff0ec969dad1 82 int MAX_PUBLISHERS=25,
nucho 1:ff0ec969dad1 83 int INPUT_SIZE=512,
nucho 1:ff0ec969dad1 84 int OUTPUT_SIZE=512>
nucho 3:1cf99502f396 85 class NodeHandle_ : public NodeHandleBase_ {
nucho 0:77afd7560544 86 protected:
nucho 0:77afd7560544 87 Hardware hardware_;
nucho 0:77afd7560544 88
nucho 0:77afd7560544 89 /* time used for syncing */
nucho 0:77afd7560544 90 unsigned long rt_time;
nucho 0:77afd7560544 91
nucho 0:77afd7560544 92 /* used for computing current time */
nucho 0:77afd7560544 93 unsigned long sec_offset, nsec_offset;
nucho 0:77afd7560544 94
nucho 0:77afd7560544 95 unsigned char message_in[INPUT_SIZE];
nucho 3:1cf99502f396 96 unsigned char message_out[OUTPUT_SIZE];
nucho 0:77afd7560544 97
nucho 0:77afd7560544 98 Publisher * publishers[MAX_PUBLISHERS];
nucho 3:1cf99502f396 99 Subscriber_ * subscribers[MAX_SUBSCRIBERS];
nucho 0:77afd7560544 100
nucho 1:ff0ec969dad1 101 /*
nucho 1:ff0ec969dad1 102 * Setup Functions
nucho 0:77afd7560544 103 */
nucho 0:77afd7560544 104 public:
nucho 3:1cf99502f396 105 NodeHandle_() : configured_(false) {}
nucho 0:77afd7560544 106
nucho 0:77afd7560544 107 Hardware* getHardware() {
nucho 0:77afd7560544 108 return &hardware_;
nucho 0:77afd7560544 109 }
nucho 0:77afd7560544 110
nucho 0:77afd7560544 111 /* Start serial, initialize buffers */
nucho 0:77afd7560544 112 void initNode() {
nucho 0:77afd7560544 113 hardware_.init();
nucho 0:77afd7560544 114 mode_ = 0;
nucho 0:77afd7560544 115 bytes_ = 0;
nucho 0:77afd7560544 116 index_ = 0;
nucho 0:77afd7560544 117 topic_ = 0;
nucho 1:ff0ec969dad1 118 checksum_=0;
nucho 0:77afd7560544 119 };
nucho 0:77afd7560544 120
nucho 0:77afd7560544 121 protected:
nucho 3:1cf99502f396 122 //State machine variables for spinOnce
nucho 0:77afd7560544 123 int mode_;
nucho 0:77afd7560544 124 int bytes_;
nucho 0:77afd7560544 125 int topic_;
nucho 0:77afd7560544 126 int index_;
nucho 0:77afd7560544 127 int checksum_;
nucho 0:77afd7560544 128
nucho 3:1cf99502f396 129 bool configured_;
nucho 3:1cf99502f396 130
nucho 0:77afd7560544 131 int total_receivers;
nucho 0:77afd7560544 132
nucho 0:77afd7560544 133 /* used for syncing the time */
nucho 0:77afd7560544 134 unsigned long last_sync_time;
nucho 0:77afd7560544 135 unsigned long last_sync_receive_time;
nucho 0:77afd7560544 136 unsigned long last_msg_timeout_time;
nucho 0:77afd7560544 137
nucho 0:77afd7560544 138 public:
nucho 0:77afd7560544 139 /* This function goes in your loop() function, it handles
nucho 0:77afd7560544 140 * serial input and callbacks for subscribers.
nucho 0:77afd7560544 141 */
nucho 0:77afd7560544 142
nucho 3:1cf99502f396 143 virtual int spinOnce() {
nucho 0:77afd7560544 144
nucho 1:ff0ec969dad1 145 /* restart if timed out */
nucho 0:77afd7560544 146 unsigned long c_time = hardware_.time();
nucho 1:ff0ec969dad1 147 if ( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ) {
nucho 3:1cf99502f396 148 configured_ = false;
nucho 0:77afd7560544 149 }
nucho 0:77afd7560544 150
nucho 1:ff0ec969dad1 151 /* reset if message has timed out */
nucho 1:ff0ec969dad1 152 if ( mode_ != MODE_FIRST_FF) {
nucho 0:77afd7560544 153 if (c_time > last_msg_timeout_time) {
nucho 0:77afd7560544 154 mode_ = MODE_FIRST_FF;
nucho 0:77afd7560544 155 }
nucho 0:77afd7560544 156 }
nucho 0:77afd7560544 157
nucho 0:77afd7560544 158 /* while available buffer, read data */
nucho 0:77afd7560544 159 while ( true ) {
nucho 1:ff0ec969dad1 160 int data = hardware_.read();
nucho 1:ff0ec969dad1 161 if ( data < 0 )//no data
nucho 0:77afd7560544 162 break;
nucho 0:77afd7560544 163 checksum_ += data;
nucho 0:77afd7560544 164 if ( mode_ == MODE_MESSAGE ) { /* message data being recieved */
nucho 0:77afd7560544 165 message_in[index_++] = data;
nucho 0:77afd7560544 166 bytes_--;
nucho 0:77afd7560544 167 if (bytes_ == 0) /* is message complete? if so, checksum */
nucho 0:77afd7560544 168 mode_ = MODE_CHECKSUM;
nucho 0:77afd7560544 169 } else if ( mode_ == MODE_FIRST_FF ) {
nucho 0:77afd7560544 170 if (data == 0xff) {
nucho 0:77afd7560544 171 mode_++;
nucho 0:77afd7560544 172 last_msg_timeout_time = c_time + MSG_TIMEOUT;
nucho 0:77afd7560544 173 }
nucho 0:77afd7560544 174 } else if ( mode_ == MODE_SECOND_FF ) {
nucho 0:77afd7560544 175 if (data == 0xff) {
nucho 0:77afd7560544 176 mode_++;
nucho 0:77afd7560544 177 } else {
nucho 0:77afd7560544 178 mode_ = MODE_FIRST_FF;
nucho 0:77afd7560544 179 }
nucho 0:77afd7560544 180 } else if ( mode_ == MODE_TOPIC_L ) { /* bottom half of topic id */
nucho 0:77afd7560544 181 topic_ = data;
nucho 0:77afd7560544 182 mode_++;
nucho 0:77afd7560544 183 checksum_ = data; /* first byte included in checksum */
nucho 0:77afd7560544 184 } else if ( mode_ == MODE_TOPIC_H ) { /* top half of topic id */
nucho 0:77afd7560544 185 topic_ += data<<8;
nucho 0:77afd7560544 186 mode_++;
nucho 0:77afd7560544 187 } else if ( mode_ == MODE_SIZE_L ) { /* bottom half of message size */
nucho 0:77afd7560544 188 bytes_ = data;
nucho 0:77afd7560544 189 index_ = 0;
nucho 0:77afd7560544 190 mode_++;
nucho 0:77afd7560544 191 } else if ( mode_ == MODE_SIZE_H ) { /* top half of message size */
nucho 0:77afd7560544 192 bytes_ += data<<8;
nucho 0:77afd7560544 193 mode_ = MODE_MESSAGE;
nucho 0:77afd7560544 194 if (bytes_ == 0)
nucho 0:77afd7560544 195 mode_ = MODE_CHECKSUM;
nucho 0:77afd7560544 196 } else if ( mode_ == MODE_CHECKSUM ) { /* do checksum */
nucho 3:1cf99502f396 197 mode_ = MODE_FIRST_FF;
nucho 0:77afd7560544 198 if ( (checksum_%256) == 255) {
nucho 3:1cf99502f396 199 if (topic_ == TopicInfo::ID_PUBLISHER) {
nucho 0:77afd7560544 200 requestSyncTime();
nucho 0:77afd7560544 201 negotiateTopics();
nucho 0:77afd7560544 202 last_sync_time = c_time;
nucho 0:77afd7560544 203 last_sync_receive_time = c_time;
nucho 3:1cf99502f396 204 return -1;
nucho 0:77afd7560544 205 } else if (topic_ == TopicInfo::ID_TIME) {
nucho 0:77afd7560544 206 syncTime(message_in);
nucho 0:77afd7560544 207 } else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST) {
nucho 0:77afd7560544 208 req_param_resp.deserialize(message_in);
nucho 0:77afd7560544 209 param_recieved= true;
nucho 0:77afd7560544 210 } else {
nucho 3:1cf99502f396 211 if (subscribers[topic_-100])
nucho 3:1cf99502f396 212 subscribers[topic_-100]->callback( message_in );
nucho 0:77afd7560544 213 }
nucho 0:77afd7560544 214 }
nucho 0:77afd7560544 215 }
nucho 0:77afd7560544 216 }
nucho 0:77afd7560544 217
nucho 0:77afd7560544 218 /* occasionally sync time */
nucho 3:1cf99502f396 219 if ( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )) {
nucho 0:77afd7560544 220 requestSyncTime();
nucho 0:77afd7560544 221 last_sync_time = c_time;
nucho 0:77afd7560544 222 }
nucho 3:1cf99502f396 223
nucho 3:1cf99502f396 224 return 0;
nucho 0:77afd7560544 225 }
nucho 0:77afd7560544 226
nucho 0:77afd7560544 227 /* Are we connected to the PC? */
nucho 3:1cf99502f396 228 virtual bool connected() {
nucho 3:1cf99502f396 229 return configured_;
nucho 0:77afd7560544 230 };
nucho 0:77afd7560544 231
nucho 3:1cf99502f396 232 /********************************************************************
nucho 0:77afd7560544 233 * Time functions
nucho 1:ff0ec969dad1 234 */
nucho 0:77afd7560544 235
nucho 0:77afd7560544 236 void requestSyncTime() {
nucho 0:77afd7560544 237 std_msgs::Time t;
nucho 3:1cf99502f396 238 publish(TopicInfo::ID_TIME, &t);
nucho 0:77afd7560544 239 rt_time = hardware_.time();
nucho 0:77afd7560544 240 }
nucho 0:77afd7560544 241
nucho 0:77afd7560544 242 void syncTime( unsigned char * data ) {
nucho 0:77afd7560544 243 std_msgs::Time t;
nucho 0:77afd7560544 244 unsigned long offset = hardware_.time() - rt_time;
nucho 0:77afd7560544 245
nucho 0:77afd7560544 246 t.deserialize(data);
nucho 0:77afd7560544 247
nucho 0:77afd7560544 248 t.data.sec += offset/1000;
nucho 0:77afd7560544 249 t.data.nsec += (offset%1000)*1000000UL;
nucho 0:77afd7560544 250
nucho 0:77afd7560544 251 this->setNow(t.data);
nucho 0:77afd7560544 252 last_sync_receive_time = hardware_.time();
nucho 0:77afd7560544 253 }
nucho 0:77afd7560544 254
nucho 0:77afd7560544 255 Time now() {
nucho 0:77afd7560544 256 unsigned long ms = hardware_.time();
nucho 0:77afd7560544 257 Time current_time;
nucho 0:77afd7560544 258 current_time.sec = ms/1000 + sec_offset;
nucho 0:77afd7560544 259 current_time.nsec = (ms%1000)*1000000UL + nsec_offset;
nucho 0:77afd7560544 260 normalizeSecNSec(current_time.sec, current_time.nsec);
nucho 0:77afd7560544 261 return current_time;
nucho 0:77afd7560544 262 }
nucho 0:77afd7560544 263
nucho 0:77afd7560544 264 void setNow( Time & new_now ) {
nucho 0:77afd7560544 265 unsigned long ms = hardware_.time();
nucho 0:77afd7560544 266 sec_offset = new_now.sec - ms/1000 - 1;
nucho 0:77afd7560544 267 nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
nucho 0:77afd7560544 268 normalizeSecNSec(sec_offset, nsec_offset);
nucho 0:77afd7560544 269 }
nucho 0:77afd7560544 270
nucho 3:1cf99502f396 271 /********************************************************************
nucho 3:1cf99502f396 272 * Topic Management
nucho 1:ff0ec969dad1 273 */
nucho 0:77afd7560544 274
nucho 3:1cf99502f396 275 /* Register a new publisher */
nucho 0:77afd7560544 276 bool advertise(Publisher & p) {
nucho 3:1cf99502f396 277 for (int i = 0; i < MAX_PUBLISHERS; i++) {
nucho 0:77afd7560544 278 if (publishers[i] == 0) { // empty slot
nucho 0:77afd7560544 279 publishers[i] = &p;
nucho 0:77afd7560544 280 p.id_ = i+100+MAX_SUBSCRIBERS;
nucho 3:1cf99502f396 281 p.nh_ = this;
nucho 3:1cf99502f396 282 return true;
nucho 3:1cf99502f396 283 }
nucho 3:1cf99502f396 284 }
nucho 3:1cf99502f396 285 return false;
nucho 3:1cf99502f396 286 }
nucho 3:1cf99502f396 287
nucho 3:1cf99502f396 288 /* Register a new subscriber */
nucho 3:1cf99502f396 289 template<typename MsgT>
nucho 3:1cf99502f396 290 bool subscribe(Subscriber< MsgT> & s) {
nucho 3:1cf99502f396 291 for (int i = 0; i < MAX_SUBSCRIBERS; i++) {
nucho 3:1cf99502f396 292 if (subscribers[i] == 0) { // empty slot
nucho 3:1cf99502f396 293 subscribers[i] = (Subscriber_*) &s;
nucho 3:1cf99502f396 294 s.id_ = i+100;
nucho 0:77afd7560544 295 return true;
nucho 0:77afd7560544 296 }
nucho 0:77afd7560544 297 }
nucho 0:77afd7560544 298 return false;
nucho 0:77afd7560544 299 }
nucho 0:77afd7560544 300
nucho 3:1cf99502f396 301 /* Register a new Service Server */
nucho 3:1cf99502f396 302 template<typename MReq, typename MRes>
nucho 3:1cf99502f396 303 bool advertiseService(ServiceServer<MReq,MRes>& srv) {
nucho 3:1cf99502f396 304 bool v = advertise(srv.pub);
nucho 3:1cf99502f396 305 for (int i = 0; i < MAX_SUBSCRIBERS; i++) {
nucho 3:1cf99502f396 306 if (subscribers[i] == 0) { // empty slot
nucho 3:1cf99502f396 307 subscribers[i] = (Subscriber_*) &srv;
nucho 3:1cf99502f396 308 srv.id_ = i+100;
nucho 3:1cf99502f396 309 return v;
nucho 3:1cf99502f396 310 }
nucho 3:1cf99502f396 311 }
nucho 3:1cf99502f396 312 return false;
nucho 0:77afd7560544 313 }
nucho 0:77afd7560544 314
nucho 3:1cf99502f396 315 /* Register a new Service Client */
nucho 3:1cf99502f396 316 template<typename MReq, typename MRes>
nucho 3:1cf99502f396 317 bool serviceClient(ServiceClient<MReq, MRes>& srv) {
nucho 3:1cf99502f396 318 bool v = advertise(srv.pub);
nucho 3:1cf99502f396 319 for (int i = 0; i < MAX_SUBSCRIBERS; i++) {
nucho 3:1cf99502f396 320 if (subscribers[i] == 0) { // empty slot
nucho 3:1cf99502f396 321 subscribers[i] = (Subscriber_*) &srv;
nucho 3:1cf99502f396 322 srv.id_ = i+100;
nucho 3:1cf99502f396 323 return v;
nucho 3:1cf99502f396 324 }
nucho 3:1cf99502f396 325 }
nucho 3:1cf99502f396 326 return false;
nucho 0:77afd7560544 327 }
nucho 0:77afd7560544 328
nucho 0:77afd7560544 329 void negotiateTopics() {
nucho 3:1cf99502f396 330 configured_ = true;
nucho 0:77afd7560544 331 rosserial_msgs::TopicInfo ti;
nucho 0:77afd7560544 332 int i;
nucho 0:77afd7560544 333 for (i = 0; i < MAX_PUBLISHERS; i++) {
nucho 0:77afd7560544 334 if (publishers[i] != 0) { // non-empty slot
nucho 0:77afd7560544 335 ti.topic_id = publishers[i]->id_;
nucho 0:77afd7560544 336 ti.topic_name = (char *) publishers[i]->topic_;
nucho 0:77afd7560544 337 ti.message_type = (char *) publishers[i]->msg_->getType();
nucho 3:1cf99502f396 338 ti.md5sum = (char *) publishers[i]->msg_->getMD5();
nucho 3:1cf99502f396 339 ti.buffer_size = OUTPUT_SIZE;
nucho 3:1cf99502f396 340
nucho 3:1cf99502f396 341 publish( publishers[i]->getEndpointType(), &ti );
nucho 0:77afd7560544 342 }
nucho 0:77afd7560544 343 }
nucho 0:77afd7560544 344 for (i = 0; i < MAX_SUBSCRIBERS; i++) {
nucho 3:1cf99502f396 345 if (subscribers[i] != 0) { // non-empty slot
nucho 3:1cf99502f396 346 ti.topic_id = subscribers[i]->id_;
nucho 3:1cf99502f396 347 ti.topic_name = (char *) subscribers[i]->topic_;
nucho 3:1cf99502f396 348 ti.message_type = (char *) subscribers[i]->getMsgType();
nucho 3:1cf99502f396 349 ti.md5sum = (char *) subscribers[i]->getMsgMD5();
nucho 3:1cf99502f396 350 ti.buffer_size = INPUT_SIZE;
nucho 3:1cf99502f396 351 publish( subscribers[i]->getEndpointType(), &ti );
nucho 0:77afd7560544 352 }
nucho 0:77afd7560544 353 }
nucho 0:77afd7560544 354 }
nucho 0:77afd7560544 355
nucho 3:1cf99502f396 356 virtual int publish(int16_t id, const Msg * msg)
nucho 3:1cf99502f396 357 {
nucho 3:1cf99502f396 358 if (!configured_) return 0;
nucho 3:1cf99502f396 359
nucho 3:1cf99502f396 360 /* serialize message */
nucho 3:1cf99502f396 361 int16_t l = msg->serialize(message_out+6);
nucho 3:1cf99502f396 362
nucho 3:1cf99502f396 363 /* setup the header */
nucho 3:1cf99502f396 364 message_out[0] = 0xff;
nucho 3:1cf99502f396 365 message_out[1] = 0xff;
nucho 3:1cf99502f396 366 message_out[2] = (unsigned char) id&255;
nucho 3:1cf99502f396 367 message_out[3] = (unsigned char) id>>8;
nucho 3:1cf99502f396 368 message_out[4] = (unsigned char) l&255;
nucho 3:1cf99502f396 369 message_out[5] = ((unsigned char) l>>8);
nucho 3:1cf99502f396 370
nucho 3:1cf99502f396 371 /* calculate checksum */
nucho 3:1cf99502f396 372 int chk = 0;
nucho 3:1cf99502f396 373 for (int i =2; i<l+6; i++)
nucho 3:1cf99502f396 374 chk += message_out[i];
nucho 3:1cf99502f396 375 l += 6;
nucho 3:1cf99502f396 376 message_out[l++] = 255 - (chk%256);
nucho 3:1cf99502f396 377
nucho 3:1cf99502f396 378 if ( l <= OUTPUT_SIZE ) {
nucho 3:1cf99502f396 379 hardware_.write(message_out, l);
nucho 3:1cf99502f396 380 return l;
nucho 3:1cf99502f396 381 } else {
nucho 3:1cf99502f396 382 logerror("Message from device dropped: message larger than buffer.");
nucho 3:1cf99502f396 383 return 1;
nucho 3:1cf99502f396 384 }
nucho 3:1cf99502f396 385
nucho 3:1cf99502f396 386 }
nucho 3:1cf99502f396 387
nucho 3:1cf99502f396 388 /********************************************************************
nucho 0:77afd7560544 389 * Logging
nucho 0:77afd7560544 390 */
nucho 1:ff0ec969dad1 391
nucho 0:77afd7560544 392 private:
nucho 0:77afd7560544 393 void log(char byte, const char * msg) {
nucho 0:77afd7560544 394 rosserial_msgs::Log l;
nucho 0:77afd7560544 395 l.level= byte;
nucho 0:77afd7560544 396 l.msg = (char*)msg;
nucho 3:1cf99502f396 397 publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
nucho 0:77afd7560544 398 }
nucho 1:ff0ec969dad1 399
nucho 0:77afd7560544 400 public:
nucho 0:77afd7560544 401 void logdebug(const char* msg) {
nucho 0:77afd7560544 402 log(rosserial_msgs::Log::DEBUG, msg);
nucho 0:77afd7560544 403 }
nucho 0:77afd7560544 404 void loginfo(const char * msg) {
nucho 0:77afd7560544 405 log(rosserial_msgs::Log::INFO, msg);
nucho 0:77afd7560544 406 }
nucho 0:77afd7560544 407 void logwarn(const char *msg) {
nucho 0:77afd7560544 408 log(rosserial_msgs::Log::WARN, msg);
nucho 0:77afd7560544 409 }
nucho 0:77afd7560544 410 void logerror(const char*msg) {
nucho 0:77afd7560544 411 log(rosserial_msgs::Log::ERROR, msg);
nucho 0:77afd7560544 412 }
nucho 0:77afd7560544 413 void logfatal(const char*msg) {
nucho 0:77afd7560544 414 log(rosserial_msgs::Log::FATAL, msg);
nucho 0:77afd7560544 415 }
nucho 0:77afd7560544 416
nucho 0:77afd7560544 417
nucho 3:1cf99502f396 418 /********************************************************************
nucho 3:1cf99502f396 419 * Parameters
nucho 1:ff0ec969dad1 420 */
nucho 1:ff0ec969dad1 421
nucho 0:77afd7560544 422 private:
nucho 0:77afd7560544 423 bool param_recieved;
nucho 0:77afd7560544 424 rosserial_msgs::RequestParamResponse req_param_resp;
nucho 1:ff0ec969dad1 425
nucho 0:77afd7560544 426 bool requestParam(const char * name, int time_out = 1000) {
nucho 0:77afd7560544 427 param_recieved = false;
nucho 0:77afd7560544 428 rosserial_msgs::RequestParamRequest req;
nucho 0:77afd7560544 429 req.name = (char*)name;
nucho 3:1cf99502f396 430 publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
nucho 0:77afd7560544 431 int end_time = hardware_.time();
nucho 0:77afd7560544 432 while (!param_recieved ) {
nucho 0:77afd7560544 433 spinOnce();
nucho 0:77afd7560544 434 if (end_time > hardware_.time()) return false;
nucho 0:77afd7560544 435 }
nucho 0:77afd7560544 436 return true;
nucho 0:77afd7560544 437 }
nucho 1:ff0ec969dad1 438
nucho 0:77afd7560544 439 public:
nucho 0:77afd7560544 440 bool getParam(const char* name, int* param, int length =1) {
nucho 0:77afd7560544 441 if (requestParam(name) ) {
nucho 0:77afd7560544 442 if (length == req_param_resp.ints_length) {
nucho 0:77afd7560544 443 //copy it over
nucho 1:ff0ec969dad1 444 for (int i=0; i<length; i++)
nucho 1:ff0ec969dad1 445 param[i] = req_param_resp.ints[i];
nucho 0:77afd7560544 446 return true;
nucho 0:77afd7560544 447 }
nucho 0:77afd7560544 448 }
nucho 0:77afd7560544 449 return false;
nucho 0:77afd7560544 450 }
nucho 0:77afd7560544 451 bool getParam(const char* name, float* param, int length=1) {
nucho 0:77afd7560544 452 if (requestParam(name) ) {
nucho 0:77afd7560544 453 if (length == req_param_resp.floats_length) {
nucho 0:77afd7560544 454 //copy it over
nucho 1:ff0ec969dad1 455 for (int i=0; i<length; i++)
nucho 1:ff0ec969dad1 456 param[i] = req_param_resp.floats[i];
nucho 0:77afd7560544 457 return true;
nucho 0:77afd7560544 458 }
nucho 0:77afd7560544 459 }
nucho 0:77afd7560544 460 return false;
nucho 0:77afd7560544 461 }
nucho 0:77afd7560544 462 bool getParam(const char* name, char** param, int length=1) {
nucho 0:77afd7560544 463 if (requestParam(name) ) {
nucho 0:77afd7560544 464 if (length == req_param_resp.strings_length) {
nucho 0:77afd7560544 465 //copy it over
nucho 1:ff0ec969dad1 466 for (int i=0; i<length; i++)
nucho 1:ff0ec969dad1 467 strcpy(param[i],req_param_resp.strings[i]);
nucho 0:77afd7560544 468 return true;
nucho 0:77afd7560544 469 }
nucho 0:77afd7560544 470 }
nucho 0:77afd7560544 471 return false;
nucho 0:77afd7560544 472 }
nucho 0:77afd7560544 473 };
nucho 0:77afd7560544 474
nucho 0:77afd7560544 475 }
nucho 0:77afd7560544 476
nucho 1:ff0ec969dad1 477 #endif