I had a session today at 2:00 PM CST and the Turtlebot experienced considerable lag when responding to changes in the topic /cmd_vel. It continued spinning (the previous publish was vel.angular.z = 0.2) even after /cmd_vel showed that vel.angular.z = 0 and vel.linear.x = 0.1.
This occurred around line ~63 in the following code after the status update “Moving closer to the wall.”
#include "ros/duration.h"
#include "ros/init.h"
#include "ros/rate.h"
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "rosject/FindWall.h"
#include <bits/stdc++.h>
#include <sensor_msgs/LaserScan.h>
class controller {
public:
controller(){
pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);
sub = nh.subscribe("/scan", 10, &controller::laserCallback, this);
my_service = nh.advertiseService("/find_wall", &controller::find_Callback, this);
}
void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
// Identify which laser ray is the shortest one. Assume that this is the one pointing to a wall.
laser_readings.clear();
// creating vector of laser distances
for (int i = 0; i < msg -> ranges.size(); i++) {
laser_readings.push_back(msg -> ranges[i]);
}
}
bool find_Callback(rosject::FindWall::Request &req,
rosject::FindWall::Response &res)
{
ROS_INFO("find_Callback has been called"); // We print an string whenever the Service gets called
// Rotate the robot until the front of it is facing the wall. This can be done by publishing an angular velocity until front ray is the smallest one.
status_update = "Rotating to face the wall.";
ROS_INFO("%s\n", status_update.c_str());
while((laser_min < 355) || (laser_min > 365)){
// finding index of min value in vector
laser_min = distance(laser_readings.begin(), min_element(laser_readings.begin(),laser_readings.end()));
if (laser_min > 360){
// if wall on the left, turn left
vel.angular.z = 0.1;
} else if (laser_min < 360){
// if wall on the right, turn right
vel.angular.z = -0.1;
} else {
// if facing wall, stop
vel.angular.z = 0;
}
pub.publish(vel);
ros::spinOnce();
}
// Move the robot forward until the front ray is smaller than 30cm.
ROS_INFO("\n Min Element = %d", laser_min);
status_update = "Moving closer to the wall.";
ROS_INFO("%s\n", status_update.c_str());
while(laser_readings[360] > 0.3){
vel.angular.z = 0;
vel.linear.x = 0.1;
pub.publish(vel);
ros::spinOnce();
}
// Now rotate the robot again until ray number 270 of the laser ranges is pointing to the wall.
ROS_INFO("\n Min Element = %d", laser_min);
status_update = "Turning...";
ROS_INFO("%s\n", status_update.c_str());
while((laser_min < 265) || (laser_min > 275)){
vel.angular.z = 0.1;
vel.linear.x = 0;
pub.publish(vel);
laser_min = distance(laser_readings.begin(), min_element(laser_readings.begin(),laser_readings.end()));
ros::spinOnce();
}
// stop robot, ready for follow_wall
vel.angular.z = 0;
vel.linear.x = 0;
pub.publish(vel);
//ROS_INFO("\n Min Element = %d", laser_min);
status_update = "In position for follow_wall.";
ROS_INFO("%s\n", status_update.c_str());
// At this point, consider that the robot is ready to start following the wall.
// Return the service message with True.
ROS_INFO("find_Callback completed");
res.wallfound = true;
return true;
}
private:
ros::Subscriber sub;
ros::Publisher pub;
ros::NodeHandle nh;
ros::ServiceServer my_service;
std::string status_update;
geometry_msgs::Twist vel;
std::vector<float> laser_readings;
int laser_min;
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "find_wall");
controller ctrl;
ros::spin();
return 0;
}
