Subscriber not working
Greeting. Sory if I made any mistake, English is not native language for me. I have three subscribers that supposed to change global variable but, as far as I understand, or they don't call or they don't change this variable. Sorry if this is something simple or obvious, but I spend quite amount of time trying to understand what is wrong.
Header:
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/LaserScan.h>
#include <math.h>
#include <stdlib.h>
void onIRSensor0(const sensor_msgs::Range &msg);
void onIRSensor1(const sensor_msgs::Range &msg);
void onIRSensor2(const sensor_msgs::Range &msg);
void onLaserSensor(const sensor_msgs::LaserScan &msg);
geometry_msgs::Twist update_velocity();
float const MYROBOT_MAX_LIN_VEL = 0.6;
float const MYROBOT_MAX_ANG_VEL = 2.0;
int const MYROBOT_SPEED = 1;
static int const SEGMENTS = 24;
float dist_front_0;
float dist_front_1;
float dist_front_2;
float target_linear_velocity = 0.0;
float target_angular_velocity = 0.0;
float distances[SEGMENTS] = {};
Node itself:
#include <auto_teleop_c.h>
float lerp(float a, float b, float coeff){
return float(a) + float(b-a) * float(coeff);
}
void onIRSensor0(const sensor_msgs::Range &msg){
ROS_INFO("inside onIRSensor0");
dist_front_0 = msg.range / msg.max_range;
}
void onIRSensor1(const sensor_msgs::Range &msg){
ROS_INFO("inside onIRSensor1");
dist_front_1 = msg.range / msg.max_range;
}
void onIRSensor2(const sensor_msgs::Range &msg){
ROS_INFO("inside onIRSensor2");
dist_front_2 = msg.range / msg.max_range;
}
void onLaserSensor(const sensor_msgs::LaserScan &msg){
for (int i = 0; i < SEGMENTS; i++){
distances[i] = 9999.0;
}
float angle = msg.angle_min;
float r = 0.0;
for (int i = 0; i < msg.ranges.size(); i++){
if (isinf(msg.ranges[i]))
float r = 12.0;
else
float r = msg.ranges[i];
float a = angle * 180 / M_PI;
int idx = int(a / (360 / SEGMENTS)) % SEGMENTS;
distances[idx] = std::min(distances[idx], r);
angle = angle + msg.angle_increment;
}
}
geometry_msgs::Twist update_velocity(){
if (dist_front_1 < 0.8){
target_angular_velocity = MYROBOT_MAX_ANG_VEL;
}
else if (dist_front_0 < 0.8){
target_linear_velocity = MYROBOT_MAX_LIN_VEL * dist_front_0;
target_angular_velocity = MYROBOT_MAX_ANG_VEL * (1 - dist_front_0);
}
else if (dist_front_2 < 0.8){
target_linear_velocity = MYROBOT_MAX_LIN_VEL * dist_front_2;
target_angular_velocity = -MYROBOT_MAX_ANG_VEL * (1 - dist_front_2);
}
else{
ROS_INFO("Case 4");
int best_idx = 12;
float best_val = distances[best_idx];
for (int i = 8; i <=16; i++){
if (distances[i] > best_val){
best_val = distances[i];
best_idx = i;
}
}
float coeff = float(best_idx - 6) / 12.0;
target_angular_velocity = lerp(-float(MYROBOT_MAX_ANG_VEL), float(MYROBOT_MAX_ANG_VEL), coeff);
target_linear_velocity = MYROBOT_MAX_LIN_VEL * (1.0 - abs(coeff - 0.5) * 2.0);
ROS_INFO("target_angular_velocity : %f", target_angular_velocity);
ROS_INFO("target_linear_velocity : %f", target_linear_velocity);
}
geometry_msgs::Twist twist;
twist.linear.x = target_linear_velocity; twist.linear.y = 0.0; twist.linear.z = 0.0;
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = target_angular_velocity;
return twist;
}
int main(int argc, char **argv){
ros::init(argc, argv, "keyboard_teleop");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/controller/cmd_vel", 10);
ros::Subscriber sub_front_0 = nh.subscribe("/myrobot/sensor/front_0", 1000, onIRSensor0);
ros::Subscriber sub_front_1 = nh.subscribe("/myrobot/sensor/front_1", 1000, onIRSensor1);
ros::Subscriber sub_front_2 = nh.subscribe("/myrobot/sensor/front_2", 1000, onIRSensor2);
ros::Subscriber sub_lidar = nh.subscribe("/myrobot/rplidar/scan", 1000, onLaserSensor);
ros::Rate rate(10);
while (ros::ok()){
pub.publish(update_velocity());
rate.sleep();
}
ros::spin();
return 0;
}