Skip to content

ROS1 Cpp Interfaces

Jump to bottom
Francesco Ganci edited this page Jul 13, 2022 · 12 revisions

Initial Remarks

⚠️ to complete ⚠️

  • I'm referring especially to the ROS1 simple node pattern, but if you need a class patter for the node, few updates are needed.

Naming Conventions - ROScpp nodes

⚠️ work in progress, see the code ⚠️

  • a topic name as a macro TOPIC_??? replacing ??? with the uppercase name of the topic
  • for queue size for one topic, Q_SZ_??? replacing ??? with the uppercase name of the topic

✉️ ROS1 C++ topics

Global Definitions

suppose you want to use the message msg inside the package pkg. The topic is simply /top; the NodeHandle object is nh.

// include the message type (pkg::msg)
#include <pkg/msg.h>

// name of the topic and queue size
#define TOPIC_TOP "/top"
#define Q_SZ_TOP 1000

open a publisher

Some remarks:

  • the main call is ros::Publisher p = nh.advertize<pkg::msg>( "topic", queue_sz ) which takes as input the type of the message, the topic (as std::string) and the queue size (usually 1000)
// define globally as pointer
ros::Publisher *pub_top;

// in the main function ... 
TLOG( "Creating publisher " << LOGSQUARE( TOPIC_TOP ) << "..." );
ros::Publisher tpub_top = nh.advertise<pkg::msg>( TOPIC_TOP, Q_SZ_TOP );
pub_top = &tpub_top;
TLOG( "Creating publisher " << LOGSQUARE( TOPIC_TOP ) << "... OK" );

Use a publisher

the main call is pub_topic->publish( m ) which simply publishes the message through the topic.

pkg::msg m;

// fill in the message
// m.field = value;

pub_topic->publish( m );
// remember to use
// ros::spinOnce();

open a subscriber

// define globally 
ros::Subscriber *sub_top;
pkg::msg last_msg_top; // a copy of the previously received message

// define the "callback"
void cbk_top( const pkg::msg::ConstPtr& pm )
{
	// save the last message
	last_msg_top = pm;
	
	// ...
}

// in main() define the interface
TLOG( "subscribing to the topic " << LOGSQUARE( TOPIC_TOP ) << "..." );
ros::Subscriber tsub_top = nh.subscribe( TOPIC_TOP, Q_SZ_TOP, cbk_top );
sub_top = &tsub_top;
TLOG( "subscribing to the topic " << LOGSQUARE( TOPIC_TOP ) << "... OK" );

Open a subscriber with the callback in a class

☝️ The method must be public.

let's say the class my_class contains the methods cbk_top() you want to use as callback. The syntax changes a bit:

TLOG( "subscribing to the topic " << LOGSQUARE( TOPIC_TOP ) << "..." );
ros::Subscriber tsub_top = nh.subscribe( TOPIC_TOP, Q_SZ_TOP, &my_class::cbk_top, &my_class );
sub_top = &tsub_top;
TLOG( "subscribing to the topic " << LOGSQUARE( TOPIC_TOP ) << "... OK" );

The function prototype is the same.

Dynamic subscribers -- close a subscriber

Sometimes it is needed to read just one message from the topic and stop reading. One solution could be to set a boolean, but this resembles a lot the busy waiting, which is a waste of computational resources. A better solution is to close the topic after a number of messages using sub_???.shutdown().

🎰 ROS1 C++ services

Global definitions

// import the message type (pkg::srv , pkg::srv::Request , pkg::srv::Response)
#include <pkg::srv.h>

// global definitions
#define SERVICE_SERV "/serv"

open a service

// define globally a pointer to the service
ros::ServiceServer *srv_serv;
pkg::srv last_srv;

// prototype of the callback
bool cbk_serv( pkg::srv::Request& req, pkg::srv::Response& res )
{
	// save the request
	last_srv.request = req;
	
	// elaborate the response
	// res.field = value;
	
	// ...
	
	// save the response
	last_srv.response = res;
	
	return true;
}

// within the main function
OUTLOG( "Advertising service " << LOGSQUARE( SERVICE_SERV  ) << "..." );
ros::ServiceServer tsrv_serv = nh.advertiseService( SERVICE_SERV, cbk_serv );
srv_serv = &tsrv_serv;
OUTLOG( "Advertising service " << LOGSQUARE( SERVICE_SERV ) << "... OK" );

Open up a service using a class method as callback

