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!