a
ros/node_handle.h@2:ab8333331642, 2018-11-02 (annotated)
- Committer:
- jvfausto
- Date:
- Fri Nov 02 21:48:22 2018 +0000
- Revision:
- 2:ab8333331642
- Parent:
- 0:9e9b7db60fd5
- Child:
- 3:a70f39eabe9a
Working towards twists
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
garyservin | 0:9e9b7db60fd5 | 1 | /* |
garyservin | 0:9e9b7db60fd5 | 2 | * Software License Agreement (BSD License) |
garyservin | 0:9e9b7db60fd5 | 3 | * |
garyservin | 0:9e9b7db60fd5 | 4 | * Copyright (c) 2011, Willow Garage, Inc. |
garyservin | 0:9e9b7db60fd5 | 5 | * All rights reserved. |
garyservin | 0:9e9b7db60fd5 | 6 | * |
garyservin | 0:9e9b7db60fd5 | 7 | * Redistribution and use in source and binary forms, with or without |
garyservin | 0:9e9b7db60fd5 | 8 | * modification, are permitted provided that the following conditions |
garyservin | 0:9e9b7db60fd5 | 9 | * are met: |
garyservin | 0:9e9b7db60fd5 | 10 | * |
garyservin | 0:9e9b7db60fd5 | 11 | * * Redistributions of source code must retain the above copyright |
garyservin | 0:9e9b7db60fd5 | 12 | * notice, this list of conditions and the following disclaimer. |
garyservin | 0:9e9b7db60fd5 | 13 | * * Redistributions in binary form must reproduce the above |
garyservin | 0:9e9b7db60fd5 | 14 | * copyright notice, this list of conditions and the following |
garyservin | 0:9e9b7db60fd5 | 15 | * disclaimer in the documentation and/or other materials provided |
garyservin | 0:9e9b7db60fd5 | 16 | * with the distribution. |
garyservin | 0:9e9b7db60fd5 | 17 | * * Neither the name of Willow Garage, Inc. nor the names of its |
garyservin | 0:9e9b7db60fd5 | 18 | * contributors may be used to endorse or promote prducts derived |
garyservin | 0:9e9b7db60fd5 | 19 | * from this software without specific prior written permission. |
garyservin | 0:9e9b7db60fd5 | 20 | * |
garyservin | 0:9e9b7db60fd5 | 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
garyservin | 0:9e9b7db60fd5 | 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
garyservin | 0:9e9b7db60fd5 | 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
garyservin | 0:9e9b7db60fd5 | 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
garyservin | 0:9e9b7db60fd5 | 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
garyservin | 0:9e9b7db60fd5 | 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
garyservin | 0:9e9b7db60fd5 | 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
garyservin | 0:9e9b7db60fd5 | 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
garyservin | 0:9e9b7db60fd5 | 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
garyservin | 0:9e9b7db60fd5 | 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
garyservin | 0:9e9b7db60fd5 | 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
garyservin | 0:9e9b7db60fd5 | 32 | * POSSIBILITY OF SUCH DAMAGE. |
garyservin | 0:9e9b7db60fd5 | 33 | */ |
garyservin | 0:9e9b7db60fd5 | 34 | |
garyservin | 0:9e9b7db60fd5 | 35 | #ifndef ROS_NODE_HANDLE_H_ |
garyservin | 0:9e9b7db60fd5 | 36 | #define ROS_NODE_HANDLE_H_ |
garyservin | 0:9e9b7db60fd5 | 37 | |
garyservin | 0:9e9b7db60fd5 | 38 | #include <stdint.h> |
garyservin | 0:9e9b7db60fd5 | 39 | |
garyservin | 0:9e9b7db60fd5 | 40 | #include "std_msgs/Time.h" |
garyservin | 0:9e9b7db60fd5 | 41 | #include "rosserial_msgs/TopicInfo.h" |
garyservin | 0:9e9b7db60fd5 | 42 | #include "rosserial_msgs/Log.h" |
garyservin | 0:9e9b7db60fd5 | 43 | #include "rosserial_msgs/RequestParam.h" |
garyservin | 0:9e9b7db60fd5 | 44 | |
garyservin | 0:9e9b7db60fd5 | 45 | #define SYNC_SECONDS 5 |
garyservin | 0:9e9b7db60fd5 | 46 | |
garyservin | 0:9e9b7db60fd5 | 47 | #define MODE_FIRST_FF 0 |
garyservin | 0:9e9b7db60fd5 | 48 | /* |
garyservin | 0:9e9b7db60fd5 | 49 | * The second sync byte is a protocol version. It's value is 0xff for the first |
garyservin | 0:9e9b7db60fd5 | 50 | * version of the rosserial protocol (used up to hydro), 0xfe for the second version |
garyservin | 0:9e9b7db60fd5 | 51 | * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable |
garyservin | 0:9e9b7db60fd5 | 52 | * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy |
garyservin | 0:9e9b7db60fd5 | 53 | * rosserial_arduino. It must be changed in both this file and in |
garyservin | 0:9e9b7db60fd5 | 54 | * rosserial_python/src/rosserial_python/SerialClient.py |
garyservin | 0:9e9b7db60fd5 | 55 | */ |
garyservin | 0:9e9b7db60fd5 | 56 | #define MODE_PROTOCOL_VER 1 |
garyservin | 0:9e9b7db60fd5 | 57 | #define PROTOCOL_VER1 0xff // through groovy |
garyservin | 0:9e9b7db60fd5 | 58 | #define PROTOCOL_VER2 0xfe // in hydro |
garyservin | 0:9e9b7db60fd5 | 59 | #define PROTOCOL_VER PROTOCOL_VER2 |
garyservin | 0:9e9b7db60fd5 | 60 | #define MODE_SIZE_L 2 |
garyservin | 0:9e9b7db60fd5 | 61 | #define MODE_SIZE_H 3 |
garyservin | 0:9e9b7db60fd5 | 62 | #define MODE_SIZE_CHECKSUM 4 // checksum for msg size received from size L and H |
garyservin | 0:9e9b7db60fd5 | 63 | #define MODE_TOPIC_L 5 // waiting for topic id |
garyservin | 0:9e9b7db60fd5 | 64 | #define MODE_TOPIC_H 6 |
garyservin | 0:9e9b7db60fd5 | 65 | #define MODE_MESSAGE 7 |
garyservin | 0:9e9b7db60fd5 | 66 | #define MODE_MSG_CHECKSUM 8 // checksum for msg and topic id |
garyservin | 0:9e9b7db60fd5 | 67 | |
garyservin | 0:9e9b7db60fd5 | 68 | |
garyservin | 0:9e9b7db60fd5 | 69 | #define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data |
garyservin | 0:9e9b7db60fd5 | 70 | |
garyservin | 0:9e9b7db60fd5 | 71 | #include "ros/msg.h" |
garyservin | 0:9e9b7db60fd5 | 72 | |
garyservin | 0:9e9b7db60fd5 | 73 | namespace ros { |
garyservin | 0:9e9b7db60fd5 | 74 | |
garyservin | 0:9e9b7db60fd5 | 75 | class NodeHandleBase_{ |
garyservin | 0:9e9b7db60fd5 | 76 | public: |
garyservin | 0:9e9b7db60fd5 | 77 | virtual int publish(int id, const Msg* msg)=0; |
garyservin | 0:9e9b7db60fd5 | 78 | virtual int spinOnce()=0; |
garyservin | 0:9e9b7db60fd5 | 79 | virtual bool connected()=0; |
garyservin | 0:9e9b7db60fd5 | 80 | }; |
garyservin | 0:9e9b7db60fd5 | 81 | } |
garyservin | 0:9e9b7db60fd5 | 82 | |
garyservin | 0:9e9b7db60fd5 | 83 | #include "ros/publisher.h" |
garyservin | 0:9e9b7db60fd5 | 84 | #include "ros/subscriber.h" |
garyservin | 0:9e9b7db60fd5 | 85 | #include "ros/service_server.h" |
garyservin | 0:9e9b7db60fd5 | 86 | #include "ros/service_client.h" |
garyservin | 0:9e9b7db60fd5 | 87 | |
garyservin | 0:9e9b7db60fd5 | 88 | namespace ros { |
garyservin | 0:9e9b7db60fd5 | 89 | |
garyservin | 0:9e9b7db60fd5 | 90 | using rosserial_msgs::TopicInfo; |
garyservin | 0:9e9b7db60fd5 | 91 | |
garyservin | 0:9e9b7db60fd5 | 92 | /* Node Handle */ |
garyservin | 0:9e9b7db60fd5 | 93 | template<class Hardware, |
garyservin | 0:9e9b7db60fd5 | 94 | int MAX_SUBSCRIBERS=25, |
garyservin | 0:9e9b7db60fd5 | 95 | int MAX_PUBLISHERS=25, |
garyservin | 0:9e9b7db60fd5 | 96 | int INPUT_SIZE=512, |
jvfausto | 2:ab8333331642 | 97 | int OUTPUT_SIZE=1024> |
garyservin | 0:9e9b7db60fd5 | 98 | class NodeHandle_ : public NodeHandleBase_ |
garyservin | 0:9e9b7db60fd5 | 99 | { |
garyservin | 0:9e9b7db60fd5 | 100 | protected: |
garyservin | 0:9e9b7db60fd5 | 101 | Hardware hardware_; |
garyservin | 0:9e9b7db60fd5 | 102 | |
garyservin | 0:9e9b7db60fd5 | 103 | /* time used for syncing */ |
garyservin | 0:9e9b7db60fd5 | 104 | uint32_t rt_time; |
garyservin | 0:9e9b7db60fd5 | 105 | |
garyservin | 0:9e9b7db60fd5 | 106 | /* used for computing current time */ |
garyservin | 0:9e9b7db60fd5 | 107 | uint32_t sec_offset, nsec_offset; |
garyservin | 0:9e9b7db60fd5 | 108 | |
garyservin | 0:9e9b7db60fd5 | 109 | uint8_t message_in[INPUT_SIZE]; |
garyservin | 0:9e9b7db60fd5 | 110 | uint8_t message_out[OUTPUT_SIZE]; |
garyservin | 0:9e9b7db60fd5 | 111 | |
garyservin | 0:9e9b7db60fd5 | 112 | Publisher * publishers[MAX_PUBLISHERS]; |
garyservin | 0:9e9b7db60fd5 | 113 | Subscriber_ * subscribers[MAX_SUBSCRIBERS]; |
garyservin | 0:9e9b7db60fd5 | 114 | |
garyservin | 0:9e9b7db60fd5 | 115 | /* |
garyservin | 0:9e9b7db60fd5 | 116 | * Setup Functions |
garyservin | 0:9e9b7db60fd5 | 117 | */ |
garyservin | 0:9e9b7db60fd5 | 118 | public: |
garyservin | 0:9e9b7db60fd5 | 119 | NodeHandle_() : configured_(false) { |
garyservin | 0:9e9b7db60fd5 | 120 | |
garyservin | 0:9e9b7db60fd5 | 121 | for(unsigned int i=0; i< MAX_PUBLISHERS; i++) |
garyservin | 0:9e9b7db60fd5 | 122 | publishers[i] = 0; |
garyservin | 0:9e9b7db60fd5 | 123 | |
garyservin | 0:9e9b7db60fd5 | 124 | for(unsigned int i=0; i< MAX_SUBSCRIBERS; i++) |
garyservin | 0:9e9b7db60fd5 | 125 | subscribers[i] = 0; |
garyservin | 0:9e9b7db60fd5 | 126 | |
garyservin | 0:9e9b7db60fd5 | 127 | for(unsigned int i=0; i< INPUT_SIZE; i++) |
garyservin | 0:9e9b7db60fd5 | 128 | message_in[i] = 0; |
garyservin | 0:9e9b7db60fd5 | 129 | |
garyservin | 0:9e9b7db60fd5 | 130 | for(unsigned int i=0; i< OUTPUT_SIZE; i++) |
garyservin | 0:9e9b7db60fd5 | 131 | message_out[i] = 0; |
garyservin | 0:9e9b7db60fd5 | 132 | |
garyservin | 0:9e9b7db60fd5 | 133 | req_param_resp.ints_length = 0; |
garyservin | 0:9e9b7db60fd5 | 134 | req_param_resp.ints = NULL; |
garyservin | 0:9e9b7db60fd5 | 135 | req_param_resp.floats_length = 0; |
garyservin | 0:9e9b7db60fd5 | 136 | req_param_resp.floats = NULL; |
garyservin | 0:9e9b7db60fd5 | 137 | req_param_resp.ints_length = 0; |
garyservin | 0:9e9b7db60fd5 | 138 | req_param_resp.ints = NULL; |
garyservin | 0:9e9b7db60fd5 | 139 | } |
garyservin | 0:9e9b7db60fd5 | 140 | |
garyservin | 0:9e9b7db60fd5 | 141 | Hardware* getHardware(){ |
garyservin | 0:9e9b7db60fd5 | 142 | return &hardware_; |
garyservin | 0:9e9b7db60fd5 | 143 | } |
garyservin | 0:9e9b7db60fd5 | 144 | |
garyservin | 0:9e9b7db60fd5 | 145 | /* Start serial, initialize buffers */ |
garyservin | 0:9e9b7db60fd5 | 146 | void initNode(){ |
garyservin | 0:9e9b7db60fd5 | 147 | hardware_.init(); |
garyservin | 0:9e9b7db60fd5 | 148 | mode_ = 0; |
garyservin | 0:9e9b7db60fd5 | 149 | bytes_ = 0; |
garyservin | 0:9e9b7db60fd5 | 150 | index_ = 0; |
garyservin | 0:9e9b7db60fd5 | 151 | topic_ = 0; |
garyservin | 0:9e9b7db60fd5 | 152 | }; |
garyservin | 0:9e9b7db60fd5 | 153 | |
garyservin | 0:9e9b7db60fd5 | 154 | /* Start a named port, which may be network server IP, initialize buffers */ |
garyservin | 0:9e9b7db60fd5 | 155 | void initNode(char *portName){ |
garyservin | 0:9e9b7db60fd5 | 156 | hardware_.init(portName); |
garyservin | 0:9e9b7db60fd5 | 157 | mode_ = 0; |
garyservin | 0:9e9b7db60fd5 | 158 | bytes_ = 0; |
garyservin | 0:9e9b7db60fd5 | 159 | index_ = 0; |
garyservin | 0:9e9b7db60fd5 | 160 | topic_ = 0; |
garyservin | 0:9e9b7db60fd5 | 161 | }; |
garyservin | 0:9e9b7db60fd5 | 162 | |
garyservin | 0:9e9b7db60fd5 | 163 | protected: |
garyservin | 0:9e9b7db60fd5 | 164 | //State machine variables for spinOnce |
garyservin | 0:9e9b7db60fd5 | 165 | int mode_; |
garyservin | 0:9e9b7db60fd5 | 166 | int bytes_; |
garyservin | 0:9e9b7db60fd5 | 167 | int topic_; |
garyservin | 0:9e9b7db60fd5 | 168 | int index_; |
garyservin | 0:9e9b7db60fd5 | 169 | int checksum_; |
garyservin | 0:9e9b7db60fd5 | 170 | |
garyservin | 0:9e9b7db60fd5 | 171 | bool configured_; |
garyservin | 0:9e9b7db60fd5 | 172 | |
garyservin | 0:9e9b7db60fd5 | 173 | /* used for syncing the time */ |
garyservin | 0:9e9b7db60fd5 | 174 | uint32_t last_sync_time; |
garyservin | 0:9e9b7db60fd5 | 175 | uint32_t last_sync_receive_time; |
garyservin | 0:9e9b7db60fd5 | 176 | uint32_t last_msg_timeout_time; |
garyservin | 0:9e9b7db60fd5 | 177 | |
garyservin | 0:9e9b7db60fd5 | 178 | public: |
garyservin | 0:9e9b7db60fd5 | 179 | /* This function goes in your loop() function, it handles |
garyservin | 0:9e9b7db60fd5 | 180 | * serial input and callbacks for subscribers. |
garyservin | 0:9e9b7db60fd5 | 181 | */ |
garyservin | 0:9e9b7db60fd5 | 182 | |
garyservin | 0:9e9b7db60fd5 | 183 | |
garyservin | 0:9e9b7db60fd5 | 184 | virtual int spinOnce(){ |
garyservin | 0:9e9b7db60fd5 | 185 | |
garyservin | 0:9e9b7db60fd5 | 186 | /* restart if timed out */ |
garyservin | 0:9e9b7db60fd5 | 187 | uint32_t c_time = hardware_.time(); |
garyservin | 0:9e9b7db60fd5 | 188 | if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){ |
garyservin | 0:9e9b7db60fd5 | 189 | configured_ = false; |
garyservin | 0:9e9b7db60fd5 | 190 | } |
garyservin | 0:9e9b7db60fd5 | 191 | |
garyservin | 0:9e9b7db60fd5 | 192 | /* reset if message has timed out */ |
garyservin | 0:9e9b7db60fd5 | 193 | if ( mode_ != MODE_FIRST_FF){ |
garyservin | 0:9e9b7db60fd5 | 194 | if (c_time > last_msg_timeout_time){ |
garyservin | 0:9e9b7db60fd5 | 195 | mode_ = MODE_FIRST_FF; |
garyservin | 0:9e9b7db60fd5 | 196 | } |
garyservin | 0:9e9b7db60fd5 | 197 | } |
garyservin | 0:9e9b7db60fd5 | 198 | |
garyservin | 0:9e9b7db60fd5 | 199 | /* while available buffer, read data */ |
garyservin | 0:9e9b7db60fd5 | 200 | while( true ) |
garyservin | 0:9e9b7db60fd5 | 201 | { |
garyservin | 0:9e9b7db60fd5 | 202 | int data = hardware_.read(); |
garyservin | 0:9e9b7db60fd5 | 203 | if( data < 0 ) |
garyservin | 0:9e9b7db60fd5 | 204 | break; |
garyservin | 0:9e9b7db60fd5 | 205 | checksum_ += data; |
garyservin | 0:9e9b7db60fd5 | 206 | if( mode_ == MODE_MESSAGE ){ /* message data being recieved */ |
garyservin | 0:9e9b7db60fd5 | 207 | message_in[index_++] = data; |
garyservin | 0:9e9b7db60fd5 | 208 | bytes_--; |
garyservin | 0:9e9b7db60fd5 | 209 | if(bytes_ == 0) /* is message complete? if so, checksum */ |
garyservin | 0:9e9b7db60fd5 | 210 | mode_ = MODE_MSG_CHECKSUM; |
garyservin | 0:9e9b7db60fd5 | 211 | }else if( mode_ == MODE_FIRST_FF ){ |
garyservin | 0:9e9b7db60fd5 | 212 | if(data == 0xff){ |
garyservin | 0:9e9b7db60fd5 | 213 | mode_++; |
garyservin | 0:9e9b7db60fd5 | 214 | last_msg_timeout_time = c_time + MSG_TIMEOUT; |
garyservin | 0:9e9b7db60fd5 | 215 | } |
garyservin | 0:9e9b7db60fd5 | 216 | else if( hardware_.time() - c_time > (SYNC_SECONDS)){ |
garyservin | 0:9e9b7db60fd5 | 217 | /* We have been stuck in spinOnce too long, return error */ |
garyservin | 0:9e9b7db60fd5 | 218 | configured_=false; |
garyservin | 0:9e9b7db60fd5 | 219 | return -2; |
garyservin | 0:9e9b7db60fd5 | 220 | } |
garyservin | 0:9e9b7db60fd5 | 221 | }else if( mode_ == MODE_PROTOCOL_VER ){ |
garyservin | 0:9e9b7db60fd5 | 222 | if(data == PROTOCOL_VER){ |
garyservin | 0:9e9b7db60fd5 | 223 | mode_++; |
garyservin | 0:9e9b7db60fd5 | 224 | }else{ |
garyservin | 0:9e9b7db60fd5 | 225 | mode_ = MODE_FIRST_FF; |
garyservin | 0:9e9b7db60fd5 | 226 | if (configured_ == false) |
garyservin | 0:9e9b7db60fd5 | 227 | requestSyncTime(); /* send a msg back showing our protocol version */ |
garyservin | 0:9e9b7db60fd5 | 228 | } |
garyservin | 0:9e9b7db60fd5 | 229 | }else if( mode_ == MODE_SIZE_L ){ /* bottom half of message size */ |
garyservin | 0:9e9b7db60fd5 | 230 | bytes_ = data; |
garyservin | 0:9e9b7db60fd5 | 231 | index_ = 0; |
garyservin | 0:9e9b7db60fd5 | 232 | mode_++; |
garyservin | 0:9e9b7db60fd5 | 233 | checksum_ = data; /* first byte for calculating size checksum */ |
garyservin | 0:9e9b7db60fd5 | 234 | }else if( mode_ == MODE_SIZE_H ){ /* top half of message size */ |
garyservin | 0:9e9b7db60fd5 | 235 | bytes_ += data<<8; |
garyservin | 0:9e9b7db60fd5 | 236 | mode_++; |
garyservin | 0:9e9b7db60fd5 | 237 | }else if( mode_ == MODE_SIZE_CHECKSUM ){ |
garyservin | 0:9e9b7db60fd5 | 238 | if( (checksum_%256) == 255) |
garyservin | 0:9e9b7db60fd5 | 239 | mode_++; |
garyservin | 0:9e9b7db60fd5 | 240 | else |
garyservin | 0:9e9b7db60fd5 | 241 | mode_ = MODE_FIRST_FF; /* Abandon the frame if the msg len is wrong */ |
garyservin | 0:9e9b7db60fd5 | 242 | }else if( mode_ == MODE_TOPIC_L ){ /* bottom half of topic id */ |
garyservin | 0:9e9b7db60fd5 | 243 | topic_ = data; |
garyservin | 0:9e9b7db60fd5 | 244 | mode_++; |
garyservin | 0:9e9b7db60fd5 | 245 | checksum_ = data; /* first byte included in checksum */ |
garyservin | 0:9e9b7db60fd5 | 246 | }else if( mode_ == MODE_TOPIC_H ){ /* top half of topic id */ |
garyservin | 0:9e9b7db60fd5 | 247 | topic_ += data<<8; |
garyservin | 0:9e9b7db60fd5 | 248 | mode_ = MODE_MESSAGE; |
garyservin | 0:9e9b7db60fd5 | 249 | if(bytes_ == 0) |
garyservin | 0:9e9b7db60fd5 | 250 | mode_ = MODE_MSG_CHECKSUM; |
garyservin | 0:9e9b7db60fd5 | 251 | }else if( mode_ == MODE_MSG_CHECKSUM ){ /* do checksum */ |
garyservin | 0:9e9b7db60fd5 | 252 | mode_ = MODE_FIRST_FF; |
garyservin | 0:9e9b7db60fd5 | 253 | if( (checksum_%256) == 255){ |
garyservin | 0:9e9b7db60fd5 | 254 | if(topic_ == TopicInfo::ID_PUBLISHER){ |
garyservin | 0:9e9b7db60fd5 | 255 | requestSyncTime(); |
garyservin | 0:9e9b7db60fd5 | 256 | negotiateTopics(); |
garyservin | 0:9e9b7db60fd5 | 257 | last_sync_time = c_time; |
garyservin | 0:9e9b7db60fd5 | 258 | last_sync_receive_time = c_time; |
garyservin | 0:9e9b7db60fd5 | 259 | return -1; |
garyservin | 0:9e9b7db60fd5 | 260 | }else if(topic_ == TopicInfo::ID_TIME){ |
garyservin | 0:9e9b7db60fd5 | 261 | syncTime(message_in); |
garyservin | 0:9e9b7db60fd5 | 262 | }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){ |
garyservin | 0:9e9b7db60fd5 | 263 | req_param_resp.deserialize(message_in); |
garyservin | 0:9e9b7db60fd5 | 264 | param_recieved= true; |
garyservin | 0:9e9b7db60fd5 | 265 | }else if(topic_ == TopicInfo::ID_TX_STOP){ |
garyservin | 0:9e9b7db60fd5 | 266 | configured_ = false; |
garyservin | 0:9e9b7db60fd5 | 267 | }else{ |
garyservin | 0:9e9b7db60fd5 | 268 | if(subscribers[topic_-100]) |
garyservin | 0:9e9b7db60fd5 | 269 | subscribers[topic_-100]->callback( message_in ); |
garyservin | 0:9e9b7db60fd5 | 270 | } |
garyservin | 0:9e9b7db60fd5 | 271 | } |
garyservin | 0:9e9b7db60fd5 | 272 | } |
garyservin | 0:9e9b7db60fd5 | 273 | } |
garyservin | 0:9e9b7db60fd5 | 274 | |
garyservin | 0:9e9b7db60fd5 | 275 | /* occasionally sync time */ |
garyservin | 0:9e9b7db60fd5 | 276 | if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){ |
garyservin | 0:9e9b7db60fd5 | 277 | requestSyncTime(); |
garyservin | 0:9e9b7db60fd5 | 278 | last_sync_time = c_time; |
garyservin | 0:9e9b7db60fd5 | 279 | } |
garyservin | 0:9e9b7db60fd5 | 280 | |
garyservin | 0:9e9b7db60fd5 | 281 | return 0; |
garyservin | 0:9e9b7db60fd5 | 282 | } |
garyservin | 0:9e9b7db60fd5 | 283 | |
garyservin | 0:9e9b7db60fd5 | 284 | |
garyservin | 0:9e9b7db60fd5 | 285 | /* Are we connected to the PC? */ |
garyservin | 0:9e9b7db60fd5 | 286 | virtual bool connected() { |
garyservin | 0:9e9b7db60fd5 | 287 | return configured_; |
garyservin | 0:9e9b7db60fd5 | 288 | }; |
garyservin | 0:9e9b7db60fd5 | 289 | |
garyservin | 0:9e9b7db60fd5 | 290 | /******************************************************************** |
garyservin | 0:9e9b7db60fd5 | 291 | * Time functions |
garyservin | 0:9e9b7db60fd5 | 292 | */ |
garyservin | 0:9e9b7db60fd5 | 293 | |
garyservin | 0:9e9b7db60fd5 | 294 | void requestSyncTime() |
garyservin | 0:9e9b7db60fd5 | 295 | { |
garyservin | 0:9e9b7db60fd5 | 296 | std_msgs::Time t; |
garyservin | 0:9e9b7db60fd5 | 297 | publish(TopicInfo::ID_TIME, &t); |
garyservin | 0:9e9b7db60fd5 | 298 | rt_time = hardware_.time(); |
garyservin | 0:9e9b7db60fd5 | 299 | } |
garyservin | 0:9e9b7db60fd5 | 300 | |
garyservin | 0:9e9b7db60fd5 | 301 | void syncTime(uint8_t * data) |
garyservin | 0:9e9b7db60fd5 | 302 | { |
garyservin | 0:9e9b7db60fd5 | 303 | std_msgs::Time t; |
garyservin | 0:9e9b7db60fd5 | 304 | uint32_t offset = hardware_.time() - rt_time; |
garyservin | 0:9e9b7db60fd5 | 305 | |
garyservin | 0:9e9b7db60fd5 | 306 | t.deserialize(data); |
garyservin | 0:9e9b7db60fd5 | 307 | t.data.sec += offset/1000; |
garyservin | 0:9e9b7db60fd5 | 308 | t.data.nsec += (offset%1000)*1000000UL; |
garyservin | 0:9e9b7db60fd5 | 309 | |
garyservin | 0:9e9b7db60fd5 | 310 | this->setNow(t.data); |
garyservin | 0:9e9b7db60fd5 | 311 | last_sync_receive_time = hardware_.time(); |
garyservin | 0:9e9b7db60fd5 | 312 | } |
garyservin | 0:9e9b7db60fd5 | 313 | |
garyservin | 0:9e9b7db60fd5 | 314 | Time now() |
garyservin | 0:9e9b7db60fd5 | 315 | { |
garyservin | 0:9e9b7db60fd5 | 316 | uint32_t ms = hardware_.time(); |
garyservin | 0:9e9b7db60fd5 | 317 | Time current_time; |
garyservin | 0:9e9b7db60fd5 | 318 | current_time.sec = ms/1000 + sec_offset; |
garyservin | 0:9e9b7db60fd5 | 319 | current_time.nsec = (ms%1000)*1000000UL + nsec_offset; |
garyservin | 0:9e9b7db60fd5 | 320 | normalizeSecNSec(current_time.sec, current_time.nsec); |
garyservin | 0:9e9b7db60fd5 | 321 | return current_time; |
garyservin | 0:9e9b7db60fd5 | 322 | } |
garyservin | 0:9e9b7db60fd5 | 323 | |
garyservin | 0:9e9b7db60fd5 | 324 | void setNow( Time & new_now ) |
garyservin | 0:9e9b7db60fd5 | 325 | { |
garyservin | 0:9e9b7db60fd5 | 326 | uint32_t ms = hardware_.time(); |
garyservin | 0:9e9b7db60fd5 | 327 | sec_offset = new_now.sec - ms/1000 - 1; |
garyservin | 0:9e9b7db60fd5 | 328 | nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL; |
garyservin | 0:9e9b7db60fd5 | 329 | normalizeSecNSec(sec_offset, nsec_offset); |
garyservin | 0:9e9b7db60fd5 | 330 | } |
garyservin | 0:9e9b7db60fd5 | 331 | |
garyservin | 0:9e9b7db60fd5 | 332 | /******************************************************************** |
garyservin | 0:9e9b7db60fd5 | 333 | * Topic Management |
garyservin | 0:9e9b7db60fd5 | 334 | */ |
garyservin | 0:9e9b7db60fd5 | 335 | |
garyservin | 0:9e9b7db60fd5 | 336 | /* Register a new publisher */ |
garyservin | 0:9e9b7db60fd5 | 337 | bool advertise(Publisher & p) |
garyservin | 0:9e9b7db60fd5 | 338 | { |
garyservin | 0:9e9b7db60fd5 | 339 | for(int i = 0; i < MAX_PUBLISHERS; i++){ |
garyservin | 0:9e9b7db60fd5 | 340 | if(publishers[i] == 0){ // empty slot |
garyservin | 0:9e9b7db60fd5 | 341 | publishers[i] = &p; |
garyservin | 0:9e9b7db60fd5 | 342 | p.id_ = i+100+MAX_SUBSCRIBERS; |
garyservin | 0:9e9b7db60fd5 | 343 | p.nh_ = this; |
garyservin | 0:9e9b7db60fd5 | 344 | return true; |
garyservin | 0:9e9b7db60fd5 | 345 | } |
garyservin | 0:9e9b7db60fd5 | 346 | } |
garyservin | 0:9e9b7db60fd5 | 347 | return false; |
garyservin | 0:9e9b7db60fd5 | 348 | } |
garyservin | 0:9e9b7db60fd5 | 349 | |
garyservin | 0:9e9b7db60fd5 | 350 | /* Register a new subscriber */ |
garyservin | 0:9e9b7db60fd5 | 351 | template<typename SubscriberT> |
garyservin | 0:9e9b7db60fd5 | 352 | bool subscribe(SubscriberT& s){ |
garyservin | 0:9e9b7db60fd5 | 353 | for(int i = 0; i < MAX_SUBSCRIBERS; i++){ |
garyservin | 0:9e9b7db60fd5 | 354 | if(subscribers[i] == 0){ // empty slot |
garyservin | 0:9e9b7db60fd5 | 355 | subscribers[i] = static_cast<Subscriber_*>(&s); |
garyservin | 0:9e9b7db60fd5 | 356 | s.id_ = i+100; |
garyservin | 0:9e9b7db60fd5 | 357 | return true; |
garyservin | 0:9e9b7db60fd5 | 358 | } |
garyservin | 0:9e9b7db60fd5 | 359 | } |
garyservin | 0:9e9b7db60fd5 | 360 | return false; |
garyservin | 0:9e9b7db60fd5 | 361 | } |
garyservin | 0:9e9b7db60fd5 | 362 | |
garyservin | 0:9e9b7db60fd5 | 363 | /* Register a new Service Server */ |
garyservin | 0:9e9b7db60fd5 | 364 | template<typename MReq, typename MRes> |
garyservin | 0:9e9b7db60fd5 | 365 | bool advertiseService(ServiceServer<MReq,MRes>& srv){ |
garyservin | 0:9e9b7db60fd5 | 366 | bool v = advertise(srv.pub); |
garyservin | 0:9e9b7db60fd5 | 367 | for(int i = 0; i < MAX_SUBSCRIBERS; i++){ |
garyservin | 0:9e9b7db60fd5 | 368 | if(subscribers[i] == 0){ // empty slot |
garyservin | 0:9e9b7db60fd5 | 369 | subscribers[i] = static_cast<Subscriber_*>(&srv); |
garyservin | 0:9e9b7db60fd5 | 370 | srv.id_ = i+100; |
garyservin | 0:9e9b7db60fd5 | 371 | return v; |
garyservin | 0:9e9b7db60fd5 | 372 | } |
garyservin | 0:9e9b7db60fd5 | 373 | } |
garyservin | 0:9e9b7db60fd5 | 374 | return false; |
garyservin | 0:9e9b7db60fd5 | 375 | } |
garyservin | 0:9e9b7db60fd5 | 376 | |
garyservin | 0:9e9b7db60fd5 | 377 | /* Register a new Service Client */ |
garyservin | 0:9e9b7db60fd5 | 378 | template<typename MReq, typename MRes> |
garyservin | 0:9e9b7db60fd5 | 379 | bool serviceClient(ServiceClient<MReq, MRes>& srv){ |
garyservin | 0:9e9b7db60fd5 | 380 | bool v = advertise(srv.pub); |
garyservin | 0:9e9b7db60fd5 | 381 | for(int i = 0; i < MAX_SUBSCRIBERS; i++){ |
garyservin | 0:9e9b7db60fd5 | 382 | if(subscribers[i] == 0){ // empty slot |
garyservin | 0:9e9b7db60fd5 | 383 | subscribers[i] = static_cast<Subscriber_*>(&srv); |
garyservin | 0:9e9b7db60fd5 | 384 | srv.id_ = i+100; |
garyservin | 0:9e9b7db60fd5 | 385 | return v; |
garyservin | 0:9e9b7db60fd5 | 386 | } |
garyservin | 0:9e9b7db60fd5 | 387 | } |
garyservin | 0:9e9b7db60fd5 | 388 | return false; |
garyservin | 0:9e9b7db60fd5 | 389 | } |
garyservin | 0:9e9b7db60fd5 | 390 | |
garyservin | 0:9e9b7db60fd5 | 391 | void negotiateTopics() |
garyservin | 0:9e9b7db60fd5 | 392 | { |
garyservin | 0:9e9b7db60fd5 | 393 | rosserial_msgs::TopicInfo ti; |
garyservin | 0:9e9b7db60fd5 | 394 | int i; |
garyservin | 0:9e9b7db60fd5 | 395 | for(i = 0; i < MAX_PUBLISHERS; i++) |
garyservin | 0:9e9b7db60fd5 | 396 | { |
garyservin | 0:9e9b7db60fd5 | 397 | if(publishers[i] != 0) // non-empty slot |
garyservin | 0:9e9b7db60fd5 | 398 | { |
garyservin | 0:9e9b7db60fd5 | 399 | ti.topic_id = publishers[i]->id_; |
garyservin | 0:9e9b7db60fd5 | 400 | ti.topic_name = (char *) publishers[i]->topic_; |
garyservin | 0:9e9b7db60fd5 | 401 | ti.message_type = (char *) publishers[i]->msg_->getType(); |
garyservin | 0:9e9b7db60fd5 | 402 | ti.md5sum = (char *) publishers[i]->msg_->getMD5(); |
garyservin | 0:9e9b7db60fd5 | 403 | ti.buffer_size = OUTPUT_SIZE; |
garyservin | 0:9e9b7db60fd5 | 404 | publish( publishers[i]->getEndpointType(), &ti ); |
garyservin | 0:9e9b7db60fd5 | 405 | } |
garyservin | 0:9e9b7db60fd5 | 406 | } |
garyservin | 0:9e9b7db60fd5 | 407 | for(i = 0; i < MAX_SUBSCRIBERS; i++) |
garyservin | 0:9e9b7db60fd5 | 408 | { |
garyservin | 0:9e9b7db60fd5 | 409 | if(subscribers[i] != 0) // non-empty slot |
garyservin | 0:9e9b7db60fd5 | 410 | { |
garyservin | 0:9e9b7db60fd5 | 411 | ti.topic_id = subscribers[i]->id_; |
garyservin | 0:9e9b7db60fd5 | 412 | ti.topic_name = (char *) subscribers[i]->topic_; |
garyservin | 0:9e9b7db60fd5 | 413 | ti.message_type = (char *) subscribers[i]->getMsgType(); |
garyservin | 0:9e9b7db60fd5 | 414 | ti.md5sum = (char *) subscribers[i]->getMsgMD5(); |
garyservin | 0:9e9b7db60fd5 | 415 | ti.buffer_size = INPUT_SIZE; |
garyservin | 0:9e9b7db60fd5 | 416 | publish( subscribers[i]->getEndpointType(), &ti ); |
garyservin | 0:9e9b7db60fd5 | 417 | } |
garyservin | 0:9e9b7db60fd5 | 418 | } |
garyservin | 0:9e9b7db60fd5 | 419 | configured_ = true; |
garyservin | 0:9e9b7db60fd5 | 420 | } |
garyservin | 0:9e9b7db60fd5 | 421 | |
garyservin | 0:9e9b7db60fd5 | 422 | virtual int publish(int id, const Msg * msg) |
garyservin | 0:9e9b7db60fd5 | 423 | { |
garyservin | 0:9e9b7db60fd5 | 424 | if(id >= 100 && !configured_) |
garyservin | 0:9e9b7db60fd5 | 425 | return 0; |
garyservin | 0:9e9b7db60fd5 | 426 | |
garyservin | 0:9e9b7db60fd5 | 427 | /* serialize message */ |
garyservin | 0:9e9b7db60fd5 | 428 | uint16_t l = msg->serialize(message_out+7); |
garyservin | 0:9e9b7db60fd5 | 429 | |
garyservin | 0:9e9b7db60fd5 | 430 | /* setup the header */ |
garyservin | 0:9e9b7db60fd5 | 431 | message_out[0] = 0xff; |
garyservin | 0:9e9b7db60fd5 | 432 | message_out[1] = PROTOCOL_VER; |
garyservin | 0:9e9b7db60fd5 | 433 | message_out[2] = (uint8_t) ((uint16_t)l&255); |
garyservin | 0:9e9b7db60fd5 | 434 | message_out[3] = (uint8_t) ((uint16_t)l>>8); |
garyservin | 0:9e9b7db60fd5 | 435 | message_out[4] = 255 - ((message_out[2] + message_out[3])%256); |
garyservin | 0:9e9b7db60fd5 | 436 | message_out[5] = (uint8_t) ((int16_t)id&255); |
garyservin | 0:9e9b7db60fd5 | 437 | message_out[6] = (uint8_t) ((int16_t)id>>8); |
garyservin | 0:9e9b7db60fd5 | 438 | |
garyservin | 0:9e9b7db60fd5 | 439 | /* calculate checksum */ |
garyservin | 0:9e9b7db60fd5 | 440 | int chk = 0; |
garyservin | 0:9e9b7db60fd5 | 441 | for(int i =5; i<l+7; i++) |
garyservin | 0:9e9b7db60fd5 | 442 | chk += message_out[i]; |
garyservin | 0:9e9b7db60fd5 | 443 | l += 7; |
garyservin | 0:9e9b7db60fd5 | 444 | message_out[l++] = 255 - (chk%256); |
garyservin | 0:9e9b7db60fd5 | 445 | |
garyservin | 0:9e9b7db60fd5 | 446 | if( l <= OUTPUT_SIZE ){ |
garyservin | 0:9e9b7db60fd5 | 447 | hardware_.write(message_out, l); |
garyservin | 0:9e9b7db60fd5 | 448 | return l; |
garyservin | 0:9e9b7db60fd5 | 449 | }else{ |
garyservin | 0:9e9b7db60fd5 | 450 | logerror("Message from device dropped: message larger than buffer."); |
garyservin | 0:9e9b7db60fd5 | 451 | return -1; |
garyservin | 0:9e9b7db60fd5 | 452 | } |
garyservin | 0:9e9b7db60fd5 | 453 | } |
garyservin | 0:9e9b7db60fd5 | 454 | |
garyservin | 0:9e9b7db60fd5 | 455 | /******************************************************************** |
garyservin | 0:9e9b7db60fd5 | 456 | * Logging |
garyservin | 0:9e9b7db60fd5 | 457 | */ |
garyservin | 0:9e9b7db60fd5 | 458 | |
garyservin | 0:9e9b7db60fd5 | 459 | private: |
garyservin | 0:9e9b7db60fd5 | 460 | void log(char byte, const char * msg){ |
garyservin | 0:9e9b7db60fd5 | 461 | rosserial_msgs::Log l; |
garyservin | 0:9e9b7db60fd5 | 462 | l.level= byte; |
garyservin | 0:9e9b7db60fd5 | 463 | l.msg = (char*)msg; |
garyservin | 0:9e9b7db60fd5 | 464 | publish(rosserial_msgs::TopicInfo::ID_LOG, &l); |
garyservin | 0:9e9b7db60fd5 | 465 | } |
garyservin | 0:9e9b7db60fd5 | 466 | |
garyservin | 0:9e9b7db60fd5 | 467 | public: |
garyservin | 0:9e9b7db60fd5 | 468 | void logdebug(const char* msg){ |
garyservin | 0:9e9b7db60fd5 | 469 | log(rosserial_msgs::Log::ROSDEBUG, msg); |
garyservin | 0:9e9b7db60fd5 | 470 | } |
garyservin | 0:9e9b7db60fd5 | 471 | void loginfo(const char * msg){ |
garyservin | 0:9e9b7db60fd5 | 472 | log(rosserial_msgs::Log::INFO, msg); |
garyservin | 0:9e9b7db60fd5 | 473 | } |
garyservin | 0:9e9b7db60fd5 | 474 | void logwarn(const char *msg){ |
garyservin | 0:9e9b7db60fd5 | 475 | log(rosserial_msgs::Log::WARN, msg); |
garyservin | 0:9e9b7db60fd5 | 476 | } |
garyservin | 0:9e9b7db60fd5 | 477 | void logerror(const char*msg){ |
garyservin | 0:9e9b7db60fd5 | 478 | log(rosserial_msgs::Log::ERROR, msg); |
garyservin | 0:9e9b7db60fd5 | 479 | } |
garyservin | 0:9e9b7db60fd5 | 480 | void logfatal(const char*msg){ |
garyservin | 0:9e9b7db60fd5 | 481 | log(rosserial_msgs::Log::FATAL, msg); |
garyservin | 0:9e9b7db60fd5 | 482 | } |
garyservin | 0:9e9b7db60fd5 | 483 | |
garyservin | 0:9e9b7db60fd5 | 484 | /******************************************************************** |
garyservin | 0:9e9b7db60fd5 | 485 | * Parameters |
garyservin | 0:9e9b7db60fd5 | 486 | */ |
garyservin | 0:9e9b7db60fd5 | 487 | |
garyservin | 0:9e9b7db60fd5 | 488 | private: |
garyservin | 0:9e9b7db60fd5 | 489 | bool param_recieved; |
garyservin | 0:9e9b7db60fd5 | 490 | rosserial_msgs::RequestParamResponse req_param_resp; |
garyservin | 0:9e9b7db60fd5 | 491 | |
garyservin | 0:9e9b7db60fd5 | 492 | bool requestParam(const char * name, int time_out = 1000){ |
garyservin | 0:9e9b7db60fd5 | 493 | param_recieved = false; |
garyservin | 0:9e9b7db60fd5 | 494 | rosserial_msgs::RequestParamRequest req; |
garyservin | 0:9e9b7db60fd5 | 495 | req.name = (char*)name; |
garyservin | 0:9e9b7db60fd5 | 496 | publish(TopicInfo::ID_PARAMETER_REQUEST, &req); |
garyservin | 0:9e9b7db60fd5 | 497 | uint16_t end_time = hardware_.time() + time_out; |
garyservin | 0:9e9b7db60fd5 | 498 | while(!param_recieved ){ |
garyservin | 0:9e9b7db60fd5 | 499 | spinOnce(); |
garyservin | 0:9e9b7db60fd5 | 500 | if (hardware_.time() > end_time) return false; |
garyservin | 0:9e9b7db60fd5 | 501 | } |
garyservin | 0:9e9b7db60fd5 | 502 | return true; |
garyservin | 0:9e9b7db60fd5 | 503 | } |
garyservin | 0:9e9b7db60fd5 | 504 | |
garyservin | 0:9e9b7db60fd5 | 505 | public: |
garyservin | 0:9e9b7db60fd5 | 506 | bool getParam(const char* name, int* param, int length =1){ |
garyservin | 0:9e9b7db60fd5 | 507 | if (requestParam(name) ){ |
garyservin | 0:9e9b7db60fd5 | 508 | if (length == req_param_resp.ints_length){ |
garyservin | 0:9e9b7db60fd5 | 509 | //copy it over |
garyservin | 0:9e9b7db60fd5 | 510 | for(int i=0; i<length; i++) |
garyservin | 0:9e9b7db60fd5 | 511 | param[i] = req_param_resp.ints[i]; |
garyservin | 0:9e9b7db60fd5 | 512 | return true; |
garyservin | 0:9e9b7db60fd5 | 513 | } |
garyservin | 0:9e9b7db60fd5 | 514 | } |
garyservin | 0:9e9b7db60fd5 | 515 | return false; |
garyservin | 0:9e9b7db60fd5 | 516 | } |
garyservin | 0:9e9b7db60fd5 | 517 | bool getParam(const char* name, float* param, int length=1){ |
garyservin | 0:9e9b7db60fd5 | 518 | if (requestParam(name) ){ |
garyservin | 0:9e9b7db60fd5 | 519 | if (length == req_param_resp.floats_length){ |
garyservin | 0:9e9b7db60fd5 | 520 | //copy it over |
garyservin | 0:9e9b7db60fd5 | 521 | for(int i=0; i<length; i++) |
garyservin | 0:9e9b7db60fd5 | 522 | param[i] = req_param_resp.floats[i]; |
garyservin | 0:9e9b7db60fd5 | 523 | return true; |
garyservin | 0:9e9b7db60fd5 | 524 | } |
garyservin | 0:9e9b7db60fd5 | 525 | } |
garyservin | 0:9e9b7db60fd5 | 526 | return false; |
garyservin | 0:9e9b7db60fd5 | 527 | } |
garyservin | 0:9e9b7db60fd5 | 528 | bool getParam(const char* name, char** param, int length=1){ |
garyservin | 0:9e9b7db60fd5 | 529 | if (requestParam(name) ){ |
garyservin | 0:9e9b7db60fd5 | 530 | if (length == req_param_resp.strings_length){ |
garyservin | 0:9e9b7db60fd5 | 531 | //copy it over |
garyservin | 0:9e9b7db60fd5 | 532 | for(int i=0; i<length; i++) |
garyservin | 0:9e9b7db60fd5 | 533 | strcpy(param[i],req_param_resp.strings[i]); |
garyservin | 0:9e9b7db60fd5 | 534 | return true; |
garyservin | 0:9e9b7db60fd5 | 535 | } |
garyservin | 0:9e9b7db60fd5 | 536 | } |
garyservin | 0:9e9b7db60fd5 | 537 | return false; |
garyservin | 0:9e9b7db60fd5 | 538 | } |
garyservin | 0:9e9b7db60fd5 | 539 | }; |
garyservin | 0:9e9b7db60fd5 | 540 | |
garyservin | 0:9e9b7db60fd5 | 541 | } |
garyservin | 0:9e9b7db60fd5 | 542 | |
garyservin | 0:9e9b7db60fd5 | 543 | #endif |