rosserial_mbed_lib - This program is porting rosserial_arduino for mbe…

Users » nucho » Code » rosserial_mbed_lib

This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial.

Dependencies:

Dependents: rosserial_mbed robot_S2

ros/node_handle.h@0:77afd7560544, 2011-08-19 (annotated)

Committer:: nucho
Date:: Fri Aug 19 09:06:30 2011 +0000
Revision:: 0:77afd7560544
Child:: 1:ff0ec969dad1

Who changed what in which revision?

User	Revision	Line number	New contents of line
nucho	0:77afd7560544	1	/*
nucho	0:77afd7560544	2	* NodeHandle.h
nucho	0:77afd7560544	3	* Software License Agreement (BSD License)
nucho	0:77afd7560544	4	*
nucho	0:77afd7560544	5	* Copyright (c) 2011, Willow Garage, Inc.
nucho	0:77afd7560544	6	* All rights reserved.
nucho	0:77afd7560544	7	*
nucho	0:77afd7560544	8	* Redistribution and use in source and binary forms, with or without
nucho	0:77afd7560544	9	* modification, are permitted provided that the following conditions
nucho	0:77afd7560544	10	* are met:
nucho	0:77afd7560544	11	*
nucho	0:77afd7560544	12	* * Redistributions of source code must retain the above copyright
nucho	0:77afd7560544	13	* notice, this list of conditions and the following disclaimer.
nucho	0:77afd7560544	14	* * Redistributions in binary form must reproduce the above
nucho	0:77afd7560544	15	* copyright notice, this list of conditions and the following
nucho	0:77afd7560544	16	* disclaimer in the documentation and/or other materials provided
nucho	0:77afd7560544	17	* with the distribution.
nucho	0:77afd7560544	18	* * Neither the name of Willow Garage, Inc. nor the names of its
nucho	0:77afd7560544	19	* contributors may be used to endorse or promote prducts derived
nucho	0:77afd7560544	20	* from this software without specific prior written permission.
nucho	0:77afd7560544	21	*
nucho	0:77afd7560544	22	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
nucho	0:77afd7560544	23	* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
nucho	0:77afd7560544	24	* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
nucho	0:77afd7560544	25	* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
nucho	0:77afd7560544	26	* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
nucho	0:77afd7560544	27	* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
nucho	0:77afd7560544	28	* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
nucho	0:77afd7560544	29	* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
nucho	0:77afd7560544	30	* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
nucho	0:77afd7560544	31	* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
nucho	0:77afd7560544	32	* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
nucho	0:77afd7560544	33	* POSSIBILITY OF SUCH DAMAGE.
nucho	0:77afd7560544	34	*/
nucho	0:77afd7560544	35
nucho	0:77afd7560544	36	/*
nucho	0:77afd7560544	37	*
nucho	0:77afd7560544	38	* Author: Michael Ferguson , Adam Stambler
nucho	0:77afd7560544	39	*/
nucho	0:77afd7560544	40
nucho	0:77afd7560544	41	#ifndef ROS_NODE_HANDLE_H_
nucho	0:77afd7560544	42	#define ROS_NODE_HANDLE_H_
nucho	0:77afd7560544	43
nucho	0:77afd7560544	44	#include "../std_msgs/Time.h"
nucho	0:77afd7560544	45	#include "../rosserial_msgs/TopicInfo.h"
nucho	0:77afd7560544	46	#include "../rosserial_msgs/Log.h"
nucho	0:77afd7560544	47	#include "../rosserial_msgs/RequestParam.h"
nucho	0:77afd7560544	48
nucho	0:77afd7560544	49	#define SYNC_SECONDS 5
nucho	0:77afd7560544	50
nucho	0:77afd7560544	51	#define MODE_FIRST_FF 0
nucho	0:77afd7560544	52	#define MODE_SECOND_FF 1
nucho	0:77afd7560544	53	#define MODE_TOPIC_L 2 // waiting for topic id
nucho	0:77afd7560544	54	#define MODE_TOPIC_H 3
nucho	0:77afd7560544	55	#define MODE_SIZE_L 4 // waiting for message size
nucho	0:77afd7560544	56	#define MODE_SIZE_H 5
nucho	0:77afd7560544	57	#define MODE_MESSAGE 6
nucho	0:77afd7560544	58	#define MODE_CHECKSUM 7
nucho	0:77afd7560544	59
nucho	0:77afd7560544	60
nucho	0:77afd7560544	61	#define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data
nucho	0:77afd7560544	62
nucho	0:77afd7560544	63	#include "node_output.h"
nucho	0:77afd7560544	64
nucho	0:77afd7560544	65	#include "publisher.h"
nucho	0:77afd7560544	66	#include "msg_receiver.h"
nucho	0:77afd7560544	67	#include "subscriber.h"
nucho	0:77afd7560544	68	#include "rosserial_ids.h"
nucho	0:77afd7560544	69	#include "service_server.h"
nucho	0:77afd7560544	70
nucho	0:77afd7560544	71
nucho	0:77afd7560544	72	namespace ros {
nucho	0:77afd7560544	73
nucho	0:77afd7560544	74	using rosserial_msgs::TopicInfo;
nucho	0:77afd7560544	75
nucho	0:77afd7560544	76	/* Node Handle */
nucho	0:77afd7560544	77	template<class Hardware, int MAX_SUBSCRIBERS=25, int MAX_PUBLISHERS=25,
nucho	0:77afd7560544	78	int INPUT_SIZE=512, int OUTPUT_SIZE=512>
nucho	0:77afd7560544	79	class NodeHandle_ {
nucho	0:77afd7560544	80
nucho	0:77afd7560544	81	protected:
nucho	0:77afd7560544	82	Hardware hardware_;
nucho	0:77afd7560544	83	NodeOutput<Hardware, OUTPUT_SIZE> no_;
nucho	0:77afd7560544	84
nucho	0:77afd7560544	85	/* time used for syncing */
nucho	0:77afd7560544	86	unsigned long rt_time;
nucho	0:77afd7560544	87
nucho	0:77afd7560544	88	/* used for computing current time */
nucho	0:77afd7560544	89	unsigned long sec_offset, nsec_offset;
nucho	0:77afd7560544	90
nucho	0:77afd7560544	91	unsigned char message_in[INPUT_SIZE];
nucho	0:77afd7560544	92
nucho	0:77afd7560544	93	Publisher * publishers[MAX_PUBLISHERS];
nucho	0:77afd7560544	94	MsgReceiver * receivers[MAX_SUBSCRIBERS];
nucho	0:77afd7560544	95
nucho	0:77afd7560544	96	/******************************
nucho	0:77afd7560544	97	* Setup Functions
nucho	0:77afd7560544	98	*/
nucho	0:77afd7560544	99	public:
nucho	0:77afd7560544	100	NodeHandle_() : no_(&hardware_) {
nucho	0:77afd7560544	101	}
nucho	0:77afd7560544	102
nucho	0:77afd7560544	103	Hardware* getHardware() {
nucho	0:77afd7560544	104	return &hardware_;
nucho	0:77afd7560544	105	}
nucho	0:77afd7560544	106
nucho	0:77afd7560544	107	/* Start serial, initialize buffers */
nucho	0:77afd7560544	108	void initNode() {
nucho	0:77afd7560544	109	hardware_.init();
nucho	0:77afd7560544	110	mode_ = 0;
nucho	0:77afd7560544	111	bytes_ = 0;
nucho	0:77afd7560544	112	index_ = 0;
nucho	0:77afd7560544	113	topic_ = 0;
nucho	0:77afd7560544	114	total_receivers=0;
nucho	0:77afd7560544	115	};
nucho	0:77afd7560544	116
nucho	0:77afd7560544	117
nucho	0:77afd7560544	118	protected:
nucho	0:77afd7560544	119
nucho	0:77afd7560544	120	//State machine variables for spinOnce
nucho	0:77afd7560544	121	int mode_;
nucho	0:77afd7560544	122	int bytes_;
nucho	0:77afd7560544	123	int topic_;
nucho	0:77afd7560544	124	int index_;
nucho	0:77afd7560544	125	int checksum_;
nucho	0:77afd7560544	126
nucho	0:77afd7560544	127	int total_receivers;
nucho	0:77afd7560544	128
nucho	0:77afd7560544	129
nucho	0:77afd7560544	130	/* used for syncing the time */
nucho	0:77afd7560544	131	unsigned long last_sync_time;
nucho	0:77afd7560544	132	unsigned long last_sync_receive_time;
nucho	0:77afd7560544	133	unsigned long last_msg_timeout_time;
nucho	0:77afd7560544	134
nucho	0:77afd7560544	135	bool registerReceiver(MsgReceiver* rcv) {
nucho	0:77afd7560544	136	if (total_receivers >= MAX_SUBSCRIBERS) return false;
nucho	0:77afd7560544	137	receivers[total_receivers] = rcv;
nucho	0:77afd7560544	138	rcv->id_ = 100+total_receivers;
nucho	0:77afd7560544	139	total_receivers++;
nucho	0:77afd7560544	140	return true;
nucho	0:77afd7560544	141	}
nucho	0:77afd7560544	142
nucho	0:77afd7560544	143
nucho	0:77afd7560544	144	public:
nucho	0:77afd7560544	145	/* This function goes in your loop() function, it handles
nucho	0:77afd7560544	146	* serial input and callbacks for subscribers.
nucho	0:77afd7560544	147	*/
nucho	0:77afd7560544	148
nucho	0:77afd7560544	149	virtual void spinOnce() {
nucho	0:77afd7560544	150	/* restart if timed-out */
nucho	0:77afd7560544	151
nucho	0:77afd7560544	152	unsigned long c_time = hardware_.time();
nucho	0:77afd7560544	153
nucho	0:77afd7560544	154	if ( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ) {
nucho	0:77afd7560544	155	no_.setConfigured(false);
nucho	0:77afd7560544	156	}
nucho	0:77afd7560544	157
nucho	0:77afd7560544	158	if ( mode_ != MODE_FIRST_FF) { //we are still in the midde of
nucho	0:77afd7560544	159	//the message, and the message's
nucho	0:77afd7560544	160	//timeout has already past, reset
nucho	0:77afd7560544	161	//state machine
nucho	0:77afd7560544	162	if (c_time > last_msg_timeout_time) {
nucho	0:77afd7560544	163	mode_ = MODE_FIRST_FF;
nucho	0:77afd7560544	164	}
nucho	0:77afd7560544	165	}
nucho	0:77afd7560544	166
nucho	0:77afd7560544	167	/* while available buffer, read data */
nucho	0:77afd7560544	168	while ( true ) {
nucho	0:77afd7560544	169	short data = hardware_.read();
nucho	0:77afd7560544	170	if ( data < 0 )
nucho	0:77afd7560544	171	break;
nucho	0:77afd7560544	172	checksum_ += data;
nucho	0:77afd7560544	173	if ( mode_ == MODE_MESSAGE ) { /* message data being recieved */
nucho	0:77afd7560544	174	message_in[index_++] = data;
nucho	0:77afd7560544	175	bytes_--;
nucho	0:77afd7560544	176	if (bytes_ == 0) /* is message complete? if so, checksum */
nucho	0:77afd7560544	177	mode_ = MODE_CHECKSUM;
nucho	0:77afd7560544	178	} else if ( mode_ == MODE_FIRST_FF ) {
nucho	0:77afd7560544	179	if (data == 0xff) {
nucho	0:77afd7560544	180	mode_++;
nucho	0:77afd7560544	181	last_msg_timeout_time = c_time + MSG_TIMEOUT;
nucho	0:77afd7560544	182	}
nucho	0:77afd7560544	183	} else if ( mode_ == MODE_SECOND_FF ) {
nucho	0:77afd7560544	184	if (data == 0xff) {
nucho	0:77afd7560544	185	mode_++;
nucho	0:77afd7560544	186	} else {
nucho	0:77afd7560544	187	mode_ = MODE_FIRST_FF;
nucho	0:77afd7560544	188	}
nucho	0:77afd7560544	189	} else if ( mode_ == MODE_TOPIC_L ) { /* bottom half of topic id */
nucho	0:77afd7560544	190	topic_ = data;
nucho	0:77afd7560544	191	mode_++;
nucho	0:77afd7560544	192	checksum_ = data; /* first byte included in checksum */
nucho	0:77afd7560544	193	} else if ( mode_ == MODE_TOPIC_H ) { /* top half of topic id */
nucho	0:77afd7560544	194	topic_ += data<<8;
nucho	0:77afd7560544	195	mode_++;
nucho	0:77afd7560544	196	} else if ( mode_ == MODE_SIZE_L ) { /* bottom half of message size */
nucho	0:77afd7560544	197	bytes_ = data;
nucho	0:77afd7560544	198	index_ = 0;
nucho	0:77afd7560544	199	mode_++;
nucho	0:77afd7560544	200	} else if ( mode_ == MODE_SIZE_H ) { /* top half of message size */
nucho	0:77afd7560544	201	bytes_ += data<<8;
nucho	0:77afd7560544	202	mode_ = MODE_MESSAGE;
nucho	0:77afd7560544	203	if (bytes_ == 0)
nucho	0:77afd7560544	204	mode_ = MODE_CHECKSUM;
nucho	0:77afd7560544	205	} else if ( mode_ == MODE_CHECKSUM ) { /* do checksum */
nucho	0:77afd7560544	206	if ( (checksum_%256) == 255) {
nucho	0:77afd7560544	207	if (topic_ == TOPIC_NEGOTIATION) {
nucho	0:77afd7560544	208	requestSyncTime();
nucho	0:77afd7560544	209	negotiateTopics();
nucho	0:77afd7560544	210	last_sync_time = c_time;
nucho	0:77afd7560544	211	last_sync_receive_time = c_time;
nucho	0:77afd7560544	212	} else if (topic_ == TopicInfo::ID_TIME) {
nucho	0:77afd7560544	213	syncTime(message_in);
nucho	0:77afd7560544	214	} else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST) {
nucho	0:77afd7560544	215	req_param_resp.deserialize(message_in);
nucho	0:77afd7560544	216	param_recieved= true;
nucho	0:77afd7560544	217	} else {
nucho	0:77afd7560544	218	if (receivers[topic_-100])
nucho	0:77afd7560544	219	receivers[topic_-100]->receive( message_in );
nucho	0:77afd7560544	220	}
nucho	0:77afd7560544	221	}
nucho	0:77afd7560544	222	mode_ = MODE_FIRST_FF;
nucho	0:77afd7560544	223	}
nucho	0:77afd7560544	224	}
nucho	0:77afd7560544	225
nucho	0:77afd7560544	226	/* occasionally sync time */
nucho	0:77afd7560544	227	if ( no_.configured() && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )) {
nucho	0:77afd7560544	228	requestSyncTime();
nucho	0:77afd7560544	229	last_sync_time = c_time;
nucho	0:77afd7560544	230	}
nucho	0:77afd7560544	231	}
nucho	0:77afd7560544	232
nucho	0:77afd7560544	233
nucho	0:77afd7560544	234	/* Are we connected to the PC? */
nucho	0:77afd7560544	235	bool connected() {
nucho	0:77afd7560544	236	return no_.configured();
nucho	0:77afd7560544	237	};
nucho	0:77afd7560544	238
nucho	0:77afd7560544	239	/**************************************************************
nucho	0:77afd7560544	240	* Time functions
nucho	0:77afd7560544	241	**************************************************************/
nucho	0:77afd7560544	242
nucho	0:77afd7560544	243	void requestSyncTime() {
nucho	0:77afd7560544	244	std_msgs::Time t;
nucho	0:77afd7560544	245	no_.publish( rosserial_msgs::TopicInfo::ID_TIME, &t);
nucho	0:77afd7560544	246	rt_time = hardware_.time();
nucho	0:77afd7560544	247	}
nucho	0:77afd7560544	248
nucho	0:77afd7560544	249	void syncTime( unsigned char * data ) {
nucho	0:77afd7560544	250	std_msgs::Time t;
nucho	0:77afd7560544	251	unsigned long offset = hardware_.time() - rt_time;
nucho	0:77afd7560544	252
nucho	0:77afd7560544	253	t.deserialize(data);
nucho	0:77afd7560544	254
nucho	0:77afd7560544	255	t.data.sec += offset/1000;
nucho	0:77afd7560544	256	t.data.nsec += (offset%1000)*1000000UL;
nucho	0:77afd7560544	257
nucho	0:77afd7560544	258	this->setNow(t.data);
nucho	0:77afd7560544	259	last_sync_receive_time = hardware_.time();
nucho	0:77afd7560544	260	}
nucho	0:77afd7560544	261
nucho	0:77afd7560544	262
nucho	0:77afd7560544	263
nucho	0:77afd7560544	264	Time now() {
nucho	0:77afd7560544	265	unsigned long ms = hardware_.time();
nucho	0:77afd7560544	266	Time current_time;
nucho	0:77afd7560544	267	current_time.sec = ms/1000 + sec_offset;
nucho	0:77afd7560544	268	current_time.nsec = (ms%1000)*1000000UL + nsec_offset;
nucho	0:77afd7560544	269	normalizeSecNSec(current_time.sec, current_time.nsec);
nucho	0:77afd7560544	270	return current_time;
nucho	0:77afd7560544	271	}
nucho	0:77afd7560544	272
nucho	0:77afd7560544	273	void setNow( Time & new_now ) {
nucho	0:77afd7560544	274	unsigned long ms = hardware_.time();
nucho	0:77afd7560544	275	sec_offset = new_now.sec - ms/1000 - 1;
nucho	0:77afd7560544	276	nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
nucho	0:77afd7560544	277	normalizeSecNSec(sec_offset, nsec_offset);
nucho	0:77afd7560544	278	}
nucho	0:77afd7560544	279
nucho	0:77afd7560544	280
nucho	0:77afd7560544	281	/************* Registeration ***************************/
nucho	0:77afd7560544	282	bool advertise(Publisher & p) {
nucho	0:77afd7560544	283	int i;
nucho	0:77afd7560544	284	for (i = 0; i < MAX_PUBLISHERS; i++) {
nucho	0:77afd7560544	285	if (publishers[i] == 0) { // empty slot
nucho	0:77afd7560544	286	publishers[i] = &p;
nucho	0:77afd7560544	287	p.id_ = i+100+MAX_SUBSCRIBERS;
nucho	0:77afd7560544	288	p.no_ = &this->no_;
nucho	0:77afd7560544	289	return true;
nucho	0:77afd7560544	290	}
nucho	0:77afd7560544	291	}
nucho	0:77afd7560544	292	return false;
nucho	0:77afd7560544	293	}
nucho	0:77afd7560544	294
nucho	0:77afd7560544	295	/* Register a subscriber with the node */
nucho	0:77afd7560544	296	template<typename MsgT>
nucho	0:77afd7560544	297	bool subscribe(Subscriber< MsgT> &s) {
nucho	0:77afd7560544	298	return registerReceiver((MsgReceiver*) &s);
nucho	0:77afd7560544	299	}
nucho	0:77afd7560544	300
nucho	0:77afd7560544	301	template<typename SrvReq, typename SrvResp>
nucho	0:77afd7560544	302	bool advertiseService(ServiceServer<SrvReq,SrvResp>& srv) {
nucho	0:77afd7560544	303	srv.no_ = &no_;
nucho	0:77afd7560544	304	return registerReceiver((MsgReceiver*) &srv);
nucho	0:77afd7560544	305	}
nucho	0:77afd7560544	306
nucho	0:77afd7560544	307	void negotiateTopics() {
nucho	0:77afd7560544	308	no_.setConfigured(true);
nucho	0:77afd7560544	309
nucho	0:77afd7560544	310	rosserial_msgs::TopicInfo ti;
nucho	0:77afd7560544	311	int i;
nucho	0:77afd7560544	312	for (i = 0; i < MAX_PUBLISHERS; i++) {
nucho	0:77afd7560544	313	if (publishers[i] != 0) { // non-empty slot
nucho	0:77afd7560544	314	ti.topic_id = publishers[i]->id_;
nucho	0:77afd7560544	315	ti.topic_name = (char *) publishers[i]->topic_;
nucho	0:77afd7560544	316	ti.message_type = (char *) publishers[i]->msg_->getType();
nucho	0:77afd7560544	317	no_.publish( TOPIC_PUBLISHERS, &ti );
nucho	0:77afd7560544	318	}
nucho	0:77afd7560544	319	}
nucho	0:77afd7560544	320	for (i = 0; i < MAX_SUBSCRIBERS; i++) {
nucho	0:77afd7560544	321	if (receivers[i] != 0) { // non-empty slot
nucho	0:77afd7560544	322	ti.topic_id = receivers[i]->id_;
nucho	0:77afd7560544	323	ti.topic_name = (char *) receivers[i]->topic_;
nucho	0:77afd7560544	324	ti.message_type = (char *) receivers[i]->getMsgType();
nucho	0:77afd7560544	325	no_.publish( TOPIC_SUBSCRIBERS, &ti );
nucho	0:77afd7560544	326	}
nucho	0:77afd7560544	327	}
nucho	0:77afd7560544	328	}
nucho	0:77afd7560544	329
nucho	0:77afd7560544	330	/*
nucho	0:77afd7560544	331	* Logging
nucho	0:77afd7560544	332	*/
nucho	0:77afd7560544	333	private:
nucho	0:77afd7560544	334	void log(char byte, const char * msg) {
nucho	0:77afd7560544	335	rosserial_msgs::Log l;
nucho	0:77afd7560544	336	l.level= byte;
nucho	0:77afd7560544	337	l.msg = (char*)msg;
nucho	0:77afd7560544	338	this->no_.publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
nucho	0:77afd7560544	339	}
nucho	0:77afd7560544	340	public:
nucho	0:77afd7560544	341	void logdebug(const char* msg) {
nucho	0:77afd7560544	342	log(rosserial_msgs::Log::DEBUG, msg);
nucho	0:77afd7560544	343	}
nucho	0:77afd7560544	344	void loginfo(const char * msg) {
nucho	0:77afd7560544	345	log(rosserial_msgs::Log::INFO, msg);
nucho	0:77afd7560544	346	}
nucho	0:77afd7560544	347	void logwarn(const char *msg) {
nucho	0:77afd7560544	348	log(rosserial_msgs::Log::WARN, msg);
nucho	0:77afd7560544	349	}
nucho	0:77afd7560544	350	void logerror(const char*msg) {
nucho	0:77afd7560544	351	log(rosserial_msgs::Log::ERROR, msg);
nucho	0:77afd7560544	352	}
nucho	0:77afd7560544	353	void logfatal(const char*msg) {
nucho	0:77afd7560544	354	log(rosserial_msgs::Log::FATAL, msg);
nucho	0:77afd7560544	355	}
nucho	0:77afd7560544	356
nucho	0:77afd7560544	357
nucho	0:77afd7560544	358	/****************************************
nucho	0:77afd7560544	359	* Retrieve Parameters
nucho	0:77afd7560544	360	*****************************************/
nucho	0:77afd7560544	361	private:
nucho	0:77afd7560544	362	bool param_recieved;
nucho	0:77afd7560544	363	rosserial_msgs::RequestParamResponse req_param_resp;
nucho	0:77afd7560544	364	bool requestParam(const char * name, int time_out = 1000) {
nucho	0:77afd7560544	365	param_recieved = false;
nucho	0:77afd7560544	366	rosserial_msgs::RequestParamRequest req;
nucho	0:77afd7560544	367	req.name = (char*)name;
nucho	0:77afd7560544	368	no_.publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
nucho	0:77afd7560544	369	int end_time = hardware_.time();
nucho	0:77afd7560544	370	while (!param_recieved ) {
nucho	0:77afd7560544	371	spinOnce();
nucho	0:77afd7560544	372	if (end_time > hardware_.time()) return false;
nucho	0:77afd7560544	373	}
nucho	0:77afd7560544	374	return true;
nucho	0:77afd7560544	375	}
nucho	0:77afd7560544	376	public:
nucho	0:77afd7560544	377	bool getParam(const char* name, int* param, int length =1) {
nucho	0:77afd7560544	378	if (requestParam(name) ) {
nucho	0:77afd7560544	379	if (length == req_param_resp.ints_length) {
nucho	0:77afd7560544	380	//copy it over
nucho	0:77afd7560544	381	for (int i=0; i<length; i++) param[i] = req_param_resp.ints[i];
nucho	0:77afd7560544	382	return true;
nucho	0:77afd7560544	383	}
nucho	0:77afd7560544	384	}
nucho	0:77afd7560544	385	return false;
nucho	0:77afd7560544	386	}
nucho	0:77afd7560544	387	bool getParam(const char* name, float* param, int length=1) {
nucho	0:77afd7560544	388	if (requestParam(name) ) {
nucho	0:77afd7560544	389	if (length == req_param_resp.floats_length) {
nucho	0:77afd7560544	390	//copy it over
nucho	0:77afd7560544	391	for (int i=0; i<length; i++) param[i] = req_param_resp.floats[i];
nucho	0:77afd7560544	392	return true;
nucho	0:77afd7560544	393	}
nucho	0:77afd7560544	394	}
nucho	0:77afd7560544	395	return false;
nucho	0:77afd7560544	396	}
nucho	0:77afd7560544	397	bool getParam(const char* name, char** param, int length=1) {
nucho	0:77afd7560544	398	if (requestParam(name) ) {
nucho	0:77afd7560544	399	if (length == req_param_resp.strings_length) {
nucho	0:77afd7560544	400	//copy it over
nucho	0:77afd7560544	401	for (int i=0; i<length; i++) strcpy(param[i],req_param_resp.strings[i]);
nucho	0:77afd7560544	402	return true;
nucho	0:77afd7560544	403	}
nucho	0:77afd7560544	404	}
nucho	0:77afd7560544	405	return false;
nucho	0:77afd7560544	406
nucho	0:77afd7560544	407	}
nucho	0:77afd7560544	408
nucho	0:77afd7560544	409	};
nucho	0:77afd7560544	410
nucho	0:77afd7560544	411
nucho	0:77afd7560544	412
nucho	0:77afd7560544	413	}
nucho	0:77afd7560544	414
nucho	0:77afd7560544	415	#endif /* NODEHANDLE_H_ */

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	19 Aug 2011
Imports:	371
Forks:	3
Commits:	5
Dependents:	2
Dependencies:	1
Followers:	23

ros/node_handle.h@0:77afd7560544, 2011-08-19 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning