C++ Archives - Page 2 of 2

[ROS Q&A] 196 – How to Output Odometry Data

In this post, you will learn how to access and output odometry data programmatically. More specifically, you will learn how to create a C++ program for subscribing to and printing different parts of the odometry message. This is useful for cases where you need to make a decision based on the robot’s current position.

This post is an answer to the following question found on ROS Answers: https://answers.ros.org/question/333391/way-to-output-odometry-info/.

Let’s get started!

Step 1: Grab the ROS Project (ROSject) on ROSDS

Click here to get a copy of the ROS project used in this post. Next, click open to load the project. Please be patient while the project loads.

Step 2: Launch the necessary tools

Launch a simulation from the Simulations menu.
- Click on the Simulations menu. A dropdown menu opens.
- In the section “Launch a provided simulation”, leave the empty world selected and choose the Turtlebot 2 robot.
- Click “Start simulation”.
Pick a Shell tool from the Tools menu. We’re be firing some commands soon.
Pick the IDE tool from the Tools menu. We’ll use this to examine the code used for solving this problem.

Step 3: Examine the structure of the Odometry message using the command line

Run the following command on the Shell tool to print the current odometry of the robot:

# The -n1 switch to the command to ensure it prints one command and exits. 
# Otherwise, it would print continuously until you press Ctrl + C.
user:~$ rostopic echo /odom -n1
header:
  seq: 4732
  stamp:
    secs: 236
    nsecs: 935000000
  frame_id: "odom"
