Merge pull request #12 from LARR-Planning/planner_control_thread

Change format
LARR-Planning · May 18, 2024 · 90eb649 · 90eb649
2 parents e66c61e + d5cc204
commit 90eb649
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 0 deletions.
diff --git a/larr_planner/include/larr_planner/wrapper/Wrapper.h b/larr_planner/include/larr_planner/wrapper/Wrapper.h
@@ -12,6 +12,7 @@ namespace larr_planner {
 class Wrapper {
 public:
   void SetRobotState(const RobotState &robot_state);
+  void OnPlanningTimerCallback();
 
 private:
 };

diff --git a/larr_planner/src/wrapper/Wrapper.cc b/larr_planner/src/wrapper/Wrapper.cc
@@ -3,3 +3,4 @@
 //
 #include "larr_planner/wrapper/Wrapper.h"
 void larr_planner::Wrapper::SetRobotState(const larr_planner::RobotState &robot_state) {}
+void larr_planner::Wrapper::OnPlanningTimerCallback() {}
diff --git a/larr_planner_ros2/include/larr_planner_ros2/planning_server/planning_server.h b/larr_planner_ros2/include/larr_planner_ros2/planning_server/planning_server.h
@@ -14,6 +14,7 @@
 
 using RobotStateMsg = geometry_msgs::msg::PoseStamped;
 using StateSubscriber = rclcpp::Subscription<RobotStateMsg>::SharedPtr;
+using RosTimer = rclcpp::TimerBase::SharedPtr;
 
 namespace larr_planner {
 
@@ -22,6 +23,11 @@ class PlanningServer : public rclcpp::Node {
   Wrapper *wrapper_ptr_;
   StateSubscriber state_subscriber_;
 
+  RosTimer planning_timer_;
+  RosTimer control_timer_;
+  void PlanningTimerCallback();
+  void ControlTimerCallback();
+
   void RobotStateCallback(const RobotStateMsg::ConstSharedPtr &msg);
   RobotState ConvertToRobotState(const RobotStateMsg &state_msg);
 

diff --git a/larr_planner_ros2/src/planning_server/planning_server.cc b/larr_planner_ros2/src/planning_server/planning_server.cc
@@ -10,6 +10,10 @@ PlanningServer::PlanningServer(const rclcpp::NodeOptions &options_input) : Node(
   options.callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   state_subscriber_ = create_subscription<RobotStateMsg>(
       "~/state", rclcpp::QoS(1), std::bind(&PlanningServer::RobotStateCallback, this, std::placeholders::_1), options);
+
+  using namespace std::chrono_literals;
+  control_timer_ = this->create_wall_timer(20ms, std::bind(&PlanningServer::ControlTimerCallback, this));
+  planning_timer_ = this->create_wall_timer(200ms, std::bind(&PlanningServer::PlanningTimerCallback, this));
 }
 
 void PlanningServer::RobotStateCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr &msg) {
@@ -30,3 +34,5 @@ RobotState PlanningServer::ConvertToRobotState(const RobotStateMsg &state_msg) {
 
   return state;
 }
+void PlanningServer::PlanningTimerCallback() { wrapper_ptr_->OnPlanningTimerCallback(); }
+void PlanningServer::ControlTimerCallback() {}