I don’t understand what I am doing wrong. The robot avoids an obstacle. The node name is correct (“topics_quiz_node”). No other programs are running.
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
float angVel = 0;
void laserCallback(const sensor_msgs::LaserScan::ConstPtr& laserScan)
{
angVel = 0.0;
ROS_INFO("Forward: %f", laserScan->ranges[360]);
ROS_INFO("Left: %f", laserScan->ranges[719]);
ROS_INFO("Right: %f", laserScan->ranges[719]);
if (laserScan->ranges[360]<1.0) angVel = 0.5;
if (laserScan->ranges[0]<1.0) angVel = 0.5;
if (laserScan->ranges[719]<1.0) angVel = -0.5;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "topics_quiz_node");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);
ros::Rate loop_rate(20);
ros::Subscriber sub = nh.subscribe("/kobuki/laser/scan", 1000, laserCallback);
geometry_msgs::Twist vel;
vel.linear.x = 0.1;
vel.angular.z = 0.0;
while (ros::ok())
{
vel.angular.z = angVel;
pub.publish(vel);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}