From 92126729c5a4c080237ad45b0c294c8faeb6c7b3 Mon Sep 17 00:00:00 2001 From: Yuezhan Tao Date: Fri, 9 Dec 2022 10:38:55 -0500 Subject: [PATCH 1/2] Queue moere msgs to make sure get enough time to takeoff crazyflie --- kr_mav_manager/src/manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 1b3cf218..9df23546 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -624,7 +624,7 @@ bool MAVManager::set_motors(bool motors) // Queue a few to make sure the signal gets through. // Also, the crazyflie interface throttles commands to 30 Hz, so this needs // to have a sufficent duration. - for(int i = 0; i < 10; i++) + for(int i = 0; i < 20; i++) { pub_so3_command_.publish(so3_cmd); pub_trpy_command_.publish(trpy_cmd); From 9fb98c2821369ed59c1ca1b61c791f047efd44cb Mon Sep 17 00:00:00 2001 From: Yuezhan Tao Date: Sun, 11 Dec 2022 21:23:34 -0500 Subject: [PATCH 2/2] Make motors on msg queue number a param, with default value same as before --- kr_mav_manager/include/kr_mav_manager/manager.h | 3 +++ kr_mav_manager/src/manager.cpp | 5 ++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/kr_mav_manager/include/kr_mav_manager/manager.h b/kr_mav_manager/include/kr_mav_manager/manager.h index 6db59677..e00e0f40 100644 --- a/kr_mav_manager/include/kr_mav_manager/manager.h +++ b/kr_mav_manager/include/kr_mav_manager/manager.h @@ -188,6 +188,9 @@ class MAVManager // Services ros::ServiceClient srv_transition_; + + // Params + int motors_msg_queue_num_; }; } // namespace kr_mav_manager diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 9df23546..f9901cb0 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -135,6 +135,9 @@ MAVManager::MAVManager(std::string ns) // Disable motors if(!this->set_motors(false)) ROS_ERROR("Could not disable motors"); + + // Params + nh_.param("motors_msg_queue_num", motors_msg_queue_num_, 10); } void MAVManager::tracker_done_callback(const actionlib::SimpleClientGoalState &state, @@ -624,7 +627,7 @@ bool MAVManager::set_motors(bool motors) // Queue a few to make sure the signal gets through. // Also, the crazyflie interface throttles commands to 30 Hz, so this needs // to have a sufficent duration. - for(int i = 0; i < 20; i++) + for(int i = 0; i < motors_msg_queue_num_; i++) { pub_so3_command_.publish(so3_cmd); pub_trpy_command_.publish(trpy_cmd);