Ask Your Question
1

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

asked 2013-02-01 02:49:00 -0500

Bernardo gravatar image

updated 2013-02-14 04:15:20 -0500

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::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
 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.linear.x = 0.3;//velocidad de avance
                vel.angular.z = 0.0;//velocidad de giro
                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.linear.x = 0.05;//velocidad de avance
                    vel.angular.z = 0.0;//velocidad de giro
                    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.linear.x = 0.0;//velocidad de avance
                    vel.angular.z = 0.0;//velocidad de giro
                    vel_pub_.publish(vel);}
                    loop_rate.sleep();

                }


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

  ros::NodeHandle n;
  ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
  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 <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::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.linear.x = 0.0;//velocidad de avance
            vel.angular.z = 0.0;//velocidad de giro
            vel_pub_.publish(vel);
 }
else {


            speed=0.5*speed;
            vel.linear.x = speed;//velocidad de avance
            vel.angular.z = 0.0;//velocidad de giro
            vel_pub_.publish(vel);
            //std::cout << speed ...
(more)
edit retag flag offensive close merge delete

Comments

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!

anamcarvalho gravatar image anamcarvalho  ( 2014-07-07 12:53:11 -0500 )edit

3 Answers

Sort by » oldest newest most voted
0

answered 2013-02-01 03:41:21 -0500

kabamaru gravatar image

updated 2013-02-01 03:48:08 -0500

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.

edit flag offensive delete link more
0

answered 2013-02-01 03:31:38 -0500

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.

edit flag offensive delete link more
0

answered 2013-02-01 03:33:48 -0500

dornhege gravatar image

updated 2013-02-04 04:38:10 -0500

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.

edit flag offensive delete link more

Comments

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

dornhege gravatar image dornhege  ( 2013-02-01 03:35:55 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2013-02-01 02:49:00 -0500

Seen: 5,172 times

Last updated: Feb 14 '13