18 #ifndef ROS_NODE_HANDLE_H_ 19 #define ROS_NODE_HANDLE_H_ 28 #define SYNC_SECONDS 5 30 #define MODE_FIRST_FF 0 39 #define MODE_PROTOCOL_VER 1 40 #define PROTOCOL_VER1 0xff // through groovy 41 #define PROTOCOL_VER2 0xfe // in hydro 42 #define PROTOCOL_VER PROTOCOL_VER2 45 #define MODE_SIZE_CHECKSUM 4 // checksum for msg size received from size L and H 46 #define MODE_TOPIC_L 5 // waiting for topic id 47 #define MODE_TOPIC_H 6 48 #define MODE_MESSAGE 7 49 #define MODE_MSG_CHECKSUM 8 // checksum for msg and topic id 52 #define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data 76 template<
class Hardware,
77 int MAX_SUBSCRIBERS=25,
78 int MAX_PUBLISHERS=25,
92 uint8_t message_in[INPUT_SIZE];
93 uint8_t message_out[OUTPUT_SIZE];
104 for(
unsigned int i=0; i< MAX_PUBLISHERS; i++)
107 for(
unsigned int i=0; i< MAX_SUBSCRIBERS; i++)
110 for(
unsigned int i=0; i< INPUT_SIZE; i++)
113 for(
unsigned int i=0; i< OUTPUT_SIZE; i++)
116 req_param_resp.ints_length = 0;
117 req_param_resp.ints = NULL;
118 req_param_resp.floats_length = 0;
119 req_param_resp.floats = NULL;
120 req_param_resp.ints_length = 0;
121 req_param_resp.ints = NULL;
139 hardware_.init(portName);
170 uint32_t c_time = hardware_.time();
171 if( (c_time - last_sync_receive_time) > (
SYNC_SECONDS*2200) ){
177 if (c_time > last_msg_timeout_time){
185 int data = hardware_.read();
190 message_in[index_++] = data;
209 if (configured_ ==
false)
221 if( (checksum_%256) == 255)
236 if( (checksum_%256) == 255){
240 last_sync_time = c_time;
241 last_sync_receive_time = c_time;
244 syncTime(message_in);
246 req_param_resp.deserialize(message_in);
247 param_recieved=
true;
251 if(subscribers[topic_-100])
252 subscribers[topic_-100]->
callback( message_in );
259 if( configured_ && ((c_time-last_sync_time) > (
SYNC_SECONDS*500) )){
261 last_sync_time = c_time;
281 rt_time = hardware_.time();
287 uint32_t offset = hardware_.time() - rt_time;
290 t.
data.sec += offset/1000;
291 t.
data.nsec += (offset%1000)*1000000UL;
293 this->setNow(t.
data);
294 last_sync_receive_time = hardware_.time();
299 uint32_t ms = hardware_.time();
301 current_time.sec = ms/1000 + sec_offset;
302 current_time.nsec = (ms%1000)*1000000UL + nsec_offset;
309 uint32_t ms = hardware_.time();
310 sec_offset = new_now.sec - ms/1000 - 1;
311 nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
322 for(
int i = 0; i < MAX_PUBLISHERS; i++){
323 if(publishers[i] == 0){
325 p.id_ = i+100+MAX_SUBSCRIBERS;
334 template<
typename MsgT>
336 for(
int i = 0; i < MAX_SUBSCRIBERS; i++){
337 if(subscribers[i] == 0){
347 template<
typename MReq,
typename MRes>
349 bool v = advertise(srv.pub);
350 for(
int i = 0; i < MAX_SUBSCRIBERS; i++){
351 if(subscribers[i] == 0){
361 template<
typename MReq,
typename MRes>
363 bool v = advertise(srv.pub);
364 for(
int i = 0; i < MAX_SUBSCRIBERS; i++){
365 if(subscribers[i] == 0){
378 for(i = 0; i < MAX_PUBLISHERS; i++)
380 if(publishers[i] != 0)
383 ti.
topic_name = (
char *) publishers[i]->topic_;
384 ti.
message_type = (
char *) publishers[i]->msg_->getType();
385 ti.
md5sum = (
char *) publishers[i]->msg_->getMD5();
387 publish( publishers[i]->getEndpointType(), &ti );
390 for(i = 0; i < MAX_SUBSCRIBERS; i++)
392 if(subscribers[i] != 0)
395 ti.
topic_name = (
char *) subscribers[i]->topic_;
396 ti.
message_type = (
char *) subscribers[i]->getMsgType();
397 ti.
md5sum = (
char *) subscribers[i]->getMsgMD5();
399 publish( subscribers[i]->getEndpointType(), &ti );
407 if(
id >= 100 && !configured_)
411 uint16_t l = msg->
serialize(message_out+7);
414 message_out[0] = 0xff;
416 message_out[2] = (uint8_t) ((uint16_t)l&255);
417 message_out[3] = (uint8_t) ((uint16_t)l>>8);
418 message_out[4] = 255 - ((message_out[2] + message_out[3])%256);
419 message_out[5] = (uint8_t) ((int16_t)
id&255);
420 message_out[6] = (uint8_t) ((int16_t)
id>>8);
424 for(
int i =5; i<l+7; i++)
425 chk += message_out[i];
427 message_out[l++] = 255 - (chk%256);
429 if( l <= OUTPUT_SIZE ){
430 hardware_.write(message_out, l);
433 logerror(
"Message from device dropped: message larger than buffer.");
443 void log(
char byte,
const char * msg){
476 param_recieved =
false;
478 req.
name = (
char*)name;
480 uint16_t end_time = hardware_.time() + time_out;
481 while(!param_recieved ){
483 if (hardware_.time() > end_time)
return false;
490 if (requestParam(name) ){
493 for(
int i=0; i<length; i++)
494 param[i] = req_param_resp.
ints[i];
501 if (requestParam(name) ){
504 for(
int i=0; i<length; i++)
505 param[i] = req_param_resp.
floats[i];
512 if (requestParam(name) ){
515 for(
int i=0; i<length; i++)
516 strcpy(param[i],req_param_resp.
strings[i]);
void logdebug(const char *msg)
void logwarn(const char *msg)
#define MODE_PROTOCOL_VER
void syncTime(uint8_t *data)
void log(char byte, const char *msg)
virtual bool connected()=0
bool subscribe(Subscriber< MsgT > &s)
void logerror(const char *msg)
uint32_t last_msg_timeout_time
void logfatal(const char *msg)
virtual void callback(unsigned char *data)=0
virtual int publish(int id, const Msg *msg)
virtual int deserialize(unsigned char *inbuffer)
#define MODE_SIZE_CHECKSUM
void loginfo(const char *msg)
bool requestParam(const char *name, int time_out=1000)
uint32_t last_sync_receive_time
void setNow(Time &new_now)
virtual int publish(int id, const Msg *msg)=0
ROSTIME_DECL void normalizeSecNSec(uint32_t &sec, uint32_t &nsec)
bool getParam(const char *name, int *param, int length=1)
#define MODE_MSG_CHECKSUM
const char * message_type
bool getParam(const char *name, float *param, int length=1)
bool getParam(const char *name, char **param, int length=1)
bool advertise(Publisher &p)
void initNode(char *portName)
virtual int serialize(unsigned char *outbuffer) const =0
rosserial_msgs::RequestParamResponse req_param_resp
bool advertiseService(ServiceServer< MReq, MRes > &srv)
bool serviceClient(ServiceClient< MReq, MRes > &srv)