0
$\begingroup$

Rosanswers logo

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 << std::endl;  
               
  
}

} 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 suscripcion al topic hasta que se reciba "Ctrl+C"

return 0;


UPDATE************************************************************************


Hello again. I managed to make my robot dodge obstacles, but there are certain points in which Turtlebot doesn't take the appropiate paths, especially when it finds a wall. The robot doesn't go to the free space and, instead, it goes directly to the wall, and I don't know what to do to improve it. Could you tell me some advice? .This is my code:

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

double angle; double distance=10;

void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg) {

for (unsigned int i=0;i< scan_msg->ranges.size();i++){   
               if (scan_msg->ranges[i]<distance){
                   distance=scan_msg->ranges[i];  
                   angle = ((double)i*scan_msg->angle_increment)+scan_msg->angle_min;
                }
                  std::cout << distance <<" ------ "<< angle << std::endl; }
  }

int main(int argc, char** argv){
 ros::init(argc, argv, "avoidObstacle");
 ros::NodeHandle n;
 ros::Publisher vel_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
 ros::Subscriber scan_sub = n.subscribe("scan", 10, scanCallback);
 geometry_msgs::Twist vel;
 ros::Rate loop_rate(10);
  while(n.ok()){

   if (distance>=0.7 && distance <=0.9){
     if (angle <=0.0){
      vel.linear.x=0.2;
      vel.angular.z=0.6;
      vel_pub_.publish(vel);}
        else {
          if (angle>0){
            vel.linear.x=0.2;
            vel.angular.z=-0.6;
            vel_pub_.publish(vel);
          }
         }
      }
       else {
          vel.linear.x=0.15;
          vel.angular.z=0.0;
          vel_pub_.publish(vel);
     }
    distance=10;     
    ros::spinOnce(); 
    loop_rate.sleep();} 
 return 0;
 }

Originally posted by Bernardo on ROS Answers with karma: 41 on 2013-02-01

Post score: 1


Original comments

Comment by anamcarvalho on 2014-07-07:
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!

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by kabamaru with karma: 35 on 2013-02-01

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.