This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial. This program contains an example.
Dependencies: rosserial_mbed_lib mbed Servo
Diff: ros/node_handle.h
- Revision:
- 0:06fc856e99ca
- Child:
- 1:098e75fd5ad2
diff -r 000000000000 -r 06fc856e99ca ros/node_handle.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros/node_handle.h Fri Aug 19 09:06:16 2011 +0000 @@ -0,0 +1,415 @@ +/* + * NodeHandle.h + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * + * Author: Michael Ferguson , Adam Stambler + */ + +#ifndef ROS_NODE_HANDLE_H_ +#define ROS_NODE_HANDLE_H_ + +#include "../std_msgs/Time.h" +#include "../rosserial_msgs/TopicInfo.h" +#include "../rosserial_msgs/Log.h" +#include "../rosserial_msgs/RequestParam.h" + +#define SYNC_SECONDS 5 + +#define MODE_FIRST_FF 0 +#define MODE_SECOND_FF 1 +#define MODE_TOPIC_L 2 // waiting for topic id +#define MODE_TOPIC_H 3 +#define MODE_SIZE_L 4 // waiting for message size +#define MODE_SIZE_H 5 +#define MODE_MESSAGE 6 +#define MODE_CHECKSUM 7 + + +#define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data + +#include "node_output.h" + +#include "publisher.h" +#include "msg_receiver.h" +#include "subscriber.h" +#include "rosserial_ids.h" +#include "service_server.h" + + +namespace ros { + +using rosserial_msgs::TopicInfo; + +/* Node Handle */ +template<class Hardware, int MAX_SUBSCRIBERS=25, int MAX_PUBLISHERS=25, +int INPUT_SIZE=512, int OUTPUT_SIZE=512> +class NodeHandle_ { + +protected: + Hardware hardware_; + NodeOutput<Hardware, OUTPUT_SIZE> no_; + + /* time used for syncing */ + unsigned long rt_time; + + /* used for computing current time */ + unsigned long sec_offset, nsec_offset; + + unsigned char message_in[INPUT_SIZE]; + + Publisher * publishers[MAX_PUBLISHERS]; + MsgReceiver * receivers[MAX_SUBSCRIBERS]; + + /****************************** + * Setup Functions + */ +public: + NodeHandle_() : no_(&hardware_) { + } + + Hardware* getHardware() { + return &hardware_; + } + + /* Start serial, initialize buffers */ + void initNode() { + hardware_.init(); + mode_ = 0; + bytes_ = 0; + index_ = 0; + topic_ = 0; + total_receivers=0; + }; + + +protected: + + //State machine variables for spinOnce + int mode_; + int bytes_; + int topic_; + int index_; + int checksum_; + + int total_receivers; + + + /* used for syncing the time */ + unsigned long last_sync_time; + unsigned long last_sync_receive_time; + unsigned long last_msg_timeout_time; + + bool registerReceiver(MsgReceiver* rcv) { + if (total_receivers >= MAX_SUBSCRIBERS) return false; + receivers[total_receivers] = rcv; + rcv->id_ = 100+total_receivers; + total_receivers++; + return true; + } + + +public: + /* This function goes in your loop() function, it handles + * serial input and callbacks for subscribers. + */ + + virtual void spinOnce() { + /* restart if timed-out */ + + unsigned long c_time = hardware_.time(); + + if ( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ) { + no_.setConfigured(false); + } + + if ( mode_ != MODE_FIRST_FF) { //we are still in the midde of + //the message, and the message's + //timeout has already past, reset + //state machine + if (c_time > last_msg_timeout_time) { + mode_ = MODE_FIRST_FF; + } + } + + /* while available buffer, read data */ + while ( true ) { + short data = hardware_.read(); + if ( data < 0 ) + break; + 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_CHECKSUM; + } else if ( mode_ == MODE_FIRST_FF ) { + if (data == 0xff) { + mode_++; + last_msg_timeout_time = c_time + MSG_TIMEOUT; + } + } else if ( mode_ == MODE_SECOND_FF ) { + if (data == 0xff) { + mode_++; + } else { + mode_ = MODE_FIRST_FF; + } + } 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_++; + } else if ( mode_ == MODE_SIZE_L ) { /* bottom half of message size */ + bytes_ = data; + index_ = 0; + mode_++; + } else if ( mode_ == MODE_SIZE_H ) { /* top half of message size */ + bytes_ += data<<8; + mode_ = MODE_MESSAGE; + if (bytes_ == 0) + mode_ = MODE_CHECKSUM; + } else if ( mode_ == MODE_CHECKSUM ) { /* do checksum */ + if ( (checksum_%256) == 255) { + if (topic_ == TOPIC_NEGOTIATION) { + requestSyncTime(); + negotiateTopics(); + last_sync_time = c_time; + last_sync_receive_time = c_time; + } else if (topic_ == TopicInfo::ID_TIME) { + syncTime(message_in); + } else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST) { + req_param_resp.deserialize(message_in); + param_recieved= true; + } else { + if (receivers[topic_-100]) + receivers[topic_-100]->receive( message_in ); + } + } + mode_ = MODE_FIRST_FF; + } + } + + /* occasionally sync time */ + if ( no_.configured() && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )) { + requestSyncTime(); + last_sync_time = c_time; + } + } + + + /* Are we connected to the PC? */ + bool connected() { + return no_.configured(); + }; + + /************************************************************** + * Time functions + **************************************************************/ + + void requestSyncTime() { + std_msgs::Time t; + no_.publish( rosserial_msgs::TopicInfo::ID_TIME, &t); + rt_time = hardware_.time(); + } + + void syncTime( unsigned char * data ) { + std_msgs::Time t; + unsigned long 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(); + } + + + + Time now() { + unsigned long 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 setNow( Time & new_now ) { + unsigned long 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); + } + + + /*************** Registeration *****************************/ + bool advertise(Publisher & p) { + int i; + for (i = 0; i < MAX_PUBLISHERS; i++) { + if (publishers[i] == 0) { // empty slot + publishers[i] = &p; + p.id_ = i+100+MAX_SUBSCRIBERS; + p.no_ = &this->no_; + return true; + } + } + return false; + } + + /* Register a subscriber with the node */ + template<typename MsgT> + bool subscribe(Subscriber< MsgT> &s) { + return registerReceiver((MsgReceiver*) &s); + } + + template<typename SrvReq, typename SrvResp> + bool advertiseService(ServiceServer<SrvReq,SrvResp>& srv) { + srv.no_ = &no_; + return registerReceiver((MsgReceiver*) &srv); + } + + void negotiateTopics() { + no_.setConfigured(true); + + rosserial_msgs::TopicInfo ti; + int i; + for (i = 0; i < MAX_PUBLISHERS; i++) { + if (publishers[i] != 0) { // non-empty slot + ti.topic_id = publishers[i]->id_; + ti.topic_name = (char *) publishers[i]->topic_; + ti.message_type = (char *) publishers[i]->msg_->getType(); + no_.publish( TOPIC_PUBLISHERS, &ti ); + } + } + for (i = 0; i < MAX_SUBSCRIBERS; i++) { + if (receivers[i] != 0) { // non-empty slot + ti.topic_id = receivers[i]->id_; + ti.topic_name = (char *) receivers[i]->topic_; + ti.message_type = (char *) receivers[i]->getMsgType(); + no_.publish( TOPIC_SUBSCRIBERS, &ti ); + } + } + } + + /* + * Logging + */ +private: + void log(char byte, const char * msg) { + rosserial_msgs::Log l; + l.level= byte; + l.msg = (char*)msg; + this->no_.publish(rosserial_msgs::TopicInfo::ID_LOG, &l); + } +public: + void logdebug(const char* msg) { + log(rosserial_msgs::Log::DEBUG, msg); + } + void loginfo(const char * msg) { + log(rosserial_msgs::Log::INFO, msg); + } + void logwarn(const char *msg) { + log(rosserial_msgs::Log::WARN, msg); + } + void logerror(const char*msg) { + log(rosserial_msgs::Log::ERROR, msg); + } + void logfatal(const char*msg) { + log(rosserial_msgs::Log::FATAL, msg); + } + + + /**************************************** + * Retrieve Parameters + *****************************************/ +private: + bool param_recieved; + rosserial_msgs::RequestParamResponse req_param_resp; + bool requestParam(const char * name, int time_out = 1000) { + param_recieved = false; + rosserial_msgs::RequestParamRequest req; + req.name = (char*)name; + no_.publish(TopicInfo::ID_PARAMETER_REQUEST, &req); + int end_time = hardware_.time(); + while (!param_recieved ) { + spinOnce(); + if (end_time > hardware_.time()) return false; + } + return true; + } +public: + bool getParam(const char* name, int* param, int length =1) { + if (requestParam(name) ) { + if (length == req_param_resp.ints_length) { + //copy it over + for (int i=0; i<length; i++) param[i] = req_param_resp.ints[i]; + return true; + } + } + return false; + } + bool getParam(const char* name, float* param, int length=1) { + if (requestParam(name) ) { + if (length == req_param_resp.floats_length) { + //copy it over + for (int i=0; i<length; i++) param[i] = req_param_resp.floats[i]; + return true; + } + } + return false; + } + bool getParam(const char* name, char** param, int length=1) { + if (requestParam(name) ) { + if (length == req_param_resp.strings_length) { + //copy it over + for (int i=0; i<length; i++) strcpy(param[i],req_param_resp.strings[i]); + return true; + } + } + return false; + + } + +}; + + + +} + +#endif /* NODEHANDLE_H_ */