ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

Help - how to avoid obstacles with Turtlebot's LaserScan.

Hello,my name is Bernardo,I've got a problem and I don't know how to solve it. I want to create a very simple program: the robot advances in a straight line and as it approaches an obstacle, it must decelerate until it stops. The problem is that while I'm subscribing to the laser, the robot (when I start the run) moves at the speed I told him, but when it reaches the minimum distance, the robot doesn't stop and I don't know how to make him do that. Please, I need help. Here's the code:

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>

void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
{
geometry_msgs::Twist vel; //Declaración de la variable global vel

ros::NodeHandle n;
ros::Rate loop_rate(50);// Frecuencia de realializacion del bucle
ros::Time ahora;

for (unsigned int x=0;x< scan_msg->ranges.size();x++){

if(scan_msg->ranges[x]>=0.8) {

vel_pub_.publish(vel);

}

ahora = ros::Time::now();
if(scan_msg->ranges[x]<=0.7){

while (ros::Time::now() < (ahora + ros::Duration(0.5))){

vel_pub_.publish(vel);
loop_rate.sleep();

}
}

ahora = ros::Time::now();
if(scan_msg->ranges[x]<= 0.6){

while (ros::Time::now() < (ahora + ros::Duration(0.5))){
vel_pub_.publish(vel);}
loop_rate.sleep();

}

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

ros::NodeHandle n;
ros::Subscriber scan_sub = n.subscribe("scan", 1, scanCallback);

ros::spin(); //Mantiene la suscripción al topic hasta que se reciba "Ctrl+C"
return 0;

}


I've just managed to make my robot to stop when it's at a minimum distance,But, as you told me, I want to reduce the velocity with Vel=vel*0.5, but instead the robot always keeps moving at a constant velocity.What can I do? Thank you for your help

include <geometry_msgs twist.h="">

void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg) { geometry_msgs::Twist vel; //DeclaraciÃ³n de la variable global vel

ros::NodeHandle n; ros::Publisher vel_pub_=n.advertise<geometry_msgs::twist>("cmd_vel", 1); ros::Time ahora; double minDist=10;
double angle;
double speed=0.5;

for (unsigned int i=0;i< scan_msg->ranges.size();i++){

angle = ((double)i*scan_msg->angle_increment)+scan_msg->angle_min;

if (scan_msg->ranges[i]< minDist){
minDist=scan_msg->ranges[i];
std::cout << minDist << std::endl;
}
}

if( minDist<=0.6){

vel_pub_.publish(vel);
}
else {

speed=0.5*speed;
vel_pub_.publish(vel);
//std::cout << speed ...
edit retag close merge delete

Hi Bernardo, did you end up solving your problem? Is it possible for you to share the solution? I am doing a similar work with another robot and it would be very helpful to see how you solved this question =) thanks in advance!

( 2014-07-07 12:53:11 -0600 )edit

Sort by » oldest newest most voted

Like shade said, since your angular velocity seems to be constantly 0, you should limit your angular range to something significantly smaller than a whole range scan.

The real problem of you approach is that you scan point by point and decide for each one. This seems completely wrong. This way your robot will decide for EVERY point in your laser scan (probably changing it's mind a lot of times within one scan). What could be better is for each scan first loop through all points (with a single for) and if there is a point that is below your threshold (eg 0.8 meters) then decelerate. Also for lowering the speed gradually you might consider using vel=vel*0.8 or something similar.

more

You're recreating the published vel_pub_ in the scan callback. This should only happen once and then you should use that one (i.e. then one you create in the main) unless there is some nifty stuff in ROS that automatically reuses the same publication when another publisher is created for the same topic.

Besides that your program flow seems wrong. You are doing everything in the scan callback. This means your whole behaviour will only consider that one scan and new scans will be ignored.

In addition you do your behaviour for each range of the scan, not for each scan.

The usual way for such a behaviour would be to first check the minimal range to the scan and then adapt the velocity that is sent out regularly. Any waiting/slowing down logic need to be done incrementally in the background as you'd probably want to react to new scans.

Update: In your new version you are not saving the speed that you reduce, but you always recalculate that. In addition you should also set a minimum speed for approaching. There is also no case for not slowing down when far away.

more

+considering @shade's answer, you don't want to look at all the ranges, just the one's you're moving towards.

( 2013-02-01 03:35:55 -0600 )edit

Wat is the angle range of laser scan ??? If it is more -90 to +90 then it can easily happen that some of the sensor readings will be more than 0.8 even if ur obstacle is in front of the robot. So try determining the position of the obstacle using sensor reading. This can be easily done if you know the angle of range reading w.r.t to robot.So using that u can estimate the position of obstacle and accordingly reduce the velocity as ur doing above. Dont hesitate to ask if u have anymore questions.

more