Skip to content

Commit bd8f422

Browse files
PrabhavSaxenaPrabhavSaxena
authored
Addition of position goal checker, fix of rotation shim controller (#5162)
* added position goal checker plugin Signed-off-by: PrabhavSaxena <[email protected]> * updated rotation shim to use position goal checker Signed-off-by: PrabhavSaxena <[email protected]> * fixed lint Signed-off-by: PrabhavSaxena <[email protected]> * added comments Signed-off-by: PrabhavSaxena <[email protected]> * fixed dynamic parameter, CMake Signed-off-by: PrabhavSaxena <[email protected]> --------- Signed-off-by: PrabhavSaxena <[email protected]>
1 parent c101adc commit bd8f422

9 files changed

Lines changed: 275 additions & 68 deletions

File tree

‎nav2_controller/CMakeLists.txt‎

Lines changed: 22 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -145,6 +145,26 @@ target_link_libraries(stopped_goal_checker PRIVATE
145145
pluginlib::pluginlib
146146
)
147147

148+
add_library(position_goal_checker SHARED plugins/position_goal_checker.cpp)
149+
target_include_directories(position_goal_checker
150+
PUBLIC
151+
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
152+
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
153+
)
154+
target_link_libraries(position_goal_checker PUBLIC
155+
${geometry_msgs_TARGETS}
156+
nav2_core::nav2_core
157+
nav2_costmap_2d::nav2_costmap_2d_core
158+
rclcpp::rclcpp
159+
rclcpp_lifecycle::rclcpp_lifecycle
160+
${rcl_interfaces_TARGETS}
161+
)
162+
target_link_libraries(position_goal_checker PRIVATE
163+
angles::angles
164+
nav2_util::nav2_util_core
165+
pluginlib::pluginlib
166+
)
167+
148168
if(BUILD_TESTING)
149169
find_package(ament_lint_auto REQUIRED)
150170
# the following line skips the linter which checks for copyrights
@@ -161,7 +181,7 @@ endif()
161181

162182
rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer")
163183

164-
install(TARGETS simple_progress_checker pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name}
184+
install(TARGETS simple_progress_checker position_goal_checker pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name}
165185
EXPORT ${library_name}
166186
ARCHIVE DESTINATION lib
167187
LIBRARY DESTINATION lib
@@ -181,6 +201,7 @@ ament_export_libraries(simple_progress_checker
181201
pose_progress_checker
182202
simple_goal_checker
183203
stopped_goal_checker
204+
position_goal_checker
184205
${library_name})
185206
ament_export_dependencies(
186207
geometry_msgs

‎nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp‎

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
// Copyright (c) 2025 Prabhav Saxena
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_
16+
#define NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_
17+
18+
#include <string>
19+
#include <memory>
20+
#include <vector>
21+
#include "rclcpp/rclcpp.hpp"
22+
#include "rclcpp_lifecycle/lifecycle_node.hpp"
23+
#include "nav2_core/goal_checker.hpp"
24+
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
25+
26+
namespace nav2_controller
27+
{
28+
29+
/**
30+
* @class PositionGoalChecker
31+
* @brief Goal Checker plugin that only checks XY position, ignoring orientation
32+
*/
33+
class PositionGoalChecker : public nav2_core::GoalChecker
34+
{
35+
public:
36+
PositionGoalChecker();
37+
~PositionGoalChecker() override = default;
38+
39+
void initialize(
40+
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
41+
const std::string & plugin_name,
42+
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
43+
44+
void reset() override;
45+
46+
bool isGoalReached(
47+
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
48+
const geometry_msgs::msg::Twist & velocity) override;
49+
50+
bool getTolerances(
51+
geometry_msgs::msg::Pose & pose_tolerance,
52+
geometry_msgs::msg::Twist & vel_tolerance) override;
53+
54+
/**
55+
* @brief Set the XY goal tolerance
56+
* @param tolerance New tolerance value
57+
*/
58+
void setXYGoalTolerance(double tolerance);
59+
60+
protected:
61+
double xy_goal_tolerance_;
62+
double xy_goal_tolerance_sq_;
63+
bool stateful_;
64+
bool position_reached_;
65+
std::string plugin_name_;
66+
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
67+
68+
/**
69+
* @brief Callback executed when a parameter change is detected
70+
* @param parameters list of changed parameters
71+
*/
72+
rcl_interfaces::msg::SetParametersResult
73+
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
74+
};
75+
76+
} // namespace nav2_controller
77+
78+
#endif // NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_

‎nav2_controller/plugins.xml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,4 +19,9 @@
1919
<description>Checks linear and angular velocity after stopping</description>
2020
</class>
2121
</library>
22+
<library path="position_goal_checker">
23+
<class type="nav2_controller::PositionGoalChecker" base_class_type="nav2_core::GoalChecker">
24+
<description>Goal checker that only checks XY position and ignores orientation</description>
25+
</class>
26+
</library>
2227
</class_libraries>

‎nav2_controller/plugins/position_goal_checker.cpp‎

Lines changed: 150 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,150 @@
1+
// Copyright (c) 2025 Prabhav Saxena
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <memory>
16+
#include <string>
17+
#include <limits>
18+
#include "nav2_controller/plugins/position_goal_checker.hpp"
19+
#include "pluginlib/class_list_macros.hpp"
20+
#include "nav2_util/node_utils.hpp"
21+
22+
using rcl_interfaces::msg::ParameterType;
23+
using std::placeholders::_1;
24+
25+
namespace nav2_controller
26+
{
27+
28+
PositionGoalChecker::PositionGoalChecker()
29+
: xy_goal_tolerance_(0.25),
30+
xy_goal_tolerance_sq_(0.0625),
31+
stateful_(true),
32+
position_reached_(false)
33+
{
34+
}
35+
36+
void PositionGoalChecker::initialize(
37+
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
38+
const std::string & plugin_name,
39+
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS>/*costmap_ros*/)
40+
{
41+
plugin_name_ = plugin_name;
42+
auto node = parent.lock();
43+
44+
nav2_util::declare_parameter_if_not_declared(
45+
node,
46+
plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25));
47+
nav2_util::declare_parameter_if_not_declared(
48+
node,
49+
plugin_name + ".stateful", rclcpp::ParameterValue(true));
50+
51+
node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_);
52+
node->get_parameter(plugin_name + ".stateful", stateful_);
53+
54+
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
55+
56+
// Add callback for dynamic parameters
57+
dyn_params_handler_ = node->add_on_set_parameters_callback(
58+
std::bind(&PositionGoalChecker::dynamicParametersCallback, this, _1));
59+
}
60+
61+
void PositionGoalChecker::reset()
62+
{
63+
position_reached_ = false;
64+
}
65+
66+
bool PositionGoalChecker::isGoalReached(
67+
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
68+
const geometry_msgs::msg::Twist &)
69+
{
70+
// If stateful and position was already reached, maintain state
71+
if (stateful_ && position_reached_) {
72+
return true;
73+
}
74+
75+
// Check if position is within tolerance
76+
double dx = query_pose.position.x - goal_pose.position.x;
77+
double dy = query_pose.position.y - goal_pose.position.y;
78+
79+
bool position_reached = (dx * dx + dy * dy <= xy_goal_tolerance_sq_);
80+
81+
// If stateful, remember that we reached the position
82+
if (stateful_ && position_reached) {
83+
position_reached_ = true;
84+
}
85+
86+
return position_reached;
87+
}
88+
89+
bool PositionGoalChecker::getTolerances(
90+
geometry_msgs::msg::Pose & pose_tolerance,
91+
geometry_msgs::msg::Twist & vel_tolerance)
92+
{
93+
double invalid_field = std::numeric_limits<double>::lowest();
94+
95+
pose_tolerance.position.x = xy_goal_tolerance_;
96+
pose_tolerance.position.y = xy_goal_tolerance_;
97+
pose_tolerance.position.z = invalid_field;
98+
99+
// Return zero orientation tolerance as we don't check it
100+
pose_tolerance.orientation.x = 0.0;
101+
pose_tolerance.orientation.y = 0.0;
102+
pose_tolerance.orientation.z = 0.0;
103+
pose_tolerance.orientation.w = 1.0;
104+
105+
vel_tolerance.linear.x = invalid_field;
106+
vel_tolerance.linear.y = invalid_field;
107+
vel_tolerance.linear.z = invalid_field;
108+
109+
vel_tolerance.angular.x = invalid_field;
110+
vel_tolerance.angular.y = invalid_field;
111+
vel_tolerance.angular.z = invalid_field;
112+
113+
return true;
114+
}
115+
116+
void nav2_controller::PositionGoalChecker::setXYGoalTolerance(double tolerance)
117+
{
118+
xy_goal_tolerance_ = tolerance;
119+
xy_goal_tolerance_sq_ = tolerance * tolerance;
120+
}
121+
122+
rcl_interfaces::msg::SetParametersResult
123+
PositionGoalChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
124+
{
125+
rcl_interfaces::msg::SetParametersResult result;
126+
for (auto & parameter : parameters) {
127+
const auto & param_type = parameter.get_type();
128+
const auto & param_name = parameter.get_name();
129+
if (param_name.find(plugin_name_ + ".") != 0) {
130+
continue;
131+
}
132+
133+
if (param_type == ParameterType::PARAMETER_DOUBLE) {
134+
if (param_name == plugin_name_ + ".xy_goal_tolerance") {
135+
xy_goal_tolerance_ = parameter.as_double();
136+
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
137+
}
138+
} else if (param_type == ParameterType::PARAMETER_BOOL) {
139+
if (param_name == plugin_name_ + ".stateful") {
140+
stateful_ = parameter.as_bool();
141+
}
142+
}
143+
}
144+
result.successful = true;
145+
return result;
146+
}
147+
148+
} // namespace nav2_controller
149+
150+
PLUGINLIB_EXPORT_CLASS(nav2_controller::PositionGoalChecker, nav2_core::GoalChecker)

‎nav2_rotation_shim_controller/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@ find_package(ament_cmake REQUIRED)
55
find_package(angles REQUIRED)
66
find_package(geometry_msgs REQUIRED)
77
find_package(nav2_common REQUIRED)
8+
find_package(nav2_controller REQUIRED)
89
find_package(nav2_core REQUIRED)
910
find_package(nav2_costmap_2d REQUIRED)
1011
find_package(nav2_util REQUIRED)
@@ -29,6 +30,7 @@ target_include_directories(${library_name}
2930
)
3031
target_link_libraries(${library_name} PUBLIC
3132
${geometry_msgs_TARGETS}
33+
nav2_controller::position_goal_checker
3234
nav2_core::nav2_core
3335
nav2_costmap_2d::nav2_costmap_2d_core
3436
pluginlib::pluginlib
@@ -69,6 +71,7 @@ ament_export_libraries(${library_name})
6971
ament_export_dependencies(
7072
geometry_msgs
7173
nav2_core
74+
nav2_controller
7275
nav2_costmap_2d
7376
pluginlib
7477
rclcpp

‎nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
#include "nav2_core/controller.hpp"
2929
#include "nav2_core/controller_exceptions.hpp"
3030
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
31+
#include "nav2_controller/plugins/position_goal_checker.hpp"
3132

3233
namespace nav2_rotation_shim_controller
3334
{
@@ -192,6 +193,7 @@ class RotationShimController : public nav2_core::Controller
192193
// Dynamic parameters handler
193194
std::mutex mutex_;
194195
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
196+
std::unique_ptr<nav2_controller::PositionGoalChecker> position_goal_checker_;
195197
};
196198

197199
} // namespace nav2_rotation_shim_controller

‎nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp‎

Lines changed: 0 additions & 60 deletions
This file was deleted.

‎nav2_rotation_shim_controller/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
<depend>angles</depend>
1414
<depend>geometry_msgs</depend>
1515
<depend>nav2_core</depend>
16+
<depend>nav2_controller</depend>
1617
<depend>nav2_costmap_2d</depend>
1718
<depend>nav2_msgs</depend>
1819
<depend>nav2_util</depend>

0 commit comments

Comments
 (0)