Soft robot team / ros_lib_melodic

ros melodic library with custom message

Dependents:   Robot_team1_QEI_Douglas Robot_team1

ros_lib/ros/node_handle.h@3:b964e3f71102, 2019-12-03 (annotated)

Committer:
florine_van
Date:
Tue Dec 03 09:39:29 2019 +0000
Revision:
3:b964e3f71102
Parent:
0:020db18a476d 
Clean code and remove unused lines

Who changed what in which revision?

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