ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Problem with data from subscriber not going anywhere

asked 2017-12-27 04:55:33 -0600

hanjuku-joshi gravatar image

Related question: https://answers.ros.org/question/2781...

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
3

answered 2017-12-27 07:39:02 -0600

There are a few issues here (some of which you could probably avoid if you go through the ROS tutorials carefully ;) )

  • Advertise and subscribe should be done outside the while loop as otherwise publishers and subscribers get re-initalized every time.
  • When running a while ros::ok() loop, you should call ros::spinOnce() instead of spin() as the latter is a blocking call.
  • The euler_t vector only gets resized in the subscriber callback. As callbacks only get serviced when a spin (or spinOnce, see above) has been called, it is perfectly normal that your code trying to access elements of the vector crashes.
  • Even if you would call spin correctly, there is no guarantee that the callback filling euler_t is called before you try accessing elements of it in the while-loop. For this reason (and many others) it's a good idea to check if the vector really has the correct size before trying to access it's elements directly.
  • Instead of filling vectors in the callbacks you could also store the shared pointer to the last received message and then check if that one is non-zero wherever you try to use the data.
edit flag offensive delete link more

Comments

Excellent advice. I actually just ended up solving the problem using arrays instead of vectors, but you certainly have me plenty information and advice to work on for the future. Legend :D

hanjuku-joshi gravatar image hanjuku-joshi  ( 2017-12-27 08:15:13 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2017-12-27 04:55:33 -0600

Seen: 167 times

Last updated: Dec 27 '17