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:
- 1:098e75fd5ad2
- Parent:
- 0:06fc856e99ca
- Child:
- 3:dff241b66f84
--- a/ros/node_handle.h Fri Aug 19 09:06:16 2011 +0000 +++ b/ros/node_handle.h Sun Oct 16 07:17:43 2011 +0000 @@ -1,5 +1,4 @@ /* - * NodeHandle.h * Software License Agreement (BSD License) * * Copyright (c) 2011, Willow Garage, Inc. @@ -33,11 +32,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ -/* - * - * Author: Michael Ferguson , Adam Stambler - */ - #ifndef ROS_NODE_HANDLE_H_ #define ROS_NODE_HANDLE_H_ @@ -57,7 +51,6 @@ #define MODE_MESSAGE 6 #define MODE_CHECKSUM 7 - #define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data #include "node_output.h" @@ -68,16 +61,17 @@ #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> +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_; @@ -93,12 +87,11 @@ Publisher * publishers[MAX_PUBLISHERS]; MsgReceiver * receivers[MAX_SUBSCRIBERS]; - /****************************** - * Setup Functions + /* + * Setup Functions */ public: - NodeHandle_() : no_(&hardware_) { - } + NodeHandle_() : no_(&hardware_) {} Hardware* getHardware() { return &hardware_; @@ -111,12 +104,12 @@ bytes_ = 0; index_ = 0; topic_ = 0; + checksum_=0; + total_receivers=0; }; - protected: - //State machine variables for spinOnce int mode_; int bytes_; @@ -126,39 +119,35 @@ 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; + 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 */ + /* restart if timed out */ unsigned long c_time = hardware_.time(); - - if ( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ) { + 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 + /* reset if message has timed out */ + if ( mode_ != MODE_FIRST_FF) { if (c_time > last_msg_timeout_time) { mode_ = MODE_FIRST_FF; } @@ -166,8 +155,8 @@ /* while available buffer, read data */ while ( true ) { - short data = hardware_.read(); - if ( data < 0 ) + int data = hardware_.read(); + if ( data < 0 )//no data break; checksum_ += data; if ( mode_ == MODE_MESSAGE ) { /* message data being recieved */ @@ -230,15 +219,14 @@ } } - /* Are we connected to the PC? */ bool connected() { return no_.configured(); }; - /************************************************************** + /* * Time functions - **************************************************************/ + */ void requestSyncTime() { std_msgs::Time t; @@ -259,8 +247,6 @@ last_sync_receive_time = hardware_.time(); } - - Time now() { unsigned long ms = hardware_.time(); Time current_time; @@ -277,8 +263,10 @@ normalizeSecNSec(sec_offset, nsec_offset); } + /* + * Registration + */ - /*************** Registeration *****************************/ bool advertise(Publisher & p) { int i; for (i = 0; i < MAX_PUBLISHERS; i++) { @@ -306,7 +294,6 @@ void negotiateTopics() { no_.setConfigured(true); - rosserial_msgs::TopicInfo ti; int i; for (i = 0; i < MAX_PUBLISHERS; i++) { @@ -330,6 +317,7 @@ /* * Logging */ + private: void log(char byte, const char * msg) { rosserial_msgs::Log l; @@ -337,6 +325,7 @@ 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); @@ -355,12 +344,14 @@ } - /**************************************** + /* * 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; @@ -373,12 +364,14 @@ } 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]; + for (int i=0; i<length; i++) + param[i] = req_param_resp.ints[i]; return true; } } @@ -388,7 +381,8 @@ 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]; + for (int i=0; i<length; i++) + param[i] = req_param_resp.floats[i]; return true; } } @@ -398,18 +392,15 @@ 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]); + for (int i=0; i<length; i++) + strcpy(param[i],req_param_resp.strings[i]); return true; } } return false; - } - }; - - } -#endif /* NODEHANDLE_H_ */ +#endif \ No newline at end of file