Cant pass laserscan massage betwene functions iside a class?(read last replay)

Hi i have done the python5 days and C++ intorduction course. and now i am working thrue the C++ course.
At the end of the python course there was an example of a class we can use and it was absolute geniouse, it made everything soooooooooooomuch more easy to understand. i have now started to program everything in classes,
but i must say that C++ is like 1 milion times harder to make a class where you can use a publisher and subscriber in the same class, there is not many examples and i am just lousing the will to live trying to figure this out.
Even to the point where i said right “i am stupid, i do not understand” so ive tried trial and error adding the & and * and even → operators to try and make the variables i read from the kobuki laser scan stay in a class variable so i can use it after the call back function but it just does not work…
can somone who know a bit more about C++ tell me what the hell i am dooing wrong

    #include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <list>
#include <vector>
#include <string>
#include <iostream>

using namespace std;

class RobotControllClass{
    float st10;
    int st_counter = 0;
    string vel_topic = "/cmd_vel";
    const string odom_topic = "/odom";
    const string laser_topic_test = "/kobuki/laser/scan";
    const string laser_topic_test_publish= "/laser_publish";

    public:
    vector<float> st550;
    nav_msgs::Odometry odom;
    geometry_msgs::Twist cmd;
    sensor_msgs::LaserScan::ConstPtr laser_scan_pointer;
    sensor_msgs::LaserScan laser_scan;
    ros::Publisher pub;
    ros::Publisher pub_laser;
    ros::Subscriber sub_laser;
    ros::NodeHandle nh;
    int counter;
    RobotControllClass(ros::NodeHandle *outside_nh){
        nh = *outside_nh;
        pub = nh.advertise<geometry_msgs::Twist> (nh.resolveName(vel_topic), 1000);
        pub_laser = nh.advertise<sensor_msgs::LaserScan> (nh.resolveName(laser_topic_test_publish), 1000);
        sub_laser = nh.subscribe(nh.resolveName(laser_topic_test), 1000, &RobotControllClass::laser_callback_number, this);
        usleep(2000000);
}
    void laser_callback_number(const sensor_msgs::LaserScan::ConstPtr &msg);
    void publish_to_topic();
    void publish_laser_scan_info();
    float get_laser_value();
    

};

    void RobotControllClass::laser_callback_number(const sensor_msgs::LaserScan::ConstPtr &msg){
        laser_scan = *msg;
        st550 = laser_scan.ranges;
        printf("number is: %f \n", st550[360]);
        // it gets printed here
        //st10 = msg -> range_min;
        //ROS_INFO("hello and fuck you");
        //pub_laser.publish(laser_scan);

    }


    void RobotControllClass::publish_to_topic(){
        //pub.publish(counter_msg);
        //ROS_INFO("message published was: %d", counter_msg.data);
        
    }

    float RobotControllClass::get_laser_value(){
    return 0;
        
    }
    void RobotControllClass::publish_laser_scan_info(){
        pub_laser.publish(laser_scan);
        float value = get_laser_value();
        cout << "hello: " << endl;
        printf("fuck offff: %f \n",1514 );
        //printf("number is: %f \n", st550[360]);
        /////////////////////////////////////////////////
        //but it does not work if i print it heere why???????????
    }



    int main (int argc, char** argv){
        ros::init(argc, argv, "node_test_node");
        ros::NodeHandle nh;
        ROS_INFO("ros beeing started");
        RobotControllClass st10(&nh);
        ros::Rate rate(1);
        while (ros::ok()){
            ROS_INFO("hello adn fuckyou");
            st10.publish_laser_scan_info();
            ros::spinOnce();
            rate.sleep();
        }
    }

The functions publish_laser_scan_info and laser_callback_number

i just want to pass the laser ranges on thrue the rest of the code, i dont understand why i cannot print them outside the callback functions why is that? is tehre any examples of a subscriber publisher class in C++

and i can pass on the range_min and max, i can pass the informaiton on the sequence number but i cannot pass ont the informaiton about ranges, why??

this is wierd as hell

why does this print out the vale:

void RobotControllClass::laser_callback_number(const sensor_msgs::LaserScan &msg){
    laser_scan = msg;
    st550 = laser_scan.ranges;
    st5511 =laser_scan.ranges[360];
    st551 = st550;
    //st650 = st550;
    printf("number is: %f \n",laser_scan.ranges[360]);
    // it gets printed here
    //st10 = msg -> range_min;
    //ROS_INFO("hello and fuck you");
    //pub_laser.publish(laser_scan);

}

but this will return the core dump error??

void RobotControllClass::publish_laser_scan_info(){
    pub_laser.publish(laser_scan);
    float value = get_laser_value();
    cout << "hello: " << endl;
    printf("fuck offff: %f \n",1514.15 );
    printf("number is: %f \n", laser_scan.ranges[360]);
    /////////////////////////////////////////////////
    //but it does not work if i print it heere why???????????
}

once i call it in main?

