23#ifndef EASYNAV_SYSTEM__GOALMANAGER_HPP_
24#define EASYNAV_SYSTEM__GOALMANAGER_HPP_
28#include "rclcpp/subscription.hpp"
29#include "rclcpp/publisher.hpp"
30#include "rclcpp/macros.hpp"
31#include "rclcpp_lifecycle/lifecycle_node.hpp"
33#include "easynav_interfaces/msg/navigation_control.hpp"
34#include "geometry_msgs/msg/pose_stamped.hpp"
35#include "nav_msgs/msg/goals.hpp"
70 rclcpp_lifecycle::LifecycleNode::SharedPtr parent_node);
76 [[nodiscard]]
inline nav_msgs::msg::Goals
get_goals()
const {
return goals_;}
99 void set_error(
const std::string & reason);
117 const geometry_msgs::msg::Pose & current_pose,
118 double position_tolerance,
double angle_tolerance);
122 rclcpp_lifecycle::LifecycleNode::SharedPtr parent_node_;
125 double position_tolerance_ {0.03};
128 double angle_tolerance_ {0.01};
131 nav_msgs::msg::Goals goals_;
134 rclcpp::Publisher<easynav_interfaces::msg::NavigationControl>::SharedPtr control_pub_;
137 rclcpp::Subscription<easynav_interfaces::msg::NavigationControl>::SharedPtr control_sub_;
140 rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr comanded_pose_sub_;
143 easynav_interfaces::msg::NavigationControl::UniquePtr last_control_;
149 std::string current_client_id_;
152 bool allow_preempt_goal_ {
true};
155 rclcpp::Time nav_start_time_;
159 const easynav_interfaces::msg::NavigationControl & msg,
160 easynav_interfaces::msg::NavigationControl & response);
163 void control_callback(easynav_interfaces::msg::NavigationControl::UniquePtr msg);
166 void comanded_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg);
169 void set_preempted();
A blackboard-like structure to hold the current state of the navigation system.
void update(NavState &nav_state)
Update internal logic, including preemption and timeout checks.
Definition GoalManager.cpp:259
nav_msgs::msg::Goals get_goals() const
Get current goals.
Definition GoalManager.hpp:76
void check_goals(const geometry_msgs::msg::Pose ¤t_pose, double position_tolerance, double angle_tolerance)
Check if the robot is currently at the first goal.
Definition GoalManager.cpp:314
GoalManager(NavState &nav_state, rclcpp_lifecycle::LifecycleNode::SharedPtr parent_node)
Constructor.
Definition GoalManager.cpp:31
void set_finished()
Mark the current goal as successfully completed.
Definition GoalManager.cpp:191
State
Current internal goal state.
Definition GoalManager.hpp:58
@ ACTIVE
A goal is currently being pursued.
Definition GoalManager.hpp:60
@ IDLE
No active goal.
Definition GoalManager.hpp:59
void set_error(const std::string &reason)
Mark the system as in error state.
Definition GoalManager.cpp:209
State get_state() const
Get current internal goal state.
Definition GoalManager.hpp:82
void set_failed(const std::string &reason)
Mark the current goal as failed.
Definition GoalManager.cpp:227
A generic, type-safe, lock-free blackboard to hold runtime state.
Definition NavState.hpp:62
Definition RTTFBuffer.hpp:30