☝️ The method must be public.

not so different. my_class containts the method cbk_serv I want to use as callback. Instanciate the class, then open the service:

// within the main function
OUTLOG( "Advertising service " << LOGSQUARE( SERVICE_SERV  ) << "..." );
ros::ServiceServer tsrv_serv = nh.advertiseService( SERVICE_SERV, &my_class::cbk_serv, &my_class );
srv_serv = &tsrv_serv;
OUTLOG( "Advertising service " << LOGSQUARE( SERVICE_SERV ) << "... OK" );

open up a client

// define also this
#define TIMEOUT_SERV 5 //seconds

// client definition
ros::ServiceClient *cl_serv;

// main function
TLOG( "Opening client " << LOGSQUARE( SERVICE_SERV ) << "..." );
ros::ServiceClient tcl_serv = nh.serviceClient<pkg::srv>( SERVICE_SERV );
if( !tcl_serv.waitForExistence( ros::Duration( TIMEOUT_SERV ) ) )
{
	TERR( "unable to contact the server - timeout expired (" << TIMEOUT_SERV << "s) " );
	return 0;
}
cl_serv = &tcl_serv;
TLOG( "Opening client " << LOGSQUARE( SERVICE_SERV ) << "... OK" );

Use a client

ROS1 clients are always blocking, unless you don't create a thread in charge of managing the request.

// service message (we're going to fill in the request only)
pkg::srv sm;

// populate the request
// sm.request.field = value;

// call the service (blocking)
if( !cl_serv->call( sm ) ) 
{ 
	TERR( "unable to make a service request -- failed calling service " 
		<< LOGSQUARE( SERVICE_SERV ) 
		<< (!cl->exists( ) ? " -- it seems not opened" : "") );
	// return 0;
}

// use the response
// sm.response.field

client using bare callings

🔗 see ros::service Namespace reference

This style allows to not create a handle for dealing with the service. Here are two ways to check if a service exists:

// === FIRST METHOD -- NON BLOCKING === //

// check if a service exists (non-blocking)
ros::service::exists( SERVICE_SERV )
{
    // the service exists... do whatever you want inside here
}

// === SECOND METHOD -- BLOCKING === //

// wait for the existence of a service (blocking, at least for a given time)
if( !ros::service::waitForExistence( SERVICE_SERV, ros::Duration( TIMEOUT_SERV ) ) )
{
	TERR( "unable to contact the server - timeout expired (" << TIMEOUT_SERV << "s) " );
	return 0;
}

And here is how to call the service using bare calling style:

// service message (we're going to fill in the request only)
pkg::srv sm;

// populate the request
// sm.request.field = value;

// call the service (blocking)
if( !ros::service::call( SERVICE_SERV, sm ) ) 
{ 
	TERR( "unable to make a service request -- failed calling service " 
		<< LOGSQUARE( SERVICE_SERV ) 
		<< (!ros::service::exists( SERVICE_SERV )) ? " -- it seems not opened" : "") );
	return 0;
}

// use the response
// sm.response.field

⚙️ ROS1 C++ Actions

☝️ here we're using simpleActionClient, meaning that a node can implement as many actions as it wants, but running only one instance per action. see this post for further information.

Globals

for Action Service:

#include <actionlib/server/simple_action_server.h>
#include <string>

// include the message 'act' from inside the package 'pkg'
#include <pkg/actAction.h>
// it defines 4 types of messages
//    pkg::actAction
//    pkg::actGoal
//    pkg::actResult
//    pkg::actFeedback
// plus...
//    pkg::act

// define for the action service
#define ACTION_ACT "act"
#define RATE_ACT 5

for Action Client:

#include "actionlib/client/simple_action_client.h"
#include "actionlib/client/terminal_state.h"
#include <string>

// include the message 'act' from inside the package 'pkg'
#include <pkg/actAction.h>
// it defines 4 types of messages
//    pkg::actAction
//    pkg::actGoal
//    pkg::actResult
//    pkg::actFeedback
// plus...
//    pkg::act

// define for the action service
#define ACTION_ACT "act"
#define TIMEOUT_ACT 5
#define TIMEOUT_WAIT_ACT 0.1
#define RES_ACT (actcl_act->getResult( ))

How to Implement a Action Service

🔗 more info inside the official Wiki

🔗 the implementation includes also the time spent in working on the goal. See time in ROS to know more about it.

