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@1:ff0ec969dad1, 2011-10-16 (annotated)

Committer:
nucho
Date:
Sun Oct 16 07:19:36 2011 +0000
Revision:
1:ff0ec969dad1
Parent:
0:77afd7560544
Child:
3:1cf99502f396
This program supported the revision of 143 of rosserial.

And the bug fix of receive of array data.

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 0:77afd7560544 38 #include "../std_msgs/Time.h"
nucho 0:77afd7560544 39 #include "../rosserial_msgs/TopicInfo.h"
nucho 0:77afd7560544 40 #include "../rosserial_msgs/Log.h"
nucho 0:77afd7560544 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 0:77afd7560544 56 #include "node_output.h"
nucho 0:77afd7560544 57
nucho 0:77afd7560544 58 #include "publisher.h"
nucho 0:77afd7560544 59 #include "msg_receiver.h"
nucho 0:77afd7560544 60 #include "subscriber.h"
nucho 0:77afd7560544 61 #include "rosserial_ids.h"
nucho 0:77afd7560544 62 #include "service_server.h"
nucho 0:77afd7560544 63
nucho 0:77afd7560544 64 namespace ros {
nucho 0:77afd7560544 65
nucho 0:77afd7560544 66 using rosserial_msgs::TopicInfo;
nucho 0:77afd7560544 67
nucho 0:77afd7560544 68 /* Node Handle */
nucho 1:ff0ec969dad1 69 template<class Hardware,
nucho 1:ff0ec969dad1 70 int MAX_SUBSCRIBERS=25,
nucho 1:ff0ec969dad1 71 int MAX_PUBLISHERS=25,
nucho 1:ff0ec969dad1 72 int INPUT_SIZE=512,
nucho 1:ff0ec969dad1 73 int OUTPUT_SIZE=512>
nucho 0:77afd7560544 74 class NodeHandle_ {
nucho 0:77afd7560544 75 protected:
nucho 0:77afd7560544 76 Hardware hardware_;
nucho 0:77afd7560544 77 NodeOutput<Hardware, OUTPUT_SIZE> no_;
nucho 0:77afd7560544 78
nucho 0:77afd7560544 79 /* time used for syncing */
nucho 0:77afd7560544 80 unsigned long rt_time;
nucho 0:77afd7560544 81
nucho 0:77afd7560544 82 /* used for computing current time */
nucho 0:77afd7560544 83 unsigned long sec_offset, nsec_offset;
nucho 0:77afd7560544 84
nucho 0:77afd7560544 85 unsigned char message_in[INPUT_SIZE];
nucho 0:77afd7560544 86
nucho 0:77afd7560544 87 Publisher * publishers[MAX_PUBLISHERS];
nucho 0:77afd7560544 88 MsgReceiver * receivers[MAX_SUBSCRIBERS];
nucho 0:77afd7560544 89
nucho 1:ff0ec969dad1 90 /*
nucho 1:ff0ec969dad1 91 * Setup Functions
nucho 0:77afd7560544 92 */
nucho 0:77afd7560544 93 public:
nucho 1:ff0ec969dad1 94 NodeHandle_() : no_(&hardware_) {}
nucho 0:77afd7560544 95
nucho 0:77afd7560544 96 Hardware* getHardware() {
nucho 0:77afd7560544 97 return &hardware_;
nucho 0:77afd7560544 98 }
nucho 0:77afd7560544 99
nucho 0:77afd7560544 100 /* Start serial, initialize buffers */
nucho 0:77afd7560544 101 void initNode() {
nucho 0:77afd7560544 102 hardware_.init();
nucho 0:77afd7560544 103 mode_ = 0;
nucho 0:77afd7560544 104 bytes_ = 0;
nucho 0:77afd7560544 105 index_ = 0;
nucho 0:77afd7560544 106 topic_ = 0;
nucho 1:ff0ec969dad1 107 checksum_=0;
nucho 1:ff0ec969dad1 108
nucho 0:77afd7560544 109 total_receivers=0;
nucho 0:77afd7560544 110 };
nucho 0:77afd7560544 111
nucho 0:77afd7560544 112 protected:
nucho 0:77afd7560544 113 //State machine variables for spinOnce
nucho 0:77afd7560544 114 int mode_;
nucho 0:77afd7560544 115 int bytes_;
nucho 0:77afd7560544 116 int topic_;
nucho 0:77afd7560544 117 int index_;
nucho 0:77afd7560544 118 int checksum_;
nucho 0:77afd7560544 119
nucho 0:77afd7560544 120 int total_receivers;
nucho 0:77afd7560544 121
nucho 0:77afd7560544 122 /* used for syncing the time */
nucho 0:77afd7560544 123 unsigned long last_sync_time;
nucho 0:77afd7560544 124 unsigned long last_sync_receive_time;
nucho 0:77afd7560544 125 unsigned long last_msg_timeout_time;
nucho 0:77afd7560544 126
nucho 0:77afd7560544 127 bool registerReceiver(MsgReceiver* rcv) {
nucho 1:ff0ec969dad1 128 if (total_receivers >= MAX_SUBSCRIBERS)
nucho 1:ff0ec969dad1 129 return false;
nucho 0:77afd7560544 130 receivers[total_receivers] = rcv;
nucho 0:77afd7560544 131 rcv->id_ = 100+total_receivers;
nucho 0:77afd7560544 132 total_receivers++;
nucho 0:77afd7560544 133 return true;
nucho 0:77afd7560544 134 }
nucho 0:77afd7560544 135
nucho 0:77afd7560544 136 public:
nucho 0:77afd7560544 137 /* This function goes in your loop() function, it handles
nucho 0:77afd7560544 138 * serial input and callbacks for subscribers.
nucho 0:77afd7560544 139 */
nucho 0:77afd7560544 140
nucho 0:77afd7560544 141 virtual void spinOnce() {
nucho 0:77afd7560544 142
nucho 1:ff0ec969dad1 143 /* restart if timed out */
nucho 0:77afd7560544 144 unsigned long c_time = hardware_.time();
nucho 1:ff0ec969dad1 145 if ( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ) {
nucho 0:77afd7560544 146 no_.setConfigured(false);
nucho 0:77afd7560544 147 }
nucho 0:77afd7560544 148
nucho 1:ff0ec969dad1 149 /* reset if message has timed out */
nucho 1:ff0ec969dad1 150 if ( mode_ != MODE_FIRST_FF) {
nucho 0:77afd7560544 151 if (c_time > last_msg_timeout_time) {
nucho 0:77afd7560544 152 mode_ = MODE_FIRST_FF;
nucho 0:77afd7560544 153 }
nucho 0:77afd7560544 154 }
nucho 0:77afd7560544 155
nucho 0:77afd7560544 156 /* while available buffer, read data */
nucho 0:77afd7560544 157 while ( true ) {
nucho 1:ff0ec969dad1 158 int data = hardware_.read();
nucho 1:ff0ec969dad1 159 if ( data < 0 )//no data
nucho 0:77afd7560544 160 break;
nucho 0:77afd7560544 161 checksum_ += data;
nucho 0:77afd7560544 162 if ( mode_ == MODE_MESSAGE ) { /* message data being recieved */
nucho 0:77afd7560544 163 message_in[index_++] = data;
nucho 0:77afd7560544 164 bytes_--;
nucho 0:77afd7560544 165 if (bytes_ == 0) /* is message complete? if so, checksum */
nucho 0:77afd7560544 166 mode_ = MODE_CHECKSUM;
nucho 0:77afd7560544 167 } else if ( mode_ == MODE_FIRST_FF ) {
nucho 0:77afd7560544 168 if (data == 0xff) {
nucho 0:77afd7560544 169 mode_++;
nucho 0:77afd7560544 170 last_msg_timeout_time = c_time + MSG_TIMEOUT;
nucho 0:77afd7560544 171 }
nucho 0:77afd7560544 172 } else if ( mode_ == MODE_SECOND_FF ) {
nucho 0:77afd7560544 173 if (data == 0xff) {
nucho 0:77afd7560544 174 mode_++;
nucho 0:77afd7560544 175 } else {
nucho 0:77afd7560544 176 mode_ = MODE_FIRST_FF;
nucho 0:77afd7560544 177 }
nucho 0:77afd7560544 178 } else if ( mode_ == MODE_TOPIC_L ) { /* bottom half of topic id */
nucho 0:77afd7560544 179 topic_ = data;
nucho 0:77afd7560544 180 mode_++;
nucho 0:77afd7560544 181 checksum_ = data; /* first byte included in checksum */
nucho 0:77afd7560544 182 } else if ( mode_ == MODE_TOPIC_H ) { /* top half of topic id */
nucho 0:77afd7560544 183 topic_ += data<<8;
nucho 0:77afd7560544 184 mode_++;
nucho 0:77afd7560544 185 } else if ( mode_ == MODE_SIZE_L ) { /* bottom half of message size */
nucho 0:77afd7560544 186 bytes_ = data;
nucho 0:77afd7560544 187 index_ = 0;
nucho 0:77afd7560544 188 mode_++;
nucho 0:77afd7560544 189 } else if ( mode_ == MODE_SIZE_H ) { /* top half of message size */
nucho 0:77afd7560544 190 bytes_ += data<<8;
nucho 0:77afd7560544 191 mode_ = MODE_MESSAGE;
nucho 0:77afd7560544 192 if (bytes_ == 0)
nucho 0:77afd7560544 193 mode_ = MODE_CHECKSUM;
nucho 0:77afd7560544 194 } else if ( mode_ == MODE_CHECKSUM ) { /* do checksum */
nucho 0:77afd7560544 195 if ( (checksum_%256) == 255) {
nucho 0:77afd7560544 196 if (topic_ == TOPIC_NEGOTIATION) {
nucho 0:77afd7560544 197 requestSyncTime();
nucho 0:77afd7560544 198 negotiateTopics();
nucho 0:77afd7560544 199 last_sync_time = c_time;
nucho 0:77afd7560544 200 last_sync_receive_time = c_time;
nucho 0:77afd7560544 201 } else if (topic_ == TopicInfo::ID_TIME) {
nucho 0:77afd7560544 202 syncTime(message_in);
nucho 0:77afd7560544 203 } else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST) {
nucho 0:77afd7560544 204 req_param_resp.deserialize(message_in);
nucho 0:77afd7560544 205 param_recieved= true;
nucho 0:77afd7560544 206 } else {
nucho 0:77afd7560544 207 if (receivers[topic_-100])
nucho 0:77afd7560544 208 receivers[topic_-100]->receive( message_in );
nucho 0:77afd7560544 209 }
nucho 0:77afd7560544 210 }
nucho 0:77afd7560544 211 mode_ = MODE_FIRST_FF;
nucho 0:77afd7560544 212 }
nucho 0:77afd7560544 213 }
nucho 0:77afd7560544 214
nucho 0:77afd7560544 215 /* occasionally sync time */
nucho 0:77afd7560544 216 if ( no_.configured() && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )) {
nucho 0:77afd7560544 217 requestSyncTime();
nucho 0:77afd7560544 218 last_sync_time = c_time;
nucho 0:77afd7560544 219 }
nucho 0:77afd7560544 220 }
nucho 0:77afd7560544 221
nucho 0:77afd7560544 222 /* Are we connected to the PC? */
nucho 0:77afd7560544 223 bool connected() {
nucho 0:77afd7560544 224 return no_.configured();
nucho 0:77afd7560544 225 };
nucho 0:77afd7560544 226
nucho 1:ff0ec969dad1 227 /*
nucho 0:77afd7560544 228 * Time functions
nucho 1:ff0ec969dad1 229 */
nucho 0:77afd7560544 230
nucho 0:77afd7560544 231 void requestSyncTime() {
nucho 0:77afd7560544 232 std_msgs::Time t;
nucho 0:77afd7560544 233 no_.publish( rosserial_msgs::TopicInfo::ID_TIME, &t);
nucho 0:77afd7560544 234 rt_time = hardware_.time();
nucho 0:77afd7560544 235 }
nucho 0:77afd7560544 236
nucho 0:77afd7560544 237 void syncTime( unsigned char * data ) {
nucho 0:77afd7560544 238 std_msgs::Time t;
nucho 0:77afd7560544 239 unsigned long offset = hardware_.time() - rt_time;
nucho 0:77afd7560544 240
nucho 0:77afd7560544 241 t.deserialize(data);
nucho 0:77afd7560544 242
nucho 0:77afd7560544 243 t.data.sec += offset/1000;
nucho 0:77afd7560544 244 t.data.nsec += (offset%1000)*1000000UL;
nucho 0:77afd7560544 245
nucho 0:77afd7560544 246 this->setNow(t.data);
nucho 0:77afd7560544 247 last_sync_receive_time = hardware_.time();
nucho 0:77afd7560544 248 }
nucho 0:77afd7560544 249
nucho 0:77afd7560544 250 Time now() {
nucho 0:77afd7560544 251 unsigned long ms = hardware_.time();
nucho 0:77afd7560544 252 Time current_time;
nucho 0:77afd7560544 253 current_time.sec = ms/1000 + sec_offset;
nucho 0:77afd7560544 254 current_time.nsec = (ms%1000)*1000000UL + nsec_offset;
nucho 0:77afd7560544 255 normalizeSecNSec(current_time.sec, current_time.nsec);
nucho 0:77afd7560544 256 return current_time;
nucho 0:77afd7560544 257 }
nucho 0:77afd7560544 258
nucho 0:77afd7560544 259 void setNow( Time & new_now ) {
nucho 0:77afd7560544 260 unsigned long ms = hardware_.time();
nucho 0:77afd7560544 261 sec_offset = new_now.sec - ms/1000 - 1;
nucho 0:77afd7560544 262 nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
nucho 0:77afd7560544 263 normalizeSecNSec(sec_offset, nsec_offset);
nucho 0:77afd7560544 264 }
nucho 0:77afd7560544 265
nucho 1:ff0ec969dad1 266 /*
nucho 1:ff0ec969dad1 267 * Registration
nucho 1:ff0ec969dad1 268 */
nucho 0:77afd7560544 269
nucho 0:77afd7560544 270 bool advertise(Publisher & p) {
nucho 0:77afd7560544 271 int i;
nucho 0:77afd7560544 272 for (i = 0; i < MAX_PUBLISHERS; i++) {
nucho 0:77afd7560544 273 if (publishers[i] == 0) { // empty slot
nucho 0:77afd7560544 274 publishers[i] = &p;
nucho 0:77afd7560544 275 p.id_ = i+100+MAX_SUBSCRIBERS;
nucho 0:77afd7560544 276 p.no_ = &this->no_;
nucho 0:77afd7560544 277 return true;
nucho 0:77afd7560544 278 }
nucho 0:77afd7560544 279 }
nucho 0:77afd7560544 280 return false;
nucho 0:77afd7560544 281 }
nucho 0:77afd7560544 282
nucho 0:77afd7560544 283 /* Register a subscriber with the node */
nucho 0:77afd7560544 284 template<typename MsgT>
nucho 0:77afd7560544 285 bool subscribe(Subscriber< MsgT> &s) {
nucho 0:77afd7560544 286 return registerReceiver((MsgReceiver*) &s);
nucho 0:77afd7560544 287 }
nucho 0:77afd7560544 288
nucho 0:77afd7560544 289 template<typename SrvReq, typename SrvResp>
nucho 0:77afd7560544 290 bool advertiseService(ServiceServer<SrvReq,SrvResp>& srv) {
nucho 0:77afd7560544 291 srv.no_ = &no_;
nucho 0:77afd7560544 292 return registerReceiver((MsgReceiver*) &srv);
nucho 0:77afd7560544 293 }
nucho 0:77afd7560544 294
nucho 0:77afd7560544 295 void negotiateTopics() {
nucho 0:77afd7560544 296 no_.setConfigured(true);
nucho 0:77afd7560544 297 rosserial_msgs::TopicInfo ti;
nucho 0:77afd7560544 298 int i;
nucho 0:77afd7560544 299 for (i = 0; i < MAX_PUBLISHERS; i++) {
nucho 0:77afd7560544 300 if (publishers[i] != 0) { // non-empty slot
nucho 0:77afd7560544 301 ti.topic_id = publishers[i]->id_;
nucho 0:77afd7560544 302 ti.topic_name = (char *) publishers[i]->topic_;
nucho 0:77afd7560544 303 ti.message_type = (char *) publishers[i]->msg_->getType();
nucho 0:77afd7560544 304 no_.publish( TOPIC_PUBLISHERS, &ti );
nucho 0:77afd7560544 305 }
nucho 0:77afd7560544 306 }
nucho 0:77afd7560544 307 for (i = 0; i < MAX_SUBSCRIBERS; i++) {
nucho 0:77afd7560544 308 if (receivers[i] != 0) { // non-empty slot
nucho 0:77afd7560544 309 ti.topic_id = receivers[i]->id_;
nucho 0:77afd7560544 310 ti.topic_name = (char *) receivers[i]->topic_;
nucho 0:77afd7560544 311 ti.message_type = (char *) receivers[i]->getMsgType();
nucho 0:77afd7560544 312 no_.publish( TOPIC_SUBSCRIBERS, &ti );
nucho 0:77afd7560544 313 }
nucho 0:77afd7560544 314 }
nucho 0:77afd7560544 315 }
nucho 0:77afd7560544 316
nucho 0:77afd7560544 317 /*
nucho 0:77afd7560544 318 * Logging
nucho 0:77afd7560544 319 */
nucho 1:ff0ec969dad1 320
nucho 0:77afd7560544 321 private:
nucho 0:77afd7560544 322 void log(char byte, const char * msg) {
nucho 0:77afd7560544 323 rosserial_msgs::Log l;
nucho 0:77afd7560544 324 l.level= byte;
nucho 0:77afd7560544 325 l.msg = (char*)msg;
nucho 0:77afd7560544 326 this->no_.publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
nucho 0:77afd7560544 327 }
nucho 1:ff0ec969dad1 328
nucho 0:77afd7560544 329 public:
nucho 0:77afd7560544 330 void logdebug(const char* msg) {
nucho 0:77afd7560544 331 log(rosserial_msgs::Log::DEBUG, msg);
nucho 0:77afd7560544 332 }
nucho 0:77afd7560544 333 void loginfo(const char * msg) {
nucho 0:77afd7560544 334 log(rosserial_msgs::Log::INFO, msg);
nucho 0:77afd7560544 335 }
nucho 0:77afd7560544 336 void logwarn(const char *msg) {
nucho 0:77afd7560544 337 log(rosserial_msgs::Log::WARN, msg);
nucho 0:77afd7560544 338 }
nucho 0:77afd7560544 339 void logerror(const char*msg) {
nucho 0:77afd7560544 340 log(rosserial_msgs::Log::ERROR, msg);
nucho 0:77afd7560544 341 }
nucho 0:77afd7560544 342 void logfatal(const char*msg) {
nucho 0:77afd7560544 343 log(rosserial_msgs::Log::FATAL, msg);
nucho 0:77afd7560544 344 }
nucho 0:77afd7560544 345
nucho 0:77afd7560544 346
nucho 1:ff0ec969dad1 347 /*
nucho 0:77afd7560544 348 * Retrieve Parameters
nucho 1:ff0ec969dad1 349 */
nucho 1:ff0ec969dad1 350
nucho 0:77afd7560544 351 private:
nucho 0:77afd7560544 352 bool param_recieved;
nucho 0:77afd7560544 353 rosserial_msgs::RequestParamResponse req_param_resp;
nucho 1:ff0ec969dad1 354
nucho 0:77afd7560544 355 bool requestParam(const char * name, int time_out = 1000) {
nucho 0:77afd7560544 356 param_recieved = false;
nucho 0:77afd7560544 357 rosserial_msgs::RequestParamRequest req;
nucho 0:77afd7560544 358 req.name = (char*)name;
nucho 0:77afd7560544 359 no_.publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
nucho 0:77afd7560544 360 int end_time = hardware_.time();
nucho 0:77afd7560544 361 while (!param_recieved ) {
nucho 0:77afd7560544 362 spinOnce();
nucho 0:77afd7560544 363 if (end_time > hardware_.time()) return false;
nucho 0:77afd7560544 364 }
nucho 0:77afd7560544 365 return true;
nucho 0:77afd7560544 366 }
nucho 1:ff0ec969dad1 367
nucho 0:77afd7560544 368 public:
nucho 0:77afd7560544 369 bool getParam(const char* name, int* param, int length =1) {
nucho 0:77afd7560544 370 if (requestParam(name) ) {
nucho 0:77afd7560544 371 if (length == req_param_resp.ints_length) {
nucho 0:77afd7560544 372 //copy it over
nucho 1:ff0ec969dad1 373 for (int i=0; i<length; i++)
nucho 1:ff0ec969dad1 374 param[i] = req_param_resp.ints[i];
nucho 0:77afd7560544 375 return true;
nucho 0:77afd7560544 376 }
nucho 0:77afd7560544 377 }
nucho 0:77afd7560544 378 return false;
nucho 0:77afd7560544 379 }
nucho 0:77afd7560544 380 bool getParam(const char* name, float* param, int length=1) {
nucho 0:77afd7560544 381 if (requestParam(name) ) {
nucho 0:77afd7560544 382 if (length == req_param_resp.floats_length) {
nucho 0:77afd7560544 383 //copy it over
nucho 1:ff0ec969dad1 384 for (int i=0; i<length; i++)
nucho 1:ff0ec969dad1 385 param[i] = req_param_resp.floats[i];
nucho 0:77afd7560544 386 return true;
nucho 0:77afd7560544 387 }
nucho 0:77afd7560544 388 }
nucho 0:77afd7560544 389 return false;
nucho 0:77afd7560544 390 }
nucho 0:77afd7560544 391 bool getParam(const char* name, char** param, int length=1) {
nucho 0:77afd7560544 392 if (requestParam(name) ) {
nucho 0:77afd7560544 393 if (length == req_param_resp.strings_length) {
nucho 0:77afd7560544 394 //copy it over
nucho 1:ff0ec969dad1 395 for (int i=0; i<length; i++)
nucho 1:ff0ec969dad1 396 strcpy(param[i],req_param_resp.strings[i]);
nucho 0:77afd7560544 397 return true;
nucho 0:77afd7560544 398 }
nucho 0:77afd7560544 399 }
nucho 0:77afd7560544 400 return false;
nucho 0:77afd7560544 401 }
nucho 0:77afd7560544 402 };
nucho 0:77afd7560544 403
nucho 0:77afd7560544 404 }
nucho 0:77afd7560544 405
nucho 1:ff0ec969dad1 406 #endif