int main (int argc, char** argv){
ros::init(argc, argv, "node_test_node");
ros::NodeHandle nh;
ROS_INFO("ros beeing started");
RobotControllClass st10(&nh);
ros::Rate rate(1);
while (ros::ok()){
    ROS_INFO("hello adn fuckyou");
    st10.publish_laser_scan_info();
    ros::spinOnce();
    rate.sleep();
}
}

is it not possible to pass laserscan massage betwene functions inside a class?

ok i have managed to debug it, the problem is that the: laser scan is empty
so i have added a while loop untill the laser scan is initialized.
how do we solve this in C++, in python there is a wait for service

in python we have functions like:

 def W_LB_publish_once(self):
		 while not self.ctr_c:
			 connections = self._W_LB_publisher.get_num_connections()
			 if connections > 0:
				 self._W_LB_publisher.publish(self.W_LB_command)
				 break
			 else:
				 self.rate.sleep()

or this for waiting for service to come on line: from action servers, there must be something simmilar to just putting a while loop in the incializaiton?
client.wait_for_server()

this is the new code:
i even tested it out on a different laser topic, and it will only start working after i post the massage manuall using rostopic pub…
even if i dont put the 720 values in the ranges, it will return the value on 360th place at zero, and also it will return the st5511 variable that is in the laser_callback function, how do we properly initialize the message variables, what am i missing?

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <list>
#include <vector>
#include <string>
#include <iostream>

using namespace std;


class RobotControllClass{
    float st10;
    int counter_i =0 ;
    int st_counter = 0;
    string vel_topic = "/cmd_vel";
    const string odom_topic = "/odom";
    const string laser_topic_test = "/kobuki/laser/scan";
    const string laser_topic_test_publish= "/laser_publish";

    public:
    vector<float> st550;
    vector<float> st551;
    //vector<float> st552 = {5, 1, 5, 2, 15};
    
    float st650[720];
    float st5511;
    nav_msgs::Odometry odom;
    geometry_msgs::Twist cmd;
    sensor_msgs::LaserScan::ConstPtr laser_scan_pointer;
    sensor_msgs::LaserScan laser_scan;
    ros::Publisher pub;
    ros::Publisher pub_laser;
    ros::Subscriber sub_laser;
    ros::NodeHandle nh;
    int counter;
    RobotControllClass(ros::NodeHandle *outside_nh){
        nh = *outside_nh;
        pub = nh.advertise<geometry_msgs::Twist> (nh.resolveName(vel_topic), 1000);
        pub_laser = nh.advertise<sensor_msgs::LaserScan> (nh.resolveName(laser_topic_test_publish), 1000);
        sub_laser = nh.subscribe(nh.resolveName(laser_topic_test), 1000, &RobotControllClass::laser_callback_number, this);
        usleep(2000000);
}
    void laser_callback_number(const sensor_msgs::LaserScan &msg);

    void publish_to_topic();
    void publish_laser_scan_info();
    float get_laser_value();
    

};

void RobotControllClass::laser_callback_number(const sensor_msgs::LaserScan &msg){
    laser_scan = msg;
    st550 = laser_scan.ranges;
    st5511 =laser_scan.ranges[360];
    st551 = st550;
    //cout <<laser_scan.ranges.size()<<endl;
    //cout << st5511 <<endl;
    //for(int i=0;i<(laser_scan.ranges.size());i++){
    //        //_lsval.ranges[i]=msg->ranges[i];
    //        if(!(laser_scan.ranges.empty())){
    //            //ROS_INFO("data %f",laser_scan.ranges[i]);
    //        }
    //        else{
    //            ROS_INFO("NO VALUE");
    //        }
    //    }
    //vect[0] = 12633;
    //st650 = st550;
    //printf("number is: %f \n",vect[2]);
    // it gets printed here
    //st10 = msg -> range_min;
    //ROS_INFO("hello and fuck you");
    //pub_laser.publish(laser_scan);

}



void RobotControllClass::publish_to_topic(){
    //pub.publish(counter_msg);
    //ROS_INFO("message published was: %d", counter_msg.data);
    
}

float RobotControllClass::get_laser_value(){
return 0;
    
}
void RobotControllClass::publish_laser_scan_info(){
    pub_laser.publish(laser_scan);
    float value = get_laser_value();
    cout << "hello: " << endl;
    counter = counter_i++;
    printf("fuck offff: %i \n",counter_i );
    printf("number is: %f \n", laser_scan.ranges[360]);
    //printf("number is: %f \n", laser_scan.ranges[360]);
    /////////////////////////////////////////////////
    //but it does not work if i print it heere why???????????
}



int main (int argc, char** argv){
    ros::init(argc, argv, "node_test_node");
    ros::NodeHandle nh;
    ROS_INFO("ros beeing started");
    RobotControllClass st10(&nh);
    ros::Rate rate(1);
    while (st10.laser_scan.ranges.empty())
    {
        ROS_INFO("waiting for laser scan to start messageing");
        ros::spinOnce();
        rate.sleep();
    }

    while (ros::ok()){
        ROS_INFO("hello adn fuckyou");
        st10.publish_laser_scan_info();
        ros::spinOnce();
        rate.sleep();
    }
}

even if i do laser_scan.ranges.push_back(0); before the while loop
all it does is the numbers become just gibberish i guess it must be some sort of memmory locaiton.
so the only way to properly initialize it is to wait for subscriber to publish the data, what difference does it make?

sub_laser = nh.subscribe(nh.resolveName(laser_topic_test), 1000, &RobotControllClass::laser_callback_number, this);

this calback function is how the class should be initialized, is it because the whole massage gets filled in?

ok this is truly a mind F.
so i took a look at the class again and tided it up a little bit, mostly i deleted all the nasty language, moved some declaraitions betwene public and private, cleand up the laser_callback function, and made the initilaizer function, but now if i coment the initilaizer functon wich waits for the laser_scan.ranges.is_empty()
it does not produce the core dump error again

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <list>
#include <vector>
#include <string>
#include <iostream>

using namespace std;


class RobotControllClass{

    public:
    nav_msgs::Odometry odom;
    geometry_msgs::Twist cmd;
    sensor_msgs::LaserScan::ConstPtr laser_scan_pointer;
    sensor_msgs::LaserScan laser_scan;
    ros::Publisher pub;
    ros::Publisher pub_laser;
    ros::Subscriber sub_laser;
    ros::NodeHandle nh;
    int counter;
    void publish_laser_scan_info();
    RobotControllClass(ros::NodeHandle *outside_nh){
        nh = *outside_nh;
        pub = nh.advertise<geometry_msgs::Twist> (nh.resolveName(vel_topic), 1000);
        pub_laser = nh.advertise<sensor_msgs::LaserScan> (nh.resolveName(laser_topic_test_publish), 1000);
        sub_laser = nh.subscribe(nh.resolveName(laser_topic_test), 1000, &RobotControllClass::laser_callback_number, this);
        laser_scan.ranges.push_back(0);
        laser_scan.ranges.push_back(15);
        //initializer_function();
        }

    private:
    int internal_rate = 50; //refresh rate to wait for sensor data to get initialized
    int counter_i =0 ;
    const string vel_topic = "/cmd_vel";
    const string odom_topic = "/odom";
    const string laser_topic_test = "/kobuki/laser/scan";
    const string laser_topic_test_publish= "/laser_publish";
    void laser_callback_number(const sensor_msgs::LaserScan &msg);
    void initializer_function();

};

void RobotControllClass::laser_callback_number(const sensor_msgs::LaserScan &msg){
    laser_scan = msg;

}

void RobotControllClass::initializer_function(){
    ros::Rate rate_internal(internal_rate);

    while (laser_scan.ranges.empty()){
        ROS_INFO("waiting for laser scan to start messageing");
        ros::spinOnce();
        rate_internal.sleep();
    }

}

void RobotControllClass::publish_laser_scan_info(){
    printf("data at sensor 360= %f \n", laser_scan.ranges[0]);
    counter_i++;
    printf("counter = %i \n", counter_i);
}


int main (int argc, char** argv){
    ros::init(argc, argv, "node_test_node");
    ros::NodeHandle nh;
    ROS_INFO("ros beeing started");
    RobotControllClass st10(&nh);
    ros::Rate rate(1);

    while (ros::ok()){
        ROS_INFO("in main loop hello adn fuckyou");
        st10.publish_laser_scan_info();
        ros::spinOnce();
        rate.sleep();
    }
}

ok i have been having abit of monologe here lol, in previus reply i was using the laser_scan.ranges.push_back(0);
when i started the clas that is why i was not getting the core dump error anymore.
if i take this out i still need something simmilar to (w8 for the first massage to be published) otherwise it is going to be comming up with those errors

My question is is there a more elegante way of dooing this apart from how i have written it? like a number_connections or something like in python?

void RobotControllClass::initializer_function(){
    ros::Rate rate_internal(internal_rate);
    ROS_INFO("starting initilaizer function");
    while (laser_scan.ranges.empty()){
        ROS_INFO("Waiting for %s to started publishing", laser_topic_test.c_str());
        ros::spinOnce();
        rate_internal.sleep();
    }
    ROS_INFO("%s started publishing", laser_topic_test.c_str());

}

Hi Ziga,

Quite an interesting monologue!

I can’t help much with the C++ part, but there’s something missing in your code, which seems to be causing your head to spin() :smiley::

You need to include ros::spin() in your class init method, where your subscriber and publisher are initialized.

ros::spin() ensures that your subscriber messages arrive. It seems your messages were not arriving; with spin() included, you shouldn’t need to publish to the laser_scan message manually, neither do you need spinOnce().

ros::spin() and rospy.spin() are different, and here lies the difference. See this post for details:

1 Like