Rosbot_class does not return laser at range index 0, why?

General Support
1

Hi

i am going thrue the C++ for bewginners course, i did not find that catoagory so i will post the question here:

when i try and read from rosbot class the laser range at index 0, i get a:
Segmentation fault (core dumped) error
i have done a python course as well and there the indexing starts at 0, also the arrays first elemnt is always 0, so how come we start with 1 in this case?

here is my code:

#include "rosbot_control/rosbot_class.h"
#include <iostream>
#include <list>
#include <map>
#include <ros/ros.h>
#include <string>
using namespace std;

string my_function_to_move(RosbotClass rosbot, string direction, float n_sec){
    string succes;
    if (direction == "CW"){
        rosbot.turn("clockwise", n_sec);
        succes = "turning clockwise";
    }
    else if (direction == "CCW"){
        rosbot.turn("counterclockwise", n_sec);
        succes = " turning counterclockwise";
    }
    else if (direction =="FWD"){
        rosbot.move_forward(n_sec);
        succes = "rosbot moving forward";
    }
    else if (direction == "break"){
        succes = "breaking the loop";
    }
    else if (direction == "stop"){
        rosbot.stop_moving();
        succes = "robot stoped";
    }
    else {
        succes = "cant recognize command";
    }
    return succes;
}

float get_laser_Data(RosbotClass rosbot){
    //get laser ranges from 700 - 719, and from 0 - 20
    float range = rosbot.get_laser(0);
    cout << "\n range is: " << range;
    
    return range;

}



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

  RosbotClass rosbot;
  string direction = "empty", succes = "empty";
  float n_seconds;
  float laser_data = 0;
  

  while (direction != "break"){
      
        cout << "\n" << "Type direction you want to travel [CW / CCW / FWD / break / stop]  :  ";
        cin >> direction;
        cout << "\n you chose: " << direction << "\n";
        if (direction == "break"){
            break;
        }
        cout << "\n How many seconds would you like to travel?: ";
        cin >> n_seconds;
        cout << "\n you want to travel : " << n_seconds << "s \n";

        succes = my_function_to_move(rosbot, direction, n_seconds);
        printf("robot made : %s \n", succes.c_str());

        laser_data = get_laser_Data(rosbot);
        cout << "\n The laser at position 0 = " << laser_data;


  }

  return 0;
    
}

and here is the robot class, BTW there was a misstake in the original version at move_backwards, velocity linear should be -0.5 i think?

#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;
}
2

ok the problem is not in the get_laser(0) but in the code itself, can anyone help me troubleshoot this

if i:
rosrun c_scripts unit5_3_exercise

Type direction you want to travel [CW / CCW / FWD / break / stop] : FWD
then put the:
How many seconds would you like to travel?: 0

i get the :
Segmentation fault (core dumped) error and the program stops.

in scenario2 if i start the program from begenning:
rosrun c_scripts unit5_3_exercise

Type direction you want to travel [CW / CCW / FWD / break / stop] : FWD
then put the:
How many seconds would you like to travel?: 1

the whole code works, it is the same case if chose CW, or CCW, apart from with stop it does not matter what i put in n seconds, also if i run it after i first travel FWD or CW or CCW for 1 second it will work like how it was supposed to.

is this a problem with my code or does the problem come from how the Robot class is written?

3

Hello @ziga.rupret,

I’ve been doing some tests on this, but couldn’t find yet the source of the problem (although I suspect it’s related to the Robot class). Since it is not a blocking issue, I will keep doing some tests in the following days and let you know once I find out the reason.

Best,

4

ok cool, what ive managed to find is if you dont put the rosbot.move_forward(1); in the main program, it will not work (i would imagine move forward or turn or something)
i remember from python course we had to do the def publish inside a class, and basicaly there was a while statement like this:

def publish_once_in_cmd_vel(self):
        """
        This is because publishing in topics sometimes fails the first time you publish.
        In continuous publishing systems, this is no big deal, but in systems that publish only
        once, it IS very important.
        """
        while not self.ctrl_c:
            connections = self.vel_publisher.get_num_connections()
            if connections > 0:
                self.vel_publisher.publish(self.cmd)
                #rospy.loginfo("Cmd Published")
                break
            else:
                self.rate.sleep()

i dont know how to implement this in CPP, but the class we have here does not look to be waiting untill we have connections established? let me know what you find, but then again why does it work if we publish once in the main loop so maybe it is not that

Ziga

1 Like