Strange behaviour get_position method

[ACTISA]

Hello, I have just started this C++ course, and despite not knowing what does the get_position method, as I would like to understand it, when called, it returns the coordinate value according to the argument you provide on the method: 1 for x coordinate, 2 for y coordinate, 3 for z coordinate.

But while doing the first exercise of the course I saw something strange that might need a fix.

The first exercise ask the user to check the position of the robot, move it and get the new position again.

The first time you run this program its perfect, if I prompt the values as logs in the ROS Network I can see:

`[INFO][<ROS_TIME>]: Initializing node.....`
`[INFO][<ROS_TIME>]: 0 and 0`
`[INFO][<ROS_TIME>]: 0.90301 and -0.0140998`

But the issue here is if I run again this node. If I don’t resent the robot position, it should print the last position we saw (0.93,-0.01), but when I run it I see the next prompt:

`[INFO][<ROS_TIME>]: Initializing node.....`
`[INFO][<ROS_TIME>]: 0 and 0`
`[INFO][<ROS_TIME>]: 1.9531 and -0.0144957`

I thought the method get_position would subscribe to the odom and check the position from it but seems it doesn’t work like that.

I just wanted to point this out just in case this behaviour wasn’t planned.

Thanks for advance and have a great day.
Ángel

[ralves]

Hi @ACTISA,

thank you very much for pointing out your observations.

I would like to ask you a question. When you first print and it shows “0 and 0”, is this the value returned by get_position, or are you printing it before you call get_position?

[ACTISA]

Hi Ralves,
thanks for your comment.

Here is the code I’m using:

#include "rosbot_control/rosbot_class.h"
#include <ros/ros.h>

int main(int argc, char **argv) {
  ros::init(argc, argv, "rosbot_node");

  RosbotClass rosbot;

  float coordinate_x = rosbot.get_position(1);
  float coordinate_y = rosbot.get_position(2);

  ROS_INFO_STREAM(coordinate_x << " and " << coordinate_y);

  rosbot.move();

  float coordinate_x2 = rosbot.get_position(1);
  float coordinate_y2 = rosbot.get_position(2);

  ROS_INFO_STREAM(coordinate_x2 << " and " << coordinate_y2);

  return 0;
}

As you can see, it is very simple, it just get the positions and print them using log, twice.
I wanted to check the RobotClass, but I’m not that confy into C++ OOP for making any conclusions.

Looking forward to know your / more opinions.

Bests,
Ángel

[ralves]

Hi @ACTISA ,

In the first Unit of the C++ for Robotics course, we are instructed to clone a repository:

cd ~/catkin_ws/src/

git clone https://bitbucket.org/theconstructcore/cpp_course_repo.git

Then, in the file you just shared here, we see:

#include "rosbot_control/rosbot_class.h"

This is instructing the compiler to include a file called rosbot_class.h

There are different ways of finding that rosbot_class.sh file.

First way

If the file exists in your workspace, you can search for it using the find command:

cd ~/catkin_ws/

find . -name rosbot_class.h

which will print:

./src/cpp_course_repo/rosbot_control/include/rosbot_control/rosbot_class.h

Then you can just open that file the explorer on the left side of the Code Editor

Second Way

A simpler way of finding a C++ file is by simultaneously pressing CTRL and clicking on the name of the file, in this case rosbot_class.h, that appears on #include "rosbot_control/rosbot_class.h"

In that rosbot_class.h file we see a “skeleton/prototype” of the method, which is:

float get_position(int param);

In the header files (.h files) we have the prototypes, and the implementations are in the .ccp files.
The .cpp file normally has the same name as the header file. For the rosbot_class.h, for example, the file is very likely to be rosbot_class.cpp. Let’s try to find it:

cd ~/catkin_ws/

find . -name rosbot_class.cpp

That output is this:

./src/cpp_course_repo/rosbot_control/src/rosbot_class.cpp

Now, we can just open that file and understand how get_position is implemented. I’ll now leave for you this task of checking how get_position is implemented.

Please let me know if you still have any questions.

[ACTISA]

Thanks Ralves for giving me these tips on finding the class.
I did indeed found it yesterday, but as I said, I’m not into OOP in C++ yet, so I don’t understand all the definition of the class.

But indeed, I did saw the callback of the odom topic, and it was refreshing the position of the robot everytime a message was published through odom, so I do not know what can be causing this issue…

Could it be that the program asked the position of the robot before the odom callback is being called?
But when defining the RobotClass, I don’t see any attributes that define the position, so shouldn’t this prompt an error that the variable ins’t defined (yet)?

I did see a timeout inside the definition of the class, maybe it is for this issue, that for the time the constructor of the class ends, there’s already a message on the odom callback queue so it defines the coordinates of the robot’s position, but then it should publish the correct initial position…

I attach the code of the constructor of the class so we can check it here and don’t have to go back to the IDE to check it:

#include "rosbot_control/rosbot_class.h"
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"
#include "sensor_msgs/LaserScan.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Int16.h"
#include "unistd.h"
#include <ros/ros.h>

#include <list>
#include <string>

using namespace std;

// RosbotClass constructor
RosbotClass::RosbotClass() {
  n = ros::NodeHandle("~");
  laser_topic = "/scan";
  laser_sub = n.subscribe(laser_topic, 10, &RosbotClass::laser_callback, this);
  vel_topic = "/cmd_vel";
  vel_pub = n.advertise<geometry_msgs::Twist>(n.resolveName(vel_topic), 1);
  odom_topic = "/odom";
  odom_sub = n.subscribe(odom_topic, 10, &RosbotClass::odom_callback, this);
  ROS_INFO("Initializing node .................................");
  usleep(2000000);
}

