ros spin inside a class error

asked 2022-09-21 01:23:11 -0500

PabloV gravatar image

updated 2022-09-21 01:50:06 -0500

ravijoshi gravatar image

Hello,

I'm trying to subscribe to 4 different topics () and process the information (transform rpm to bytes) and then publish the result in a common topic (data don't need to be synchronized).

Something like this:

[topic1, topic2, topic3, topic4] ---> | Node | ----> [topic5]

I'm not proficient on C++, so I did my code based on:

https://levelup.gitconnected.com/ros-...

Thus, my node is as follow:

#include <sub_pub.hpp>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "steeringmotors_message");
    ros::NodeHandle n1, n2, n3, n4, np;
    sub_pub steering_motors(n1, n2, n3, n4, np);
    steering_motors.spinners();

    return 0;
}

and my class is defined as follow:

cpp:

#include "sub_pub.hpp"


sub_pub::sub_pub(ros::NodeHandle n1, ros::NodeHandle n2, ros::NodeHandle n3, ros::NodeHandle n4, ros::NodeHandle np)
{

    this->_n1 = n1;
    this->_n2 = n2;
    this->_n3 = n3;
    this->_n4 = n4;
    this->_np = np;



    this->_pidWheel_1 = this->_n1.subscribe("/wheel_1_steering_pid/control_effort", 1, &sub_pub::_callback_pidWheel_1, this);

    this->_n2.setCallbackQueue(&_callback_queue_wheel_2);
    this->_pidWheel_2 = this->_n2.subscribe("/wheel_2_steering_pid/control_effort", 1, &sub_pub::_callback_pidWheel_2, this);

    this->_n3.setCallbackQueue(&_callback_queue_wheel_3);
    this->_pidWheel_3 = this->_n3.subscribe("/wheel_3_steering_pid/control_effort", 1, &sub_pub::_callback_pidWheel_3, this);

    this->_n4.setCallbackQueue(&_callback_queue_wheel_4);
    this->_pidWheel_4 = this->_n4.subscribe("/wheel_4_steering_pid/control_effort", 1, &sub_pub::_callback_pidWheel_4, this) ;

    this->_pub = this->_np.advertise<std_msgs::Int64MultiArray>("/socket2/send", 1);

}

sub_pub::~sub_pub() 
{
}

int* sub_pub::_rpmTobyte(double recv_rpm)
{
    int rpm = (int)recv_rpm;
    static int bytes2return[2];
    // If rpm are positive
    if (rpm >= 0)
    {
        // 16 bytes = 8bytes0 8bytes1 
        bytes2return[1] = (rpm >> 8) & 0xff; //byte0
        bytes2return[0] = rpm & 0xff; //byte1
        return bytes2return;
    }

    // If rpm are negative
    else
    {
        int neg_dec;
        neg_dec = 65535 - abs(rpm); // FFFF- rpm
        // 16 bytes = 8bytes0 8bytes1 
        bytes2return[1] = (neg_dec >> 8) & 0xff;
        bytes2return[0] = neg_dec & 0xff;
        return bytes2return;
    }

}

void sub_pub::spinners(void)
{
    std::thread spinner_thread_wheel2([&]()
    {
        this->_spinner_2.spin(&_callback_queue_wheel_2);
    });

    std::thread spinner_thread_wheel3([&](){
        this->_spinner_3.spin(&_callback_queue_wheel_3);
    });

    std::thread spinner_thread_wheel4([&](){
        this->_spinner_4.spin(&_callback_queue_wheel_4);
    });

    ros::spin(); // spin the n1

    // Spin
    spinner_thread_wheel2.join();
    spinner_thread_wheel3.join();
    spinner_thread_wheel4.join();
}


void sub_pub::_callback_pidWheel_1(const std_msgs::Float64& value)
{
    //ros::NodeHandle np;
    //ros::Publisher pub = np.advertise<std_msgs::Int64MultiArray>("/socket2/send", 1);

    //                                     MotorID  PID130  b0   b1
    int setup_array[13] = {0x00, 0x00, 0x00, 0x01, 0x82, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

    // Get rpm bytes
    int *temp_p;
    temp_p = _rpmTobyte(value.data);
    // Setup array
    setup_array[5] = temp_p[0];
    setup_array[6] = temp_p[1];

    // Create and publish message
    std_msgs::Int64MultiArray msg;
    int i = 0;
    for (int e : setup_array)
    {
        msg.data[i] = e;
        i++;
    }
    this->_pub.publish(msg);
}


void sub_pub::_callback_pidWheel_2(const std_msgs::Float64& value)
{
    //ros::NodeHandle np;
    //ros::Publisher pub = np.advertise<std_msgs::String>("/socket2/send", 1);

    //                                     MotorID  PID130  b0   b1
    int setup_array[13] = {0x00, 0x00, 0x00, 0x02, 0x82, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

    // Get rpm bytes
    int *temp_p;
    temp_p = _rpmTobyte(value.data);
    // Setup array
    setup_array[5] = temp_p[0];
    setup_array[6] = temp_p[1];

    // Create and publish message
    std_msgs::Int64MultiArray msg;
    int i = 0;
    for (int e : setup_array)
    {
        msg.data[i] = e;
        i++;
    }
    this->_pub.publish(msg);

}

void sub_pub::_callback_pidWheel_3(const std_msgs::Float64& value)
{
    //ros::NodeHandle np;
    //ros::Publisher pub = np.advertise<std_msgs::String>("/socket2/send", 1);

    //                                     MotorID  PID130  b0   b1
    int setup_array[13] = {0x00, 0x00, 0x00, 0x03, 0x82, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 ...
(more)
edit retag flag offensive close merge delete

Comments

1

I had a glance at the code and found 2 issue.

  1. msg.data[i] = e; should be msg.data.push_back(e);.
  2. You need to define the data layout of your multi-array.

To me, it seems like a 1D array, though. I am not sure why you are using a multi-array. Furthermore, I suspect that there could be more issues in your code. Please run a debugger and fix them one by one.

ravijoshi gravatar image ravijoshi  ( 2022-09-21 01:49:09 -0500 )edit

Thank you! 1) solved my problem.

:D

PabloV gravatar image PabloV  ( 2022-09-29 09:17:24 -0500 )edit

Glad you made it work!

ravijoshi gravatar image ravijoshi  ( 2022-09-29 23:26:21 -0500 )edit