Problem with data from subscriber not going anywhere
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