Something seems to be off with the simulation, sometimes the robot would only move forward for a few seconds and gets stuck there physically, despite the function still printing out moving forward or turning.
Sometimes restarting the entire simulator will work…
#include "rosbot_control/rosbot_class.h"
#include <iostream>
#include <math.h>
#include <ros/ros.h>
#include <string>
using namespace std;
class RobotMove {
public:
RosbotClass rosbot;
void escape_kitchen();
};
void RobotMove::escape_kitchen() {
rosbot.move_forward(1);
ROS_INFO_STREAM("Laser frontal reading: " << rosbot.get_laser(0));
while (rosbot.get_laser(0) > 2.00) {
ROS_INFO_STREAM("Laser frontal reading: " << rosbot.get_laser(0));
rosbot.move_forward(1);
}
// ROS_INFO_STREAM("Laser left reading: " << rosbot.get_laser(431));
// turn 90 degress to the right
rosbot.turn("clockwise", 3);
rosbot.move_forward(5);
// ROS_INFO_STREAM("Laser left reading: " << rosbot.get_laser(431));
// cout << rosbot.get_laser(431) << endl;
// // cout << rosbot.get_laser(90) << endl;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "Rosbot_move_node");
RobotMove rosbot_moves;
rosbot_moves.escape_kitchen();
// rosbot_moves.avoid_wall();
}