I am trying to solve exercise 4.4 where turtlebot is supposed to avoid a wall
For now I’m trying to make it stop at 1m distance from wall
But it doesnt and continues with same velocity
Laser
#include “ros/node_handle.h”
#include “ros/subscriber.h”
#include “ros/subscription_callback_helper.h”
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
void odom_Cb(const sensor_msgs::LaserScan::ConstPtr &msg) {
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>(“/cmd_vel”, 1000);
ros::Rate loop_rate(2);
geometry_msgs::Twist vel;
if (msg->ranges[180] >= 2.0) {
vel.linear.x = 0.05;
pub.publish(vel);
// loop_rate.sleep();
ROS_INFO(“gOING AHEAD, %f”, vel.linear.x);
ROS_INFO(“Distance ahead %f”, msg->ranges[360]);
} else {
vel.linear.x = 0;
ROS_INFO("Hitting wall, %f", vel.linear.x);
ROS_INFO("Distance ahead %f", msg->ranges[360]);
pub.publish(vel);
loop_rate.sleep();
}
// vel.linear.x = 0;
// pub.publish(vel);
}
int main(int argc, char **argv) {
ros::init(argc, argv, “topics_quiz_node”);
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe(“kobuki/laser/scan”, 1000, odom_Cb);
ros::spin();
return 0;
}