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