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@4:684f39d0c346, 2012-02-29 (annotated)

Committer:: nucho
Date:: Wed Feb 29 23:00:21 2012 +0000
Revision:: 4:684f39d0c346
Parent:: 3:1cf99502f396

Who changed what in which revision?

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

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@4:684f39d0c346, 2012-02-29 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning