Subscriber not working

edit

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;
}