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

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