diff --git a/include/roscontroller.h b/include/roscontroller.h index 4169307..74db7ae 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -111,6 +111,7 @@ private: ros::Subscriber Robot_id_sub; ros::Subscriber relative_altitude_sub; + std::string local_pos_sub_name; ros::Subscriber local_pos_sub; double local_pos_new[3]; diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml index c53989a..6ad2ec8 100644 --- a/launch/launch_config/solo.yaml +++ b/launch/launch_config/solo.yaml @@ -6,6 +6,7 @@ topics: setpoint: mavros/setpoint_position/local armclient: mavros/cmd/arming modeclient: mavros/set_mode + localpos: /mavros/local_position/pose stream: mavros/set_stream_rate altitude: mavros/global_position/rel_alt type: diff --git a/script/testsolo.bo b/script/testsolo.bo index d44323f..294076a 100644 Binary files a/script/testsolo.bo and b/script/testsolo.bo differ diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 84b7bff..b26e649 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -215,7 +215,8 @@ namespace rosbzz_node{ else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} if(node_handle.getParam("topics/stream", stream_client_name)); else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} - + if(node_handle.getParam("topics/localpos", local_pos_sub_name)); + else {ROS_ERROR("Provide a localpos name in YAML file"); system("rosnode kill rosbuzz_node");} @@ -250,9 +251,9 @@ namespace rosbzz_node{ stream_client = n_c.serviceClient(stream_client_name); users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos,this); + local_pos_sub = n_c.subscribe(local_pos_sub_name, 1000, &roscontroller::local_pos_callback, this); + - local_pos_sub = n_c.subscribe("/local_position", 1000, &roscontroller::local_pos_callback, this); - multi_msg=true; } /*---------------------------------------