void RosbotClass::laser_callback(
    const sensor_msgs::LaserScan::ConstPtr &laser_msg) {
  laser_range = laser_msg->ranges;
  // ROS_INFO("Laser value: %f", laser_range);
}
void RosbotClass::odom_callback(const nav_msgs::Odometry::ConstPtr &odom_msg) {
  x_pos = odom_msg->pose.pose.position.x;
  y_pos = odom_msg->pose.pose.position.y;
  z_pos = odom_msg->pose.pose.position.z;
  // ROS_INFO_STREAM("Odometry: x=" << x_pos << " y=" << y_pos << " z=" <<
  // z_pos);
}
void RosbotClass::move() {
  // Rate of publishing
  ros::Rate rate(10);

  ros::Time start_time = ros::Time::now();
  ros::Duration timeout(2.0); // Timeout of 2 seconds
  while (ros::Time::now() - start_time < timeout) {
    ros::spinOnce();
    vel_msg.linear.x = +0.5;
    vel_msg.angular.z = 0.0;
    vel_pub.publish(vel_msg);
    rate.sleep();
  }
  vel_msg.linear.x = 0.0;
  vel_msg.angular.z = 0.0;
  vel_pub.publish(vel_msg);
}

void RosbotClass::move_forward(int time) {
  // Rate of publishing
  ros::Rate rate(10);

  ros::Time start_time = ros::Time::now();
  ros::Duration timeout(time);
  while (ros::Time::now() - start_time < timeout) {
    ROS_INFO_STREAM("Moving forward ........... ");
    ros::spinOnce();
    vel_msg.linear.x = 0.4;
    vel_msg.angular.z = 0.0;
    vel_pub.publish(vel_msg);
    rate.sleep();
  }
  vel_msg.linear.x = 0.0;
  vel_msg.angular.z = 0.0;
  vel_pub.publish(vel_msg);
}

void RosbotClass::move_backwards(int time) {
  // Rate of publishing
  ros::Rate rate(10);

  ros::Time start_time = ros::Time::now();
  ros::Duration timeout(time);
  while (ros::Time::now() - start_time < timeout) {
    ROS_INFO_STREAM("Moving backwards ........... ");
    ros::spinOnce();
    vel_msg.linear.x = -0.5;
    vel_msg.angular.z = 0.0;
    vel_pub.publish(vel_msg);
    rate.sleep();
  }
  vel_msg.linear.x = 0.0;
  vel_msg.angular.z = 0.0;
  vel_pub.publish(vel_msg);
}

void RosbotClass::turn(string clock, int n_secs) {
  ros::Rate rate(10);
  ros::Time start_time = ros::Time::now();
  ros::Duration timeout(n_secs);

  double WZ = 0.0;
  if (clock == "clockwise") {
    ROS_INFO_STREAM("Turning clockwise..............");
    WZ = -2.5;
  } else if (clock == "counterclockwise") {
    ROS_INFO_STREAM("Turning counterclockwiseeee ........... ");
    WZ = 2.5;
  }

  while (ros::Time::now() - start_time < timeout) {
    ros::spinOnce();
    vel_msg.linear.x = 0.5;
    vel_msg.angular.z = WZ;
    vel_pub.publish(vel_msg);
    rate.sleep();
  }
  vel_msg.linear.x = 0.0;
  vel_msg.angular.z = 0.0;
  vel_pub.publish(vel_msg);
}

void RosbotClass::stop_moving() {
  ROS_INFO_STREAM("Stopping the robot ........... ");
  vel_msg.linear.x = 0.0;
  vel_msg.angular.z = 0.0;
  vel_pub.publish(vel_msg);
}

float RosbotClass::get_position(int param) {
  if (param == 1) {
    return this->x_pos;
  } else if (param == 2) {
    return this->y_pos;
  } else if (param == 3) {
    return this->z_pos;
  }
  return 0;
}

list<float> RosbotClass::get_position_full() {
  list<float> coordinates({this->x_pos, this->y_pos, this->z_pos});
  return coordinates;
}

double RosbotClass::get_time() {
  double secs = ros::Time::now().toSec();
  return secs;
}

float RosbotClass::get_laser(int index) { return laser_range[index]; }

float *RosbotClass::get_laser_full() {
  float *laser_range_pointer = laser_range.data();
  return laser_range_pointer;
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "rosbot_class_node");

  RosbotClass rosbot;

  rosbot.move();

  float coordinate = rosbot.get_position(1);

  ROS_INFO_STREAM(coordinate);

  return 0;
}

Bests,
Ángel

[ralves]

Hi @ACTISA ,

You said:

@ACTISA

But when defining the RobotClass, I don’t see any attributes that define the position, so shouldn’t this prompt an error that the variable ins’t defined (yet)?

Variables are normally defined in the in the “.h” header files.

The reason why the value is zero is probably because when you first called the method, the callback that sets the position was not executed yet.

The reason why the default value is zero, is because this is the default value set by ROS, when you create a variable that was defined by ROS, as you can see in the “Default values” section of the link below:

• Interface definition using .msg / .srv / .action files

A field can optionally specify a default value. If no default value is specified a common default value is used:

• for bool it is false
• for numeric types it is a 0 value
• for string it is an empty string
• for static size arrays it is an array of N elements with its fields zero-initialized
• for bounded size arrays and dynamic size arrays it is an empty array []

[ACTISA]

Hello @ralves , I’m sorry for answering late, but I was busy in Italy hehe.
Thanks a lot for the “Default values” link, I never thought about it.

But then, which would be the way to fix this? I thought of using a delay with functions such as usleep but it keeps prompting the 0 and 0 on the coordinates…