Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Problem with data from subscriber not going anywhere

Related question: https://answers.ros.org/question/278181/issues-with-seg-faults-and-node-not-publishing-anything/

In my code there are two subscriber functions. I want to pass the shared pointer data to other data defined in my file. My code is below:

    std::vector<double> imu_, euler_t;
    ros::Publisher escs_pub;

    ros::Subscriber imu_sub;
    ros::Subscriber DOF_sub;
    double I_cm_xx, I_cm_yy, I_cm_zz, P_q, P_w;

    void KeyboardCallback(const maple::Euler_Thrust::ConstPtr& msg){
        euler_t.clear();
        euler_t.resize(4);
        euler_t.push_back((msg->psi)*3.14159265359/180);
        euler_t.push_back((msg->phi)*3.14159265359/180);
        euler_t.push_back((msg->theta)*3.14159265359/180);
        euler_t.push_back(msg->thrust);
    }

    void imuCallback(const sensor_msgs::Imu::ConstPtr &imu) {
        imu_.clear();
        imu_.resize(7);
        imu_.push_back(imu->orientation.x); 
        imu_.push_back(imu->orientation.y); 
        imu_.push_back(imu->orientation.z); 
        imu_.push_back(imu->orientation.w);
        imu_.push_back(imu->angular_velocity.x);
        imu_.push_back(imu->angular_velocity.y);
        imu_.push_back(imu->angular_velocity.z);
    }

    int main(int argc, char **argv){
        ros::init(argc, argv, "P_2");
            ros::NodeHandle nh;
            ros::NodeHandle pnh("~");
            pnh.param("I_cm_xx", I_cm_xx, 0.0118);
            pnh.param("I_cm_yy", I_cm_yy, 0.0118);
            pnh.param("I_cm_zz", I_cm_zz, 0.0181);
            pnh.param("P_q", P_q, 2.27);
            pnh.param("P_w", P_w, 0.85);
        while(ros::ok()){
    ROS_INFO("oy1");
            escs_pub = nh.advertise<maple::ESCs>("ESCs",1);
            DOF_sub = nh.subscribe("KeyboardTeleop", 1, KeyboardCallback);
            imu_sub = nh.subscribe("imu", 1, imuCallback);
            my_pkg::std_msg msg;

            double q_ref_w = cos(euler_t[0]/2)*cos(euler_t[1]/2)*cos(euler_t[2]/2); //THIS IS WHERE THE SEGMENTATION FAULT OCCURS, BECAUSE THE EULER_T VECTOR IS EMPTY OR UNPOPULATED!!!!!!

                  //more math  

            escs_pub.publish(msg);
            ros::spin();
        }

        return 0;
    }

I'm so lost... When excluding any operations with the vectors doing ROS_INFO inside the subscriber functions shows that they do indeed get the data too. I made this code as simple as it can be and it's still not working. I have a feeling it's a problem with the vectors not being passed the values in the right order and I tried that but nothing works. Please, any help would be massively appreciated