Robotics StackExchange | Archived questions

ros spin inside a class error

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-spinning-threading-queuing-aac9c0a793f

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, 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_4(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, 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);
}

hpp:

#include "ros/ros.h"
#include <thread>
#include <string>
#include <ros/callback_queue.h>

#include "std_msgs/Float64.h"
#include "std_msgs/Int64MultiArray.h"
#include "std_msgs/String.h"

// Motor IDs
#define motorId1 0x01
#define motorId2 0x02
#define motorId3 0x03
#define motorId4 0x04



class sub_pub
{
    public:
        sub_pub(ros::NodeHandle n1, ros::NodeHandle n2, ros::NodeHandle n3, ros::NodeHandle n4, ros::NodeHandle np);
        ~sub_pub();

        // Spinner
        void spinners(void);

    private:
        // Variables
        //string _pubTopic;
        // Nodehandlers
        ros::NodeHandle _n1;
        ros::NodeHandle _n2;
        ros::NodeHandle _n3;
        ros::NodeHandle _n4;
        ros::NodeHandle _np;
        // Callbacks queues
        ros::CallbackQueue _callback_queue_wheel_2;
        ros::CallbackQueue _callback_queue_wheel_3;
        ros::CallbackQueue _callback_queue_wheel_4;
        // Subscribers
        ros::Subscriber _pidWheel_1;
        ros::Subscriber _pidWheel_2;
        ros::Subscriber _pidWheel_3;
        ros::Subscriber _pidWheel_4;
        // Publisher
        ros::Publisher _pub;
        // Spinners
        ros::SingleThreadedSpinner _spinner_2;
        ros::SingleThreadedSpinner _spinner_3;
        ros::SingleThreadedSpinner _spinner_4;
        // Methods
        int* _rpmTobyte(double);
        // Callbacks
        void _callback_pidWheel_1(const std_msgs::Float64&);
        void _callback_pidWheel_2(const std_msgs::Float64&);
        void _callback_pidWheel_3(const std_msgs::Float64&);
        void _callback_pidWheel_4(const std_msgs::Float64&);
};

The problem comes when running the ros::spin()

[remap_ce_node-6] process has died [pid 24109, exit code -11, cmd /home/sibl/github/Real-Four-Wheel-Independent-Steering-Robot/devel/lib/controllers/remap_ce_node __name:=remap_ce_node __log:=/home/sibl/.ros/log/e84f01ba-3966-11ed-841f-556160d11213/remap_ce_node-6.log].
log file: /home/sibl/.ros/log/e84f01ba-3966-11ed-841f-556160d11213/remap_ce_node-6*.log

If I remove the ros::spin(), there is no error, but also it doesn't publish.

Asked by PabloV on 2022-09-21 01:23:11 UTC

Comments

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.

Asked by ravijoshi on 2022-09-21 01:49:09 UTC

Thank you! 1) solved my problem.

:D

Asked by PabloV on 2022-09-29 09:17:24 UTC

Glad you made it work!

Asked by ravijoshi on 2022-09-29 23:26:21 UTC

Answers