🔗 if you want to manage multiple requests in parallel on the same action server, see the actionlib class reference

Here's a customizable implementation of a action server. act is the name of the implemented action. Remember to adapt the names to your application!

class actclass_act
{
protected:
	
	// the nodeHandle
	ros::NodeHandle nh;
	
	// the action service handle
	actionlib::SimpleActionServer<pkg::act> as;
	
	// the service name
	std::string action_name;
	
	// last sent feedback
	pkg::actFeedback feedback;
	
	// last sent result
	pkg::actResult result;
	
	// starting time for the action
	ros::Time t_start = ros::Time::now( );
	
	// action end time
	ros::Time t_end = ros::Time::now( );
	
	// time spent for the previously completed action
	ros::Duration t( 0 );
	
public:
	
	// action class constructor
	actclass_act( std::string aname = ACTION_ACT ) :
		as( nh, aname, boost::bind(&actclass_act::cbk_act, this, _1), false ),
		action_name( aname )
	{
		// ...
		
		as.start( );
	}
	
	// destructor
	~actclass_act( void )
	{
		// ...
	}
	
	// TODO action implementation
	void cbk_act( const pkg::act::ConstPtr& goal )
	{
		TLOG( "action " << LOGSQUARE( ACTION_ACT ) << " START" );
		// this->print_goal( goal );
		
		bool success = false;
		ros::Rate r( RATE_ACT );
		
		// TODO to set a known initial state before executing the main loop
		// this->reset_class( );
		
		this->t_start = ros::Time::now( );
		while( true )
		{
			// check first for preemption
			if ( as.isPreemptRequested( ) || !ros::ok( ) )
			{
				TLOG( "[" << LOGSQUARE( ACTION_ACT ) << "] CANCELLED " );
				as.setPreempted( );
				success = false;
				break;
			}
			
			// TODO working on the message ...
			// ...
			
			// TODO fill in the feedback
			// this->feedback.field = value;
			as.publishFeedback( this->feedback );
			
			// sleep until the next cycle
			r.sleep( );
		}
		this->t_end   = ros::Time::now( );
		this->t = this->t_end - this->t_start;
		
		if( success )
		{
			TLOG( "[" << LOGSQUARE( ACTION_ACT ) << "] SUCCESS " 
				<< "(in " << this->t.toSec( ) << "s)" );
			
			// TODO fill in the result
			// this->result.field = value;
			
			as.setSucceed( this->result );
		}
		else
		{
			// TODO what to do in case of failure? 
			// TLOG( "[" << LOGSQUARE( ACTION_ACT ) << "] FAILED " << "(in " << this->t.toSec( ) << "s)" );
		}
	}

private:

	// TODO other class data ...
	// ...
	
	// TODO print the message
	void print_goal( const pkg::act::ConstPtr& goal )
	{
		/*
		TLOG( "[" << LOGSQUARE( ACTION_ACT ) << "] Received goal: \n" 
			<< "\t" << goal.field << " \n"
		);
		*/
	}
	
	// TODO print the result
	void print_result( const pkg::actResult& res )
	{
		/*
		TLOG( "[" << LOGSQUARE( ACTION_ACT ) << "] Sending result: \n" 
			<< "\t" << res.field << " \n"
		);
		*/
	}
	
	// TODO set a known state before starting the loop
	void reset_class( )
	{
		// empty the class messages
		this->feedback = pkg::actFeedback( );
		this->result = pkg::actResult( );
		
		// reset the timer
		this->t_start = ros::Time::now(  );
		this->t_end   = ros::Time::now(  );
		this->t       = ros::Duration( 0 );
		
		// TODO other operations before starting
		// ... 
	}
}

Create a action server

// define globally
actclass_act *act_act;

// then, in main function ...
TLOG( "opening action " << LOGSQUARE( ACTION_ACT ) << " ... " );
actclass_act tact_act( ACTION_ACT );
act_act = &tact_act;
TLOG( "opening action " << LOGSQUARE( ACTION_ACT ) << " ... OK!" );

Open up a Action Client

🔗 here's a good example of client implementation even if a bit limited.

🔗 see also the actionlib::SimpleActionClient class reference

🔗 see this reference to understand how to deal with service states. See also getState()

🔗 see this post, which discusses a class implementation of the Action Client. Interesting also for showing another way to pass the goal to the service.

// define globally
actionlib::SimpleActionClient<pkg::act> *actcl_act( );

