multithread can't read 2 topics
I am trying to subscribe to multiple topics and then publish them in a single personal topic, somehow i can't make it run with 2 topics... it only gets the subscription callback for fcu/current_pose (yes, there are fcu/status being published) so it never reaches the publish (which btw also gives me error if uncommented)
this is my code atm (please ignore those ugly global variables (or provide me a better solution if pos))
#include <string>
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <tf/transform_broadcaster.h>
#include "std_msgs/String.h"
#include <geometry_msgs/PoseStamped.h>
#include <asctec_hl_comm/mav_status.h>
//configuration parameters
int factorFrecuency = 10;
int msg_queue_size = 500;
double cpuLoadActual = 0.0;
double actual_poseX = 0.0;
double actual_poseY = 0.0;
double actual_poseZ = 0.0;
//update flags
bool recivedStatus = false;
bool recivedPose = false;
//publisher & subscriber
ros::Publisher reducerPublisher;
ros::Subscriber multiSubscriber;
//callbacks
void poseCallback(const geometry_msgs::PoseStamped& msg)
{
if (!recivedPose) {
ROS_INFO(" - Recived first data X, Y y Z.");
}
actual_poseX = msg.pose.position.x;
actual_poseY = msg.pose.position.y;
actual_poseZ = msg.pose.position.z;
ROS_INFO("location: X[%lf], Y[%lf], Z[%lf]", actual_poseX, actual_poseY, actual_poseZ);
recivedPose = true;
}
void statusCallback(const asctec_hl_comm::mav_status& msg)
{
if (!recivedStatus) {
ROS_INFO(" - Recived first data CPU.");
}
cpuLoadActual = msg.cpu_load;
ROS_INFO("cpu: [%lf]", cpuLoadActual);
recivedStatus = true;
}
//publish new topic
void publishReduced(){
if(recivedStatus && recivedPose){
std_msgs::String message;
std::stringstream ss;
ss << "hello subscribers to reductions "; //<< cpuLoadActual << " y posicionamiento posicion del dron: X" << actual_poseX << " Y" << actual_poseY << " Z" << actual_poseZ;
message.data = ss.str();
//reducerPublisher.publish(ss); //<- error S:
}
}
int main(int argc, char **argv)
{
//inicializar ros
ROS_INFO("reducer testing");
ros::init(argc, argv, "Reducer");
ros::NodeHandle node;
//Topic you want to publish
reducerPublisher = node.advertise<std_msgs::String>("reductions", 1000);
//Topics to subscribe
multiSubscriber = node.subscribe("fcu/status", msg_queue_size, &statusCallback);
multiSubscriber = node.subscribe("fcu/current_pose", msg_queue_size, &poseCallback);
// Ratio de iteración (Hz)
int loop_rate_hz = factorFrecuency;
ros::Rate loop_rate(loop_rate_hz);
ros::AsyncSpinner spinner(4); // Use 4 threads
spinner.start();
while (ros::ok())
{
publishReduced();
loop_rate.sleep();
}
return 0;
};