rosserial_mbed - This program is porting rosserial_arduino for mbe…

Users » nucho » Code » rosserial_mbed

nucho / Mbed 2 deprecated rosserial_mbed

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

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

Committer:: nucho
Date:: Fri Aug 19 09:06:16 2011 +0000
Revision:: 0:06fc856e99ca
Child:: 1:098e75fd5ad2

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	19 Aug 2011
Imports:	387
Forks:	1
Commits:	5
Dependents:	0
Dependencies:	4
Followers:	6

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

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning