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