Adding a sonar Range subscriber c++

So I am trying to add a subscriber to a specific node.
The purpose of the subscriber is to get range messages from the pi_sonar topic and use it in the code.
This is the code here

so, if I wanted to add the sonar messages, should it look like this:

void turtlebot::range_sub('pacakge name of sonars'::Range msg){
        pi_sonar_x::Range = msg.Range;

Ofcourse however, I realized there is another node which is responsible for receiving these messages which are built here then sent over to the motion node. So, what I realize I should do is I would want to have the subscriber to the range messages there, and then use them here?

questions would be:

  • do I need to add the range message to the package xml file
  • do I need to add the range message to the cmakelist file?
  • do I need to add the messages in the messages folder? (I don’t think so based on my understanding)
  • what are other things I need to look out for?

If any further clarifications are need, let me know.
here is the actual code I am using:


Hmm, I think these should be of type sensor_msgs/Range, so you can treat them exactly the same as you do the geometry_msgs/Twist. I’m not seeing why this would be any different.

I don’t quite get what you’re trying to do.

1 Like

Please see this example for C++ sonar usage. I think it answers fully your questions.

1 Like

Thank you for your response!

So to clarify, what I am trying to do is I want to use the sonar messages in my code alongside the direction messages, which are generated by the package.

So, I would just want to make the robot check the ranges, and then check the direction. basically a nested if.
So, I am just like trying to see how do I add to the node those messages to be read and used.

Ah I see what you mean, in that case you have three options depending on which message you want to parse quicker:

Store twist, wait for sonar:

Create a class variable to store the Twist message, then in the vel_cmd callback only do a stored_twist = msg; or something like that.

Afterwards in the sonar callback node you can then always check the stored_twist and process it along with the sonar data. The stored_twist will get updated every time and the sonar message will work with the latest version received.

Since roscpp processes callbacks single threaded (by default) you have no danger of the vel_cmd changing the stored_twist while you’re working on it in the range_sub. Still, I’d make a deep copy at the very first line before using it just to be safe in case that ever changes.

Store sonar, wait for twist:

This is probably the better option, since sonar messages should arrive at a lower frequency, but the principle is the same.

You make a variable to store the Range message, something like stored_range = msg; and then access it in the vel_cmd callback when processing.

Process at a fixed frequency

The third option is to store both the range and twist message in each callback and do nothing else, so that you have the latest messages stored.

Then you can process both of them at the same time in int main, where it’s doing the spins:

ros::Rate r(20);
while (ros::ok())
	ros::spinOnce(); //callbacks update the stored vars here in theory

	geometry_msgs::Twist latest_twist;
	latest_twist = yourclass.stored_twist;
	sensor_msgs::Range latest_range;
	latest_range = yourclass.stored_range;
	//TODO process data using both 


This won’t react to a brand new message until the rate timer comes around of course, but it’ll be giving updates at a constant frequency which is sometimes better. It’s a more “game loop” style approach.

Of course this one will run even if no new data is received from both callbacks, so if that’s something you need I’d add a boolean “has_changed” that gets set to true by the callbacks and to false by the loop, and the loop only processes if the value is true.

Anyhow my cpp is a bit rusty, I mainly write python these days so take that as more of a pseudocode.


So, should be something like this, based on what I understood, to store the range value, it would be something like this:

here I think the direction is stored in the callback function and is being used in the vel_cmd function

Is this what I should do for the Sonar messages?

Yep, that should work I think.

1 Like

Thank you so much!! I got it to work eventually. I’ll post later about this project for anyone who wants to build this project as well!