// in the main function ...
TLOG( "opening action client " << LOGSQUARE( ACTION_ACT ) << " ... " );
actionlib::SimpleActionClient<pkg::act> tactcl_act( ACTION_ACT, true );
if( !tactcl_act.waitForServer( ros::Duration( TIMEOUT_ACT ) ) )
{
	TERR( "unable to connect to the action server (timeout " << TIMEOUT_ACT << "s) " 
		<< "-- action " << LOGSQUARE( ACTION_ACT ) << "\n"
		<< "\t " << (tactcl_act.isServerConnected( ) ? " it seems not online " : " service online ")  ) << "\n"
		<< "\t" << "STATUS: " << tactcl_act.getState( ).toString( ) );
	return 0;
}
actcl_act = & tactcl_act;
TLOG( "opening action client" << LOGSQUARE( ACTION_ACT ) << " ... OK!" );

how to use a Action Client

First phase: create the goal and send it to the service.

// declare the goal 
pkg::actGoal goal;

// fill in the goal
// goal.field = value;

// send the goal
actcl_act->sendGoal( goal );

Simplified waiting, blocking:

⚠️ This pattern cannot be used when the node also implements some topic or services, since the waitForResults( ) is completely blocking: it makes the entire process to sleep!

☝️ Look at this page about asyncSpin(). Using this kind of spinner allows to use this kind of waiting without blocking other interfaces implemented in the same node, if any.

🔗 see also the ros::CallbackQueueInterface Class

if ( !actcl_act->waitForResult( ros::Duration(TIMEOUT_ACT) ) )
{
	TERR( "action client for " << LOGSQUARE( ACTION_ACT ) << "TIMEOUT EXPIRED " );
	actcl_act->cancelAllGoals( );

	// TODO how to deal with it?
	// ...
}

// obtain the pointer to the result
auto res = actcl_act->getResult( );
// res->field; CONST PTR!

Manual waiting, non-blocking and appliable in a single-thread context (but choosing a small timeout):

{
float wtime = 0.0;
std::string state = "ACTIVE";	
while( true )
{
	// wait and spin
	(ros::Duration( TIMEOUT_WAIT_ACT )).sleep( );
	ros::spinOnce( );
	
	// TODO other things here ... ?
	// ...
	
	// check the status
	wtime += TIMEOUT_WAIT_ACT;
	state = actcl_act.getState( ).toString( );
	if( state == "SUCCEEDED" )
	{
		TLOG( LOGSQUARE( ACTION_ACT ) << " SUCCESS in " << (int) wtime << "s" );
		
		// ...
		
		break;
	}
	else if( state == "PREEMPTED" )
	{
		TLOG( LOGSQUARE( ACTION_ACT ) << " CANCELLED!" );
		break;
	}
	else if( wtime > TIMEOUT_ACT )
	{
		TLOG( LOGSQUARE( ACTION_ACT ) << "unable to complete the action in time; cancelling ..." );
		actcl_act->cancelAllGoals( );
		break;
	}
}
}

// obtain the pointer to the result
auto res = actcl_act->getResult( );
// res->field; CONST PTR!

cancel a goal

very simple: just use

actcl_act->cancelAllGoals( );

💾 ROS1 C++ Parameter Server

🔗 see the official ROS Wiki

🔗 in particular, see the C++ interface for ROS parameter server

SET -- Load data in the parameter server

ros::param::set("/global_param", 5);
ros::param::set("relative_param", "my_string");
ros::param::set("bool_param", false);

Load data from Bash

🔗 see the ROSparam command line

# set one value
rosparam set parameter_name value

# from file
rosparam load param-file.yaml

Loading from YAML files the parameters is for sure the most interesting method. Here's a simple example of YAML parameters files, just to appreciate a bit of its syntax:

string: 'foo'
integer: 1234
float: 1234.5
boolean: true
list: [1.0, mixed list]
dictionary: {a: b, c: d}

Load data from Launch Files

<!-- one parameter -->
<param name="somefloat1" value="3.14159" type="double" />

<!-- a YAML file -->
<param name="configfile" textfile="$(find roslaunch)/example.yaml" />

DEL -- Delete data in the parameter server

ros::param::del("my_param");

CHECK -- check if data exist

if( ros::param::has("my_param") )
{
    // it exists... yuppie!
}

GET -- read data from the parameter server

ros::param::get("/global_name", value_reference);
Clone this wiki locally