Gary Servin
/
rosserial_mbed_lib
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
rosserial_mbed_lib
by 
nucho
ros/node_handle.h@4:684f39d0c346, 2012-02-29 (annotated)
- Committer:
- nucho
- Date:
- Wed Feb 29 23:00:21 2012 +0000
- Revision:
- 4:684f39d0c346
- Parent:
- 3:1cf99502f396
- Child:
- 5:19c5437ed9fe
Who changed what in which revision?
| User | Revision | Line number | New 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 | 0:77afd7560544 | 118 | }; |
| nucho | 0:77afd7560544 | 119 | |
| nucho | 0:77afd7560544 | 120 | protected: |
| nucho | 3:1cf99502f396 | 121 | //State machine variables for spinOnce |
| nucho | 0:77afd7560544 | 122 | int mode_; |
| nucho | 0:77afd7560544 | 123 | int bytes_; |
| nucho | 0:77afd7560544 | 124 | int topic_; |
| nucho | 0:77afd7560544 | 125 | int index_; |
| nucho | 0:77afd7560544 | 126 | int checksum_; |
| nucho | 0:77afd7560544 | 127 | |
| nucho | 3:1cf99502f396 | 128 | bool configured_; |
| nucho | 3:1cf99502f396 | 129 | |
| nucho | 0:77afd7560544 | 130 | /* used for syncing the time */ |
| nucho | 0:77afd7560544 | 131 | unsigned long last_sync_time; |
| nucho | 0:77afd7560544 | 132 | unsigned long last_sync_receive_time; |
| nucho | 0:77afd7560544 | 133 | unsigned long last_msg_timeout_time; |
| nucho | 0:77afd7560544 | 134 | |
| nucho | 0:77afd7560544 | 135 | public: |
| nucho | 0:77afd7560544 | 136 | /* This function goes in your loop() function, it handles |
| nucho | 0:77afd7560544 | 137 | * serial input and callbacks for subscribers. |
| nucho | 0:77afd7560544 | 138 | */ |
| nucho | 0:77afd7560544 | 139 | |
| nucho | 3:1cf99502f396 | 140 | virtual int spinOnce() { |
| nucho | 0:77afd7560544 | 141 | |
| nucho | 1:ff0ec969dad1 | 142 | /* restart if timed out */ |
| nucho | 0:77afd7560544 | 143 | unsigned long c_time = hardware_.time(); |
| nucho | 1:ff0ec969dad1 | 144 | if ( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ) { |
| nucho | 3:1cf99502f396 | 145 | configured_ = false; |
| nucho | 0:77afd7560544 | 146 | } |
| nucho | 0:77afd7560544 | 147 | |
| nucho | 1:ff0ec969dad1 | 148 | /* reset if message has timed out */ |
| nucho | 1:ff0ec969dad1 | 149 | if ( mode_ != MODE_FIRST_FF) { |
| nucho | 0:77afd7560544 | 150 | if (c_time > last_msg_timeout_time) { |
| nucho | 0:77afd7560544 | 151 | mode_ = MODE_FIRST_FF; |
| nucho | 0:77afd7560544 | 152 | } |
| nucho | 0:77afd7560544 | 153 | } |
| nucho | 0:77afd7560544 | 154 | |
| nucho | 0:77afd7560544 | 155 | /* while available buffer, read data */ |
| nucho | 0:77afd7560544 | 156 | while ( true ) { |
| nucho | 1:ff0ec969dad1 | 157 | int data = hardware_.read(); |
| nucho | 1:ff0ec969dad1 | 158 | if ( data < 0 )//no data |
| nucho | 0:77afd7560544 | 159 | break; |
| nucho | 0:77afd7560544 | 160 | checksum_ += data; |
| nucho | 0:77afd7560544 | 161 | if ( mode_ == MODE_MESSAGE ) { /* message data being recieved */ |
| nucho | 0:77afd7560544 | 162 | message_in[index_++] = data; |
| nucho | 0:77afd7560544 | 163 | bytes_--; |
| nucho | 0:77afd7560544 | 164 | if (bytes_ == 0) /* is message complete? if so, checksum */ |
| nucho | 0:77afd7560544 | 165 | mode_ = MODE_CHECKSUM; |
| nucho | 0:77afd7560544 | 166 | } else if ( mode_ == MODE_FIRST_FF ) { |
| nucho | 0:77afd7560544 | 167 | if (data == 0xff) { |
| nucho | 0:77afd7560544 | 168 | mode_++; |
| nucho | 0:77afd7560544 | 169 | last_msg_timeout_time = c_time + MSG_TIMEOUT; |
| nucho | 0:77afd7560544 | 170 | } |
| nucho | 0:77afd7560544 | 171 | } else if ( mode_ == MODE_SECOND_FF ) { |
| nucho | 0:77afd7560544 | 172 | if (data == 0xff) { |
| nucho | 0:77afd7560544 | 173 | mode_++; |
| nucho | 0:77afd7560544 | 174 | } else { |
| nucho | 0:77afd7560544 | 175 | mode_ = MODE_FIRST_FF; |
| nucho | 0:77afd7560544 | 176 | } |
| nucho | 0:77afd7560544 | 177 | } else if ( mode_ == MODE_TOPIC_L ) { /* bottom half of topic id */ |
| nucho | 0:77afd7560544 | 178 | topic_ = data; |
| nucho | 0:77afd7560544 | 179 | mode_++; |
| nucho | 0:77afd7560544 | 180 | checksum_ = data; /* first byte included in checksum */ |
| nucho | 0:77afd7560544 | 181 | } else if ( mode_ == MODE_TOPIC_H ) { /* top half of topic id */ |
| nucho | 0:77afd7560544 | 182 | topic_ += data<<8; |
| nucho | 0:77afd7560544 | 183 | mode_++; |
| nucho | 0:77afd7560544 | 184 | } else if ( mode_ == MODE_SIZE_L ) { /* bottom half of message size */ |
| nucho | 0:77afd7560544 | 185 | bytes_ = data; |
| nucho | 0:77afd7560544 | 186 | index_ = 0; |
| nucho | 0:77afd7560544 | 187 | mode_++; |
| nucho | 0:77afd7560544 | 188 | } else if ( mode_ == MODE_SIZE_H ) { /* top half of message size */ |
| nucho | 0:77afd7560544 | 189 | bytes_ += data<<8; |
| nucho | 0:77afd7560544 | 190 | mode_ = MODE_MESSAGE; |
| nucho | 0:77afd7560544 | 191 | if (bytes_ == 0) |
| nucho | 0:77afd7560544 | 192 | mode_ = MODE_CHECKSUM; |
| nucho | 0:77afd7560544 | 193 | } else if ( mode_ == MODE_CHECKSUM ) { /* do checksum */ |
| nucho | 3:1cf99502f396 | 194 | mode_ = MODE_FIRST_FF; |
| nucho | 0:77afd7560544 | 195 | if ( (checksum_%256) == 255) { |
| nucho | 3:1cf99502f396 | 196 | if (topic_ == TopicInfo::ID_PUBLISHER) { |
| 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 | 3:1cf99502f396 | 201 | return -1; |
| nucho | 0:77afd7560544 | 202 | } else if (topic_ == TopicInfo::ID_TIME) { |
| nucho | 0:77afd7560544 | 203 | syncTime(message_in); |
| nucho | 0:77afd7560544 | 204 | } else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST) { |
| nucho | 0:77afd7560544 | 205 | req_param_resp.deserialize(message_in); |
| nucho | 0:77afd7560544 | 206 | param_recieved= true; |
| nucho | 0:77afd7560544 | 207 | } else { |
| nucho | 3:1cf99502f396 | 208 | if (subscribers[topic_-100]) |
| nucho | 3:1cf99502f396 | 209 | subscribers[topic_-100]->callback( message_in ); |
| nucho | 0:77afd7560544 | 210 | } |
| nucho | 0:77afd7560544 | 211 | } |
| nucho | 0:77afd7560544 | 212 | } |
| nucho | 0:77afd7560544 | 213 | } |
| nucho | 0:77afd7560544 | 214 | |
| nucho | 0:77afd7560544 | 215 | /* occasionally sync time */ |
| nucho | 3:1cf99502f396 | 216 | if ( 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 | 3:1cf99502f396 | 220 | |
| nucho | 3:1cf99502f396 | 221 | return 0; |
| nucho | 0:77afd7560544 | 222 | } |
| nucho | 0:77afd7560544 | 223 | |
| nucho | 0:77afd7560544 | 224 | /* Are we connected to the PC? */ |
| nucho | 3:1cf99502f396 | 225 | virtual bool connected() { |
| nucho | 3:1cf99502f396 | 226 | return configured_; |
| nucho | 0:77afd7560544 | 227 | }; |
| nucho | 0:77afd7560544 | 228 | |
| nucho | 3:1cf99502f396 | 229 | /******************************************************************** |
| nucho | 0:77afd7560544 | 230 | * Time functions |
| nucho | 1:ff0ec969dad1 | 231 | */ |
| nucho | 0:77afd7560544 | 232 | |
| nucho | 0:77afd7560544 | 233 | void requestSyncTime() { |
| nucho | 0:77afd7560544 | 234 | std_msgs::Time t; |
| nucho | 3:1cf99502f396 | 235 | publish(TopicInfo::ID_TIME, &t); |
| nucho | 0:77afd7560544 | 236 | rt_time = hardware_.time(); |
| nucho | 0:77afd7560544 | 237 | } |
| nucho | 0:77afd7560544 | 238 | |
| nucho | 0:77afd7560544 | 239 | void syncTime( unsigned char * data ) { |
| nucho | 0:77afd7560544 | 240 | std_msgs::Time t; |
| nucho | 0:77afd7560544 | 241 | unsigned long offset = hardware_.time() - rt_time; |
| nucho | 0:77afd7560544 | 242 | |
| nucho | 0:77afd7560544 | 243 | t.deserialize(data); |
| nucho | 0:77afd7560544 | 244 | t.data.sec += offset/1000; |
| nucho | 0:77afd7560544 | 245 | t.data.nsec += (offset%1000)*1000000UL; |
| nucho | 0:77afd7560544 | 246 | |
| nucho | 0:77afd7560544 | 247 | this->setNow(t.data); |
| nucho | 0:77afd7560544 | 248 | last_sync_receive_time = hardware_.time(); |
| nucho | 0:77afd7560544 | 249 | } |
| nucho | 0:77afd7560544 | 250 | |
| nucho | 0:77afd7560544 | 251 | Time now() { |
| nucho | 0:77afd7560544 | 252 | unsigned long ms = hardware_.time(); |
| nucho | 0:77afd7560544 | 253 | Time current_time; |
| nucho | 0:77afd7560544 | 254 | current_time.sec = ms/1000 + sec_offset; |
| nucho | 0:77afd7560544 | 255 | current_time.nsec = (ms%1000)*1000000UL + nsec_offset; |
| nucho | 0:77afd7560544 | 256 | normalizeSecNSec(current_time.sec, current_time.nsec); |
| nucho | 0:77afd7560544 | 257 | return current_time; |
| nucho | 0:77afd7560544 | 258 | } |
| nucho | 0:77afd7560544 | 259 | |
| nucho | 0:77afd7560544 | 260 | void setNow( Time & new_now ) { |
| nucho | 0:77afd7560544 | 261 | unsigned long ms = hardware_.time(); |
| nucho | 0:77afd7560544 | 262 | sec_offset = new_now.sec - ms/1000 - 1; |
| nucho | 0:77afd7560544 | 263 | nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL; |
| nucho | 0:77afd7560544 | 264 | normalizeSecNSec(sec_offset, nsec_offset); |
| nucho | 0:77afd7560544 | 265 | } |
| nucho | 0:77afd7560544 | 266 | |
| nucho | 3:1cf99502f396 | 267 | /******************************************************************** |
| nucho | 3:1cf99502f396 | 268 | * Topic Management |
| nucho | 1:ff0ec969dad1 | 269 | */ |
| nucho | 0:77afd7560544 | 270 | |
| nucho | 3:1cf99502f396 | 271 | /* Register a new publisher */ |
| nucho | 0:77afd7560544 | 272 | bool advertise(Publisher & p) { |
| nucho | 3:1cf99502f396 | 273 | for (int i = 0; i < MAX_PUBLISHERS; i++) { |
| nucho | 0:77afd7560544 | 274 | if (publishers[i] == 0) { // empty slot |
| nucho | 0:77afd7560544 | 275 | publishers[i] = &p; |
| nucho | 0:77afd7560544 | 276 | p.id_ = i+100+MAX_SUBSCRIBERS; |
| nucho | 3:1cf99502f396 | 277 | p.nh_ = this; |
| nucho | 3:1cf99502f396 | 278 | return true; |
| nucho | 3:1cf99502f396 | 279 | } |
| nucho | 3:1cf99502f396 | 280 | } |
| nucho | 3:1cf99502f396 | 281 | return false; |
| nucho | 3:1cf99502f396 | 282 | } |
| nucho | 3:1cf99502f396 | 283 | |
| nucho | 3:1cf99502f396 | 284 | /* Register a new subscriber */ |
| nucho | 3:1cf99502f396 | 285 | template<typename MsgT> |
| nucho | 3:1cf99502f396 | 286 | bool subscribe(Subscriber< MsgT> & s) { |
| nucho | 3:1cf99502f396 | 287 | for (int i = 0; i < MAX_SUBSCRIBERS; i++) { |
| nucho | 3:1cf99502f396 | 288 | if (subscribers[i] == 0) { // empty slot |
| nucho | 3:1cf99502f396 | 289 | subscribers[i] = (Subscriber_*) &s; |
| nucho | 3:1cf99502f396 | 290 | s.id_ = i+100; |
| nucho | 0:77afd7560544 | 291 | return true; |
| nucho | 0:77afd7560544 | 292 | } |
| nucho | 0:77afd7560544 | 293 | } |
| nucho | 0:77afd7560544 | 294 | return false; |
| nucho | 0:77afd7560544 | 295 | } |
| nucho | 0:77afd7560544 | 296 | |
| nucho | 3:1cf99502f396 | 297 | /* Register a new Service Server */ |
| nucho | 3:1cf99502f396 | 298 | template<typename MReq, typename MRes> |
| nucho | 3:1cf99502f396 | 299 | bool advertiseService(ServiceServer<MReq,MRes>& srv) { |
| nucho | 3:1cf99502f396 | 300 | bool v = advertise(srv.pub); |
| nucho | 3:1cf99502f396 | 301 | for (int i = 0; i < MAX_SUBSCRIBERS; i++) { |
| nucho | 3:1cf99502f396 | 302 | if (subscribers[i] == 0) { // empty slot |
| nucho | 3:1cf99502f396 | 303 | subscribers[i] = (Subscriber_*) &srv; |
| nucho | 3:1cf99502f396 | 304 | srv.id_ = i+100; |
| nucho | 3:1cf99502f396 | 305 | return v; |
| nucho | 3:1cf99502f396 | 306 | } |
| nucho | 3:1cf99502f396 | 307 | } |
| nucho | 3:1cf99502f396 | 308 | return false; |
| nucho | 0:77afd7560544 | 309 | } |
| nucho | 0:77afd7560544 | 310 | |
| nucho | 3:1cf99502f396 | 311 | /* Register a new Service Client */ |
| nucho | 3:1cf99502f396 | 312 | template<typename MReq, typename MRes> |
| nucho | 3:1cf99502f396 | 313 | bool serviceClient(ServiceClient<MReq, MRes>& srv) { |
| nucho | 3:1cf99502f396 | 314 | bool v = advertise(srv.pub); |
| nucho | 3:1cf99502f396 | 315 | for (int i = 0; i < MAX_SUBSCRIBERS; i++) { |
| nucho | 3:1cf99502f396 | 316 | if (subscribers[i] == 0) { // empty slot |
| nucho | 3:1cf99502f396 | 317 | subscribers[i] = (Subscriber_*) &srv; |
| nucho | 3:1cf99502f396 | 318 | srv.id_ = i+100; |
| nucho | 3:1cf99502f396 | 319 | return v; |
| nucho | 3:1cf99502f396 | 320 | } |
| nucho | 3:1cf99502f396 | 321 | } |
| nucho | 3:1cf99502f396 | 322 | return false; |
| nucho | 0:77afd7560544 | 323 | } |
| nucho | 0:77afd7560544 | 324 | |
| nucho | 0:77afd7560544 | 325 | void negotiateTopics() { |
| nucho | 3:1cf99502f396 | 326 | configured_ = true; |
| nucho | 4:684f39d0c346 | 327 | |
| nucho | 0:77afd7560544 | 328 | rosserial_msgs::TopicInfo ti; |
| nucho | 0:77afd7560544 | 329 | int i; |
| nucho | 4:684f39d0c346 | 330 | for (i = 0; i < MAX_PUBLISHERS; i++) |
| nucho | 4:684f39d0c346 | 331 | { |
| nucho | 4:684f39d0c346 | 332 | if (publishers[i] != 0) // non-empty slot |
| nucho | 4:684f39d0c346 | 333 | { |
| nucho | 0:77afd7560544 | 334 | ti.topic_id = publishers[i]->id_; |
| nucho | 0:77afd7560544 | 335 | ti.topic_name = (char *) publishers[i]->topic_; |
| nucho | 0:77afd7560544 | 336 | ti.message_type = (char *) publishers[i]->msg_->getType(); |
| nucho | 3:1cf99502f396 | 337 | ti.md5sum = (char *) publishers[i]->msg_->getMD5(); |
| nucho | 3:1cf99502f396 | 338 | ti.buffer_size = OUTPUT_SIZE; |
| nucho | 3:1cf99502f396 | 339 | publish( publishers[i]->getEndpointType(), &ti ); |
| nucho | 0:77afd7560544 | 340 | } |
| nucho | 0:77afd7560544 | 341 | } |
| nucho | 4:684f39d0c346 | 342 | for (i = 0; i < MAX_SUBSCRIBERS; i++) |
| nucho | 4:684f39d0c346 | 343 | { |
| nucho | 4:684f39d0c346 | 344 | if (subscribers[i] != 0) // non-empty slot |
| nucho | 4:684f39d0c346 | 345 | { |
| 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 |