diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp index e4428ef..3b7e57f 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -84,6 +84,8 @@ namespace Modelec rclcpp::Subscription::SharedPtr start_odo_sub_; bool start_odo_ = false; + rclcpp::TimerBase::SharedPtr odo_get_pos_timer_; + // Promises and mutexes to synchronize service responses asynchronously std::queue> tof_promises_; std::mutex tof_mutex_; diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 8f12e18..280d8db 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -181,6 +181,16 @@ namespace Modelec SendOrder("START", {std::to_string(msg->data)}); } }); + + odo_get_pos_timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), + [this]() + { + if (isOk && start_odo_) + { + GetPos(); + } + }); } PCBOdoInterface::~PCBOdoInterface()