From d6ac011485c99169d71d96ffd2f6bb333067f427 Mon Sep 17 00:00:00 2001 From: Robin Vanhove Date: Fri, 17 Nov 2017 18:29:13 +0100 Subject: [PATCH] update --- src/ros_lib/RosQtSocket.cpp | 6 + src/ros_lib/node_handle.cpp | 437 ++++++++++++++++++++++++++++ src/ros_lib/ros/node_handle.h | 441 +++-------------------------- src/ros_lib/ros/service_client.h | 4 - test/Basic/Basic.pro | 1 + test/Basic/Basic.pro.user | 2 +- test/LoopbackTest/LoopbackTest.pro | 1 + test/LoopbackTest/node.cpp | 2 +- 8 files changed, 487 insertions(+), 407 deletions(-) create mode 100644 src/ros_lib/node_handle.cpp diff --git a/src/ros_lib/RosQtSocket.cpp b/src/ros_lib/RosQtSocket.cpp index e311717..2ef6e1a 100644 --- a/src/ros_lib/RosQtSocket.cpp +++ b/src/ros_lib/RosQtSocket.cpp @@ -1,5 +1,7 @@ #include "ros/RosQtSocket.h" +//#define ROSQT_VERBOSE + using std::string; using std::cerr; using std::endl; @@ -40,7 +42,9 @@ int64_t RosQtSocket::read(unsigned char *data, int max_length) { if(socket_.state() != QAbstractSocket::ConnectedState) { +#ifdef ROSQT_VERBOSE std::cerr << "Failed to receive data from server, not connected " << std::endl; +#endif doConnect(); return -1; } @@ -59,7 +63,9 @@ bool RosQtSocket::write(const unsigned char *data, int length) { if(socket_valid_ && socket_.state() != QAbstractSocket::ConnectedState) { +#ifdef ROSQT_VERBOSE std::cerr << "Failed to write data to the server, not connected " << std::endl; +#endif doConnect(); return false; } diff --git a/src/ros_lib/node_handle.cpp b/src/ros_lib/node_handle.cpp new file mode 100644 index 0000000..a7299ae --- /dev/null +++ b/src/ros_lib/node_handle.cpp @@ -0,0 +1,437 @@ +#include "ros/node_handle.h" + +#ifdef ERROR + // ach, windows.h polluting everything again, + // clashes with autogenerated rosgraph_msgs/Log.h + #undef ERROR +#endif +#include "rosserial_msgs/Log.h" + +using namespace ros; + +namespace ros { + const uint8_t SYNC_SECONDS = 5; + const uint8_t MODE_FIRST_FF = 0; + /* + * The second sync byte is a protocol version. It's value is 0xff for the first + * version of the rosserial protocol (used up to hydro), 0xfe for the second version + * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable + * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy + * rosserial_arduino. It must be changed in both this file and in + * rosserial_python/src/rosserial_python/SerialClient.py + */ + const uint8_t MODE_PROTOCOL_VER = 1; + const uint8_t PROTOCOL_VER1 = 0xff; // through groovy + const uint8_t PROTOCOL_VER2 = 0xfe; // in hydro + const uint8_t PROTOCOL_VER = PROTOCOL_VER2; + const uint8_t MODE_SIZE_B1 = 2; // LSB + const uint8_t MODE_SIZE_B2 = 3; + const uint8_t MODE_SIZE_B3 = 4; + const uint8_t MODE_SIZE_B4 = 5; // MSB + const uint8_t MODE_SIZE_CHECKSUM = 6; // checksum for msg size received from size L and H + const uint8_t MODE_TOPIC_L = 7; // waiting for topic id + const uint8_t MODE_TOPIC_H = 8; + const uint8_t MODE_MESSAGE = 9; + const uint8_t MODE_MSG_CHECKSUM = 10; // checksum for msg and topic id + + const uint32_t SERIAL_MSG_TIMEOUT = 2000; // 2000 milliseconds to recieve all of message data + const uint32_t READ_BUFFER_SIZE = 4096; // at each read() we will ask up to READ_BUFFER_SIZE bytes + + const size_t MESSAGE_HEADER_BYTES = 10; // size of the header + const size_t BUFFER_OVER_ALLOCATION = 100*1024; //if the tx or rx buffer hasn't enough capacity, we will allocate this more +} + +ros::NodeHandle::NodeHandle(uint32_t input_size, uint32_t output_size, QObject *parent) : + NodeHandleBase_(parent), + INPUT_SIZE(input_size), + OUTPUT_SIZE(output_size), + configured_(false), + isActive_(false) +{ + message_in.resize(INPUT_SIZE); + message_out.resize(OUTPUT_SIZE); + + for(unsigned int i=0; i< INPUT_SIZE; i++) + message_in[i] = 0; + + for(unsigned int i=0; i< OUTPUT_SIZE; i++) + message_out[i] = 0; + + connect(&hardware_, SIGNAL(readyRead()), this, SLOT(onReadyRead())); + startTimer(100); +} + +void ros::NodeHandle::timerEvent(QTimerEvent *event) +{ + (void)event; + spinOnce(); +} + +void ros::NodeHandle::open(const std::string &hostName, uint16_t port){ + isActive_ = true; + hardware_.open(hostName, port); + mode_ = 0; + bytes_ = 0; + index_ = 0; + topic_ = 0; +} + +void ros::NodeHandle::close() +{ + isActive_ = false; + setConfigured(false); + hardware_.close(); +} + +int ros::NodeHandle::spinOnce(){ + unsigned char buffer[READ_BUFFER_SIZE]; + + if(!isActive_) + return 0; + + /* restart if timed out */ + uint32_t c_time = hardware_.time(); + if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){ + setConfigured(false); + } + + /* reset if message has timed out */ + if ( mode_ != MODE_FIRST_FF){ + if (c_time > last_msg_timeout_time){ + mode_ = MODE_FIRST_FF; + } + } + + /* while available buffer, read data */ + while(true) + { + int64_t result = hardware_.read(buffer, READ_BUFFER_SIZE); + if(result == 0) + { + // no more data + break; + } + else if(result < 0) + { + // connection lost + setConfigured(false); + return -2; + } + + for(size_t i=0 ; i<(size_t)result ; i++) + { + unsigned char data = buffer[i]; + + checksum_ += data; + if( mode_ == MODE_MESSAGE ){ /* message data being recieved */ + message_in[index_++] = data; + bytes_--; + if(bytes_ == 0) /* is message complete? if so, checksum */ + mode_ = MODE_MSG_CHECKSUM; + }else if( mode_ == MODE_FIRST_FF ){ + if(data == 0xff){ + mode_++; + last_msg_timeout_time = c_time + SERIAL_MSG_TIMEOUT; + } + else if( hardware_.time() - c_time > (SYNC_SECONDS*1000)){ + /* We have been stuck in spinOnce too long, return error */ + setConfigured(false); + return -2; + } + }else if( mode_ == MODE_PROTOCOL_VER ){ + if(data == PROTOCOL_VER){ + mode_++; + }else{ + mode_ = MODE_FIRST_FF; + if (configured_ == false) + requestSyncTime(); /* send a msg back showing our protocol version */ + } + }else if( mode_ == MODE_SIZE_B1 ){ /* bottom half of message size */ + bytes_ = data; + index_ = 0; + mode_++; + checksum_ = data; /* first byte for calculating size checksum */ + }else if( mode_ == MODE_SIZE_B2 ){ /* part 2 of message size */ + bytes_ += data<<8; + mode_++; + }else if( mode_ == MODE_SIZE_B3 ){ /* part 3 of message size */ + bytes_ += data<<16; + mode_++; + }else if( mode_ == MODE_SIZE_B4 ){ /* part 4 of message size */ + bytes_ += data<<24; + mode_++; + } + else if( mode_ == MODE_SIZE_CHECKSUM ){ + if( (checksum_%256) == 255) + { + mode_++; + } + else + { + mode_ = MODE_FIRST_FF; /* Abandon the frame if the msg len is wrong */ + } + }else if( mode_ == MODE_TOPIC_L ){ /* bottom half of topic id */ + topic_ = data; + mode_++; + checksum_ = data; /* first byte included in checksum */ + }else if( mode_ == MODE_TOPIC_H ){ /* top half of topic id */ + topic_ += data<<8; + mode_ = MODE_MESSAGE; + + // if needed expand buffer + if(bytes_ + MESSAGE_HEADER_BYTES > message_in.size()) + { + message_in.resize(bytes_ + MESSAGE_HEADER_BYTES + BUFFER_OVER_ALLOCATION); + } + + if(bytes_ == 0) + mode_ = MODE_MSG_CHECKSUM; + }else if( mode_ == MODE_MSG_CHECKSUM ){ /* do checksum */ + mode_ = MODE_FIRST_FF; + if( (checksum_%256) == 255){ + if(topic_ == TopicInfo::ID_PUBLISHER){ + requestSyncTime(); + negotiateTopics(); + last_sync_time = c_time; + last_sync_receive_time = c_time; + return -1; + }else if(topic_ == TopicInfo::ID_TIME){ + syncTime(message_in.data()); + }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){ + req_param_resp.deserialize(message_in.data()); + param_recieved= true; + }else if(topic_ == TopicInfo::ID_TX_STOP){ + setConfigured(false); + }else{ + if(subscribers[topic_-100]) + subscribers[topic_-100]->callback( message_in.data()); + } + } + } + } + } + + /* occasionally sync time */ + if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){ + requestSyncTime(); + last_sync_time = c_time; + } + + return 0; +} + +bool ros::NodeHandle::connected() const { + return configured_; +} + +void ros::NodeHandle::requestSyncTime() +{ + std_msgs::Time t; + publish(TopicInfo::ID_TIME, &t); + rt_time = hardware_.time(); +} + +void ros::NodeHandle::syncTime(uint8_t *data) +{ + std_msgs::Time t; + uint32_t offset = hardware_.time() - rt_time; + + t.deserialize(data); + t.data.sec += offset/1000; + t.data.nsec += (offset%1000)*1000000UL; + + this->setNow(t.data); + last_sync_receive_time = hardware_.time(); +} + +ros::Time ros::NodeHandle::now() +{ + uint32_t ms = hardware_.time(); + Time current_time; + current_time.sec = ms/1000 + sec_offset; + current_time.nsec = (ms%1000)*1000000UL + nsec_offset; + normalizeSecNSec(current_time.sec, current_time.nsec); + return current_time; +} + +void ros::NodeHandle::setNow(ros::Time &new_now) +{ + uint32_t ms = hardware_.time(); + sec_offset = new_now.sec - ms/1000 - 1; + nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL; + normalizeSecNSec(sec_offset, nsec_offset); +} + +void ros::NodeHandle::negotiateTopics() +{ + rosserial_msgs::TopicInfo ti; + size_t i; + for(i = 0; i < publishers.size(); i++) + { + if(publishers[i] != 0) // non-empty slot + { + ti.topic_id = publishers[i]->id_; + ti.topic_name = publishers[i]->topic_; + ti.message_type = publishers[i]->getMsgType(); + ti.md5sum = publishers[i]->getMsgMD5(); + ti.buffer_size = OUTPUT_SIZE; + publish( publishers[i]->getEndpointType(), &ti ); + } + } + for(i = 0; i < subscribers.size(); i++) + { + if(subscribers[i] != 0) // non-empty slot + { + ti.topic_id = subscribers[i]->id_; + ti.topic_name = subscribers[i]->topic_; + ti.message_type = subscribers[i]->getMsgType(); + ti.md5sum = subscribers[i]->getMsgMD5(); + ti.buffer_size = INPUT_SIZE; + publish( subscribers[i]->getEndpointType(), &ti ); + } + } + setConfigured(true); +} + +int ros::NodeHandle::publish(int id, const ros::Msg *msg) +{ + if(id >= 100 && !configured_) + return 0; + + /* serialize message */ + size_t expected_l = msg->serialize_size(); + + if(expected_l + MESSAGE_HEADER_BYTES > message_out.size()) + { + message_out.resize(expected_l + MESSAGE_HEADER_BYTES + BUFFER_OVER_ALLOCATION); + } + + size_t l = msg->serialize(message_out.data()+9); + + if(expected_l != l) + { + logerror("Internal error : expected_l != l ; memory could be corrupted"); + } + + /* setup the header */ + message_out[0] = 0xff; + message_out[1] = PROTOCOL_VER; + message_out[2] = (uint8_t) ((uint32_t)l&255); + message_out[3] = (uint8_t) ((uint32_t)l>>8); + message_out[4] = (uint8_t) ((uint32_t)l>>16); + message_out[5] = (uint8_t) ((uint32_t)l>>24); + message_out[6] = 255 - ((message_out[2] + message_out[3] + message_out[4] + message_out[5])%256); + message_out[7] = (uint8_t) ((int16_t)id&255); + message_out[8] = (uint8_t) ((int16_t)id>>8); + + /* calculate checksum */ + int chk = 0; + l += 9; + for(size_t i =7; i end_time) { + logwarn("Failed to get param: timeout expired"); + return false; + } + } + return true; +} + +bool ros::NodeHandle::getParam(const std::string &name, int *param, unsigned int length, int timeout){ + if (requestParam(name, timeout) ){ + if (length == req_param_resp.ints.size()){ + //copy it over + for(size_t i=0; i + #include #include "std_msgs/Time.h" #include "rosserial_msgs/TopicInfo.h" -#include "rosserial_msgs/Log.h" #include "rosserial_msgs/RequestParam.h" #include "ros/msg.h" -#include - namespace ros { class NodeHandleBase_ : public QObject{ - Q_OBJECT public: NodeHandleBase_(QObject *parent=0) : QObject(parent){} virtual int publish(int id, const Msg* msg)=0; virtual int spinOnce()=0; - virtual bool connected()=0; + virtual bool connected() const=0; }; } @@ -66,42 +64,15 @@ class NodeHandleBase_ : public QObject{ namespace ros { -const uint8_t SYNC_SECONDS = 5; -const uint8_t MODE_FIRST_FF = 0; -/* - * The second sync byte is a protocol version. It's value is 0xff for the first - * version of the rosserial protocol (used up to hydro), 0xfe for the second version - * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable - * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy - * rosserial_arduino. It must be changed in both this file and in - * rosserial_python/src/rosserial_python/SerialClient.py - */ -const uint8_t MODE_PROTOCOL_VER = 1; -const uint8_t PROTOCOL_VER1 = 0xff; // through groovy -const uint8_t PROTOCOL_VER2 = 0xfe; // in hydro -const uint8_t PROTOCOL_VER = PROTOCOL_VER2; -const uint8_t MODE_SIZE_B1 = 2; // LSB -const uint8_t MODE_SIZE_B2 = 3; -const uint8_t MODE_SIZE_B3 = 4; -const uint8_t MODE_SIZE_B4 = 5; // MSB -const uint8_t MODE_SIZE_CHECKSUM = 6; // checksum for msg size received from size L and H -const uint8_t MODE_TOPIC_L = 7; // waiting for topic id -const uint8_t MODE_TOPIC_H = 8; -const uint8_t MODE_MESSAGE = 9; -const uint8_t MODE_MSG_CHECKSUM = 10; // checksum for msg and topic id - -const uint32_t SERIAL_MSG_TIMEOUT = 2000; // 2000 milliseconds to recieve all of message data -const uint32_t READ_BUFFER_SIZE = 4096; // at each read() we will ask up to READ_BUFFER_SIZE bytes - -const size_t MESSAGE_HEADER_BYTES = 10; // size of the header -const size_t BUFFER_OVER_ALLOCATION = 100*1024; //if the tx or rx buffer hasn't enough capacity, we will allocate this more - using rosserial_msgs::TopicInfo; /* Node Handle */ class NodeHandle : public NodeHandleBase_ { - Q_OBJECT + Q_OBJECT + + Q_PROPERTY(bool isConnected READ connected NOTIFY isConnectedChanged) + protected: RosQtSocket hardware_; @@ -124,48 +95,15 @@ class NodeHandle : public NodeHandleBase_ * Setup Functions */ public: - NodeHandle(uint32_t input_size=10000, uint32_t output_size=10000, QObject *parent=0) : - NodeHandleBase_(parent), - INPUT_SIZE(input_size), - OUTPUT_SIZE(output_size), - configured_(false), - isActive_(false) - { - message_in.resize(INPUT_SIZE); - message_out.resize(OUTPUT_SIZE); - - for(unsigned int i=0; i< INPUT_SIZE; i++) - message_in[i] = 0; + NodeHandle(uint32_t input_size=10000, uint32_t output_size=10000, QObject *parent=0); - for(unsigned int i=0; i< OUTPUT_SIZE; i++) - message_out[i] = 0; - - connect(&hardware_, SIGNAL(readyRead()), this, SLOT(onReadyRead())); - startTimer(100); - } - - void timerEvent(QTimerEvent *event) - { - (void)event; - spinOnce(); - } + void timerEvent(QTimerEvent *event); /* Start a named port, which may be network server IP, initialize buffers */ - void open(const std::string &hostName, uint16_t port = 11411){ - isActive_ = true; - hardware_.open(hostName, port); - mode_ = 0; - bytes_ = 0; - index_ = 0; - topic_ = 0; - } + void open(const std::string &hostName, uint16_t port = 11411); - void close() - { - isActive_ = false; - configured_ = false; - hardware_.close(); - } + /* Disconnect from server */ + void close(); protected: //State machine variables for spinOnce @@ -189,194 +127,27 @@ class NodeHandle : public NodeHandleBase_ */ - virtual int spinOnce(){ - unsigned char buffer[READ_BUFFER_SIZE]; - - if(!isActive_) - return 0; - - /* restart if timed out */ - uint32_t c_time = hardware_.time(); - if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){ - configured_ = false; - } - - /* reset if message has timed out */ - if ( mode_ != MODE_FIRST_FF){ - if (c_time > last_msg_timeout_time){ - mode_ = MODE_FIRST_FF; - } - } - - /* while available buffer, read data */ - while(true) - { - int64_t result = hardware_.read(buffer, READ_BUFFER_SIZE); - if(result == 0) - { - // no more data - break; - } - else if(result < 0) - { - // connection lost - configured_ = false; - return -2; - } - - for(size_t i=0 ; i<(size_t)result ; i++) - { - unsigned char data = buffer[i]; - - checksum_ += data; - if( mode_ == MODE_MESSAGE ){ /* message data being recieved */ - message_in[index_++] = data; - bytes_--; - if(bytes_ == 0) /* is message complete? if so, checksum */ - mode_ = MODE_MSG_CHECKSUM; - }else if( mode_ == MODE_FIRST_FF ){ - if(data == 0xff){ - mode_++; - last_msg_timeout_time = c_time + SERIAL_MSG_TIMEOUT; - } - else if( hardware_.time() - c_time > (SYNC_SECONDS*1000)){ - /* We have been stuck in spinOnce too long, return error */ - configured_=false; - return -2; - } - }else if( mode_ == MODE_PROTOCOL_VER ){ - if(data == PROTOCOL_VER){ - mode_++; - }else{ - mode_ = MODE_FIRST_FF; - if (configured_ == false) - requestSyncTime(); /* send a msg back showing our protocol version */ - } - }else if( mode_ == MODE_SIZE_B1 ){ /* bottom half of message size */ - bytes_ = data; - index_ = 0; - mode_++; - checksum_ = data; /* first byte for calculating size checksum */ - }else if( mode_ == MODE_SIZE_B2 ){ /* part 2 of message size */ - bytes_ += data<<8; - mode_++; - }else if( mode_ == MODE_SIZE_B3 ){ /* part 3 of message size */ - bytes_ += data<<16; - mode_++; - }else if( mode_ == MODE_SIZE_B4 ){ /* part 4 of message size */ - bytes_ += data<<24; - mode_++; - } - else if( mode_ == MODE_SIZE_CHECKSUM ){ - if( (checksum_%256) == 255) - { - mode_++; - } - else - { - mode_ = MODE_FIRST_FF; /* Abandon the frame if the msg len is wrong */ - } - }else if( mode_ == MODE_TOPIC_L ){ /* bottom half of topic id */ - topic_ = data; - mode_++; - checksum_ = data; /* first byte included in checksum */ - }else if( mode_ == MODE_TOPIC_H ){ /* top half of topic id */ - topic_ += data<<8; - mode_ = MODE_MESSAGE; - - // if needed expand buffer - if(bytes_ + MESSAGE_HEADER_BYTES > message_in.size()) - { - message_in.resize(bytes_ + MESSAGE_HEADER_BYTES + BUFFER_OVER_ALLOCATION); - } - - if(bytes_ == 0) - mode_ = MODE_MSG_CHECKSUM; - }else if( mode_ == MODE_MSG_CHECKSUM ){ /* do checksum */ - mode_ = MODE_FIRST_FF; - if( (checksum_%256) == 255){ - if(topic_ == TopicInfo::ID_PUBLISHER){ - requestSyncTime(); - negotiateTopics(); - last_sync_time = c_time; - last_sync_receive_time = c_time; - return -1; - }else if(topic_ == TopicInfo::ID_TIME){ - syncTime(message_in.data()); - }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){ - req_param_resp.deserialize(message_in.data()); - param_recieved= true; - }else if(topic_ == TopicInfo::ID_TX_STOP){ - configured_ = false; - }else{ - if(subscribers[topic_-100]) - subscribers[topic_-100]->callback( message_in.data()); - } - } - } - } - } - - /* occasionally sync time */ - if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){ - requestSyncTime(); - last_sync_time = c_time; - } - - return 0; - } + virtual int spinOnce(); /* Are we connected to the PC? */ - virtual bool connected() { - return configured_; - } + bool connected() const override; /******************************************************************** * Time functions */ - void requestSyncTime() - { - std_msgs::Time t; - publish(TopicInfo::ID_TIME, &t); - rt_time = hardware_.time(); - } - - void syncTime(uint8_t * data) - { - std_msgs::Time t; - uint32_t offset = hardware_.time() - rt_time; - - t.deserialize(data); - t.data.sec += offset/1000; - t.data.nsec += (offset%1000)*1000000UL; + void requestSyncTime(); - this->setNow(t.data); - last_sync_receive_time = hardware_.time(); - } + void syncTime(uint8_t * data); - Time now() - { - uint32_t ms = hardware_.time(); - Time current_time; - current_time.sec = ms/1000 + sec_offset; - current_time.nsec = (ms%1000)*1000000UL + nsec_offset; - normalizeSecNSec(current_time.sec, current_time.nsec); - return current_time; - } + Time now(); - void setNow( Time & new_now ) - { - uint32_t ms = hardware_.time(); - sec_offset = new_now.sec - ms/1000 - 1; - nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL; - normalizeSecNSec(sec_offset, nsec_offset); - } + void setNow( Time & new_now ); /******************************************************************** - * Topic Management - */ + * Topic Management + */ /* Register a new publisher */ template @@ -418,113 +189,29 @@ class NodeHandle : public NodeHandleBase_ return v; } - void negotiateTopics() - { - rosserial_msgs::TopicInfo ti; - size_t i; - for(i = 0; i < publishers.size(); i++) - { - if(publishers[i] != 0) // non-empty slot - { - ti.topic_id = publishers[i]->id_; - ti.topic_name = publishers[i]->topic_; - ti.message_type = publishers[i]->getMsgType(); - ti.md5sum = publishers[i]->getMsgMD5(); - ti.buffer_size = OUTPUT_SIZE; - publish( publishers[i]->getEndpointType(), &ti ); - } - } - for(i = 0; i < subscribers.size(); i++) - { - if(subscribers[i] != 0) // non-empty slot - { - ti.topic_id = subscribers[i]->id_; - ti.topic_name = subscribers[i]->topic_; - ti.message_type = subscribers[i]->getMsgType(); - ti.md5sum = subscribers[i]->getMsgMD5(); - ti.buffer_size = INPUT_SIZE; - publish( subscribers[i]->getEndpointType(), &ti ); - } - } - configured_ = true; - } + void negotiateTopics(); - virtual int publish(int id, const Msg * msg) - { - if(id >= 100 && !configured_) - return 0; - - /* serialize message */ - size_t expected_l = msg->serialize_size(); - - if(expected_l + MESSAGE_HEADER_BYTES > message_out.size()) - { - message_out.resize(expected_l + MESSAGE_HEADER_BYTES + BUFFER_OVER_ALLOCATION); - } - - size_t l = msg->serialize(message_out.data()+9); - - if(expected_l != l) - { - logerror("Internal error : expected_l != l ; memory could be corrupted"); - } - - /* setup the header */ - message_out[0] = 0xff; - message_out[1] = PROTOCOL_VER; - message_out[2] = (uint8_t) ((uint32_t)l&255); - message_out[3] = (uint8_t) ((uint32_t)l>>8); - message_out[4] = (uint8_t) ((uint32_t)l>>16); - message_out[5] = (uint8_t) ((uint32_t)l>>24); - message_out[6] = 255 - ((message_out[2] + message_out[3] + message_out[4] + message_out[5])%256); - message_out[7] = (uint8_t) ((int16_t)id&255); - message_out[8] = (uint8_t) ((int16_t)id>>8); - - /* calculate checksum */ - int chk = 0; - l += 9; - for(size_t i =7; i end_time) { - logwarn("Failed to get param: timeout expired"); - return false; - } - } - return true; - } + bool requestParam(const std::string &name, int time_out = 1000); public: - bool getParam(const char* name, int* param, unsigned int length =1, int timeout = 1000){ - if (requestParam(name, timeout) ){ - if (length == req_param_resp.ints.size()){ - //copy it over - for(size_t i=0; itopic_ << std::endl; - } if(!pub.nh_->connected()) return; waiting = true; cb_ = callback; diff --git a/test/Basic/Basic.pro b/test/Basic/Basic.pro index 21ef07a..683b3ff 100644 --- a/test/Basic/Basic.pro +++ b/test/Basic/Basic.pro @@ -17,6 +17,7 @@ SOURCES += main.cpp \ ../../src/ros_lib/duration.cpp \ ../../src/ros_lib/time.cpp \ ../../src/ros_lib/RosQtSocket.cpp \ + ../../src/ros_lib/node_handle.cpp \ node.cpp DEFINES += QT_DEPRECATED_WARNINGS diff --git a/test/Basic/Basic.pro.user b/test/Basic/Basic.pro.user index 4761995..6d71bbc 100644 --- a/test/Basic/Basic.pro.user +++ b/test/Basic/Basic.pro.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/test/LoopbackTest/LoopbackTest.pro b/test/LoopbackTest/LoopbackTest.pro index cf63452..990bfec 100644 --- a/test/LoopbackTest/LoopbackTest.pro +++ b/test/LoopbackTest/LoopbackTest.pro @@ -17,6 +17,7 @@ SOURCES += main.cpp \ ../../src/ros_lib/duration.cpp \ ../../src/ros_lib/time.cpp \ ../../src/ros_lib/RosQtSocket.cpp \ + ../../src/ros_lib/node_handle.cpp \ node.cpp DEFINES += QT_DEPRECATED_WARNINGS diff --git a/test/LoopbackTest/node.cpp b/test/LoopbackTest/node.cpp index 2a683be..3f0e927 100644 --- a/test/LoopbackTest/node.cpp +++ b/test/LoopbackTest/node.cpp @@ -25,7 +25,7 @@ Node::Node(): } printf ("Connecting to server at %s\n", ros_master.c_str()); - nh.initNode (ros_master.c_str()); + nh.open(ros_master.c_str()); nh.subscribe (chatterSub); nh.advertise(chatter);