child_frame_id: "base_footprint"
pose:
  pose:
    position:
      x: 0.00211759816401
      y: 4.81456536954e-05
      z: -0.000247189070632
    orientation:
      x: 0.000385225477218
      y: -0.00721104301471
      z: 0.000107238788561
      w: 0.99997392014
  covariance: [1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05, 0.0,0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
twist:
  twist:
    linear:
      x: -0.000279472699265
      y: -4.17203978095e-05
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -7.05905617719e-06
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

The command line output above shows the parts the make up an odometry message. The hottest part is pose, which tells us the current position of the robot. twist is also important, because it tells us the current message being published to move the robot.

Now let’s see how to access and output odometry data, considering the hottest part.

Step 4: See how to access and output odometry data in code

Switch to the IDE tool and locate the catkin_ws directory. You should find a C++ file in a package odom_subscriber. The full path to the file is catkin_ws/src/odom_subscriber/src/odom_sub.cpp.

Let’s see the content of this file:

#include <nav_msgs/Odometry.h> // Needed for accessing Odometry data
#include <ros/ros.h>           // Needed for creating the node, etc

// This functions get called when a new odometry message gets to the subscriber
// It automatically gets the odometry message as a parameter
// It prints out various parts of the message
void counterCallback(const nav_msgs::Odometry::ConstPtr &msg) {
  // ROS_INFO("%s", msg->header.frame_id.c_str());
  // ROS_INFO("%f", msg->twist.twist.linear.x);
  ROS_INFO("%f", msg->pose.pose.position.x);
}

int main(int argc, char **argv) {
  // Create a node to run the code
  ros::init(argc, argv, "odom_sub_node");
  ros::NodeHandle nh;

  // create a subscriber to the "/odom" topic so we can get the odometry message
  ros::Subscriber sub = nh.subscribe("odom", 1000, counterCallback);
  
  // Run program until manually stopped
  ros::spin();

  return 0;
}

I have added comments to the code to explain what each line or code block does, for better clarity.

The `counterCallback` function prints out the current x position of the robot. Observe how parts of the message are accessed in hierarchical format; using the structure in Step 3 as a guide, you can access and output any part of the Odometry message.

You can uncomment the commented parts to print out parts of the header and twist messages or even write other statements that print out other parts of the message!

Step 5: Move the robot and see the odometry data printed in real-time!

Run the C++ program in the shell, to print out the current x position of the robot. We can see the robot is basically at the “origin” and is stationary. Leave this program running.

user:~$ rosrun odom_subscriber odom_sub_node
[ INFO] [1575339924.601377783, 49.183000000]: 0.000240
[ INFO] [1575339924.651682994, 49.233000000]: 0.000240
...

Pick another Shell tool from the tools menu and run a command to move the robot:

user:~$ rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 0.2
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0"
publishing and latching message. Press ctrl-C to terminate

Now you should see the robot moving. You should also see that the Odometry data being output in the other shell is changing.

In the second Shell, press Ctrl + C to stop the current program. Then publish the following message to stop the robot. After this, you should see the odometry position data become static again.

user:~$ rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0"
publishing and latching message. Press ctrl-C to terminate

Ant that was it!

Extra: Video

Prefer to watch a video demonstrating the steps above? We have one for you below!

Related Resources and Further Learning

If you are a ROS beginner and want to learn ROS basics fast, we recommend you take any of the following courses on Robot Ignite Academy:

Feedback

Did you like this post? Do you have questions about what is explained? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS topics, please let us know in the comments area and we will do a video or post about it.

—

Edited by Bayode Aderinola

—
#ROStutorials #Odometry #ROSsubscriber #ROS #Robot #C++

[Gazebo Q&A] 007 – Set Velocity of a Joint in Gazebo with Cpp

In this video we show you how to set the velocity of a joint with a plugin in gazebo written in C++. We show how to apply it to a two-wheel robot using a simple method and PID method. You can find the complete Gazebo Answer here.

Here is the ROSject of the project, just click and the whole project will be copied to your ROSDS workspace
Here is the code of this Answer
Here you have the link to RobotIgniteAcademy courses

[ROS Q&A] 161 – Action Status in C++ in ROS

In this video we are going to answer a common question that is how to set the succeded in an action server in c++ in ROS.

Related resources:

The original question
The ROSject of the project, just click and the whole project will be copied to your ROSDS workspace.
Here is the code of this Answer
Here you have the link to RobotIgniteAcademy courses

[ROS Q&A] 134 – Simple ROS Publisher in C++

In this video we are going to see how to create a simple ROS Publisher in C++.

* This is a video based on the question at the ROS Answers Forum

Step 1. Create a project in ROS Development Studio(ROSDS)

ROSDS helps you follow our tutorial in a fast pace without dealing without setting up an environment locally. If you haven’t had an account yet, you can create a free account here. Let’s create a new project and call it abc_qa.

Step 2. Create a package

Let’s create a new package called abc_qa with dependency for the code.

cd catkin_ws/src
catkin_create_pkg abc_qa roscpp

Then we create a source file called abc_code.cpp under the abc_qa/src directory with the following content

int main()
{
     //Initialize and start the node
     ros::Publisher pub;
     //Define and create some messages
     abc;
     while(ros : ok)
     {
           ros_time = ros::time::now();
           pub.publish(abc);
           ros::spinOnce();               
     }

 }

In order to compile the code, we have to change the following BUILD part in the CMakeLists.txt.

...
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(abc_node src/abc_code.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(abc_node ${abc_node_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
target_link_libraries(abc_node
  ${catkin_LIBRARIES}
)

...

Then we can try to compile it with the following command

cd ~/catkin_ws
catkin_make

You should get several compilation errors. Let’s fix the code. It should look like this after changing.

#include <ros/ros.h>
#include <std_msgs/Int32.h>

int main(int argc, char ** argv)
{
     //Initialize and start the node
     ros::init(argc, argv, "abc");
     ros::NodeHandle nh;
     
     ros::Publisher pub = nh.advertise<std_msgs::Int32>("abc_topic", 1000);
     //Define and create some messages
     std_msgs::Int32 abc;
     abc.data = 0;
     
     ros::Rate(200);
     
     while(ros::ok)
     {
           pub.publish(abc);
           ros::spinOnce();               
     }

 }

It should work now. Try to compile and run it again with the following command

cd ~/catkin_ws
catkin_make
source devel/setup.bash
rosrun abc_qa abc_node

Now we got error said that it’s missing master now. You can either run whatever simulation from Simulations or run roscore in a new shell to solve this problem. After launching the master, execute the code again. Although it shows nothing, it is actually starting to publish to topic /abc_topic you can see it with the command rostopic echo /abc_topic. You can also try to increment the counter in the while loop.

Related Course

ROS Basics C++ Course Cover - ROS Online Courses - Robot Ignite Academy

ROS Basics for Beginners (C++)

Feedback

Did you like this video? Do you have questions about what is explained? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other. If you want to learn about other ROS topics, please let us know in the comments area and we will do a video about it.

Next Entries »