/
navigate_to_pose.hpp
136 lines (115 loc) · 4.37 KB
/
navigate_to_pose.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
// Copyright (c) 2021 Samsung Research
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_
#define NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_
#include <string>
#include <vector>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_bt_navigator/navigator.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_util/odometry_utils.hpp"
namespace nav2_bt_navigator
{
/**
* @class NavigateToPoseNavigator
* @brief A navigator for navigating to a specified pose
*/
class NavigateToPoseNavigator
: public nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateToPose>
{
public:
using ActionT = nav2_msgs::action::NavigateToPose;
/**
* @brief A constructor for NavigateToPoseNavigator
*/
NavigateToPoseNavigator()
: Navigator() {}
/**
* @brief A configure state transition to configure navigator's state
* @param node Weakptr to the lifecycle node
* @param odom_smoother Object to get current smoothed robot's speed
*/
bool configure(
rclcpp_lifecycle::LifecycleNode::WeakPtr node,
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother) override;
/**
* @brief A cleanup state transition to remove memory allocated
*/
bool cleanup() override;
/**
* @brief A subscription and callback to handle the topic-based goal published
* from rviz
* @param pose Pose received via atopic
*/
void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose);
/**
* @brief Get action name for this navigator
* @return string Name of action server
*/
std::string getName() override {return std::string("navigate_to_pose");}
/**
* @brief Get navigator's default BT
* @param node WeakPtr to the lifecycle node
* @return string Filepath to default XML
*/
std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) override;
protected:
/**
* @brief A callback to be called when a new goal is received by the BT action server
* Can be used to check if goal is valid and put values on
* the blackboard which depend on the received goal
* @param goal Action template's goal message
* @return bool if goal was received successfully to be processed
*/
bool goalReceived(ActionT::Goal::ConstSharedPtr goal) override;
/**
* @brief A callback that defines execution that happens on one iteration through the BT
* Can be used to publish action feedback
*/
void onLoop() override;
/**
* @brief A callback that is called when a preempt is requested
*/
void onPreempt(ActionT::Goal::ConstSharedPtr goal) override;
/**
* @brief A callback that is called when a the action is completed, can fill in
* action result message or indicate that this action is done.
* @param result Action template result message to populate
* @param final_bt_status Resulting status of the behavior tree execution that may be
* referenced while populating the result.
*/
void goalCompleted(
typename ActionT::Result::SharedPtr result,
const nav2_behavior_tree::BtStatus final_bt_status) override;
/**
* @brief Goal pose initialization on the blackboard
* @param goal Action template's goal message to process
*/
void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal);
rclcpp::Time start_time_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
rclcpp_action::Client<ActionT>::SharedPtr self_client_;
std::string goal_blackboard_id_;
std::string path_blackboard_id_;
// Odometry smoother object
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
};
} // namespace nav2_bt_navigator
#endif // NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_