int main(int argc, char **argv)
{
ros::init(argc, argv, "navigation_node");
ros::NodeHandle nh;
//enable callback
bool enable_imu, enable_mag, enable_attitude;
//leggo i parametri dal lauch file
nh.param<bool>("/NavigationSystem/imu_enable", enable_imu, false);
nh.param<bool>("/NavigationSystem/mag_enable", enable_mag, false);
nh.param<bool>("/NavigationSystem/attitude_enable", enable_attitude, false);
nh.param<int>("/NavigationSystem/log_file", log_file, 1);
nh.param<int>("/NavigationSystem/freq_ros_node", freq_ros_node, 50);
//imu del drone
if(enable_imu)
imu_sub = nh.subscribe<sensor_msgs::Imu>("/mavros/imu/data_raw", 1, imu_cb);
//magnetometro del drone
if(enable_mag)
mag_sub = nh.subscribe<sensor_msgs::MagneticField>("/mavros/imu/mag", 1, mag_cb);
//assetto del drone
if(enable_attitude)
attitude_sub = nh.subscribe<sensor_msgs::Imu>("/mavros/imu/data", 1, attitude_cb);
//service client
get_time_sec0 = nh.serviceClient<autopilot_manager::init_time>("/get_time_t0");
init_global_var();
//frequenza a cui far girare il nodo
ros::Rate loop_rate(freq_ros_node);
while(ros::ok())
{
//A)leggo i topic
ros::spinOnce();
//loop rate
loop_rate.sleep();
}
return 0;
}
void init()
{
init_global_var();
_stop_threads = 0;
_host = NULL;
}
