Easy Navigation
Loading...
Searching...
No Matches
GoalManager.hpp
Go to the documentation of this file.
1// Copyright 2025 Intelligent Robotics Lab
2//
3// This file is part of the project Easy Navigation (EasyNav in short)
4// licensed under the GNU General Public License v3.0.
5// See <http://www.gnu.org/licenses/> for details.
6//
7// Easy Navigation program is free software: you can redistribute it and/or modify
8// it under the terms of the GNU General Public License as published by
9// the Free Software Foundation, either version 3 of the License, or
10// (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License
18// along with this program. If not, see <http://www.gnu.org/licenses/>.
19
22
23#ifndef EASYNAV_SYSTEM__GOALMANAGER_HPP_
24#define EASYNAV_SYSTEM__GOALMANAGER_HPP_
25
26#include <expected>
27
28#include "rclcpp/subscription.hpp"
29#include "rclcpp/publisher.hpp"
30#include "rclcpp/macros.hpp"
31#include "rclcpp_lifecycle/lifecycle_node.hpp"
32
33#include "easynav_interfaces/msg/navigation_control.hpp"
34#include "geometry_msgs/msg/pose_stamped.hpp"
35#include "nav_msgs/msg/goals.hpp"
36
38
39namespace easynav
40{
41
49{
50public:
51 RCLCPP_SMART_PTR_DEFINITIONS(GoalManager)
52
53
57 enum class State
58 {
61 };
62
69 NavState & nav_state,
70 rclcpp_lifecycle::LifecycleNode::SharedPtr parent_node);
71
76 [[nodiscard]] inline nav_msgs::msg::Goals get_goals() const {return goals_;}
77
82 [[nodiscard]] inline State get_state() const {return state_;}
83
87 void set_finished();
88
93 void set_failed(const std::string & reason);
94
99 void set_error(const std::string & reason);
100
104 void update(NavState & nav_state);
105
116 void check_goals(
117 const geometry_msgs::msg::Pose & current_pose,
118 double position_tolerance, double angle_tolerance);
119
120private:
122 rclcpp_lifecycle::LifecycleNode::SharedPtr parent_node_;
123
125 double position_tolerance_ {0.03};
126
128 double angle_tolerance_ {0.01};
129
131 nav_msgs::msg::Goals goals_;
132
134 rclcpp::Publisher<easynav_interfaces::msg::NavigationControl>::SharedPtr control_pub_;
135
137 rclcpp::Subscription<easynav_interfaces::msg::NavigationControl>::SharedPtr control_sub_;
138
140 rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr comanded_pose_sub_;
141
143 easynav_interfaces::msg::NavigationControl::UniquePtr last_control_;
144
146 std::string id_;
147
149 std::string current_client_id_;
150
152 bool allow_preempt_goal_ {true};
153
155 rclcpp::Time nav_start_time_;
156
158 void accept_request(
159 const easynav_interfaces::msg::NavigationControl & msg,
160 easynav_interfaces::msg::NavigationControl & response);
161
163 void control_callback(easynav_interfaces::msg::NavigationControl::UniquePtr msg);
164
166 void comanded_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg);
167
169 void set_preempted();
170
172 State state_ {State::IDLE};
173};
174
175} // namespace easynav
176
177#endif // EASYNAV_SYSTEM__GOALMANAGER_HPP_
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 &current_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