add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,97 @@
|
||||
// Copyright (c) 2023 Dexory
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_controller/plugins/pose_progress_checker.hpp"
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include "angles/angles.h"
|
||||
#include "nav_2d_utils/conversions.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "geometry_msgs/msg/pose2_d.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
using std::placeholders::_1;
|
||||
|
||||
namespace nav2_controller
|
||||
{
|
||||
|
||||
void PoseProgressChecker::initialize(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
const std::string & plugin_name)
|
||||
{
|
||||
plugin_name_ = plugin_name;
|
||||
SimpleProgressChecker::initialize(parent, plugin_name);
|
||||
auto node = parent.lock();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, plugin_name + ".required_movement_angle", rclcpp::ParameterValue(0.5));
|
||||
node->get_parameter_or(plugin_name + ".required_movement_angle", required_movement_angle_, 0.5);
|
||||
|
||||
// Add callback for dynamic parameters
|
||||
dyn_params_handler_ = node->add_on_set_parameters_callback(
|
||||
std::bind(&PoseProgressChecker::dynamicParametersCallback, this, _1));
|
||||
}
|
||||
|
||||
bool PoseProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose)
|
||||
{
|
||||
// relies on short circuit evaluation to not call is_robot_moved_enough if
|
||||
// baseline_pose is not set.
|
||||
geometry_msgs::msg::Pose2D current_pose2d;
|
||||
current_pose2d = nav_2d_utils::poseToPose2D(current_pose.pose);
|
||||
|
||||
if (!baseline_pose_set_ || PoseProgressChecker::isRobotMovedEnough(current_pose2d)) {
|
||||
resetBaselinePose(current_pose2d);
|
||||
return true;
|
||||
}
|
||||
return clock_->now() - baseline_time_ <= time_allowance_;
|
||||
}
|
||||
|
||||
bool PoseProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
return pose_distance(pose, baseline_pose_) > radius_ ||
|
||||
poseAngleDistance(pose, baseline_pose_) > required_movement_angle_;
|
||||
}
|
||||
|
||||
double PoseProgressChecker::poseAngleDistance(
|
||||
const geometry_msgs::msg::Pose2D & pose1,
|
||||
const geometry_msgs::msg::Pose2D & pose2)
|
||||
{
|
||||
return abs(angles::shortest_angular_distance(pose1.theta, pose2.theta));
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
PoseProgressChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
for (auto parameter : parameters) {
|
||||
const auto & type = parameter.get_type();
|
||||
const auto & name = parameter.get_name();
|
||||
|
||||
if (type == ParameterType::PARAMETER_DOUBLE) {
|
||||
if (name == plugin_name_ + ".required_movement_angle") {
|
||||
required_movement_angle_ = parameter.as_double();
|
||||
}
|
||||
}
|
||||
}
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace nav2_controller
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_controller::PoseProgressChecker, nav2_core::ProgressChecker)
|
||||
@@ -0,0 +1,147 @@
|
||||
// Copyright (c) 2025 Prabhav Saxena
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
#include "nav2_controller/plugins/position_goal_checker.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
using std::placeholders::_1;
|
||||
|
||||
namespace nav2_controller
|
||||
{
|
||||
|
||||
PositionGoalChecker::PositionGoalChecker()
|
||||
: xy_goal_tolerance_(0.25),
|
||||
xy_goal_tolerance_sq_(0.0625),
|
||||
stateful_(true),
|
||||
position_reached_(false)
|
||||
{
|
||||
}
|
||||
|
||||
void PositionGoalChecker::initialize(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
const std::string & plugin_name,
|
||||
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS>/*costmap_ros*/)
|
||||
{
|
||||
plugin_name_ = plugin_name;
|
||||
auto node = parent.lock();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
plugin_name + ".stateful", rclcpp::ParameterValue(true));
|
||||
|
||||
node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_);
|
||||
node->get_parameter(plugin_name + ".stateful", stateful_);
|
||||
|
||||
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
|
||||
|
||||
// Add callback for dynamic parameters
|
||||
dyn_params_handler_ = node->add_on_set_parameters_callback(
|
||||
std::bind(&PositionGoalChecker::dynamicParametersCallback, this, _1));
|
||||
}
|
||||
|
||||
void PositionGoalChecker::reset()
|
||||
{
|
||||
position_reached_ = false;
|
||||
}
|
||||
|
||||
bool PositionGoalChecker::isGoalReached(
|
||||
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
|
||||
const geometry_msgs::msg::Twist &)
|
||||
{
|
||||
// If stateful and position was already reached, maintain state
|
||||
if (stateful_ && position_reached_) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Check if position is within tolerance
|
||||
double dx = query_pose.position.x - goal_pose.position.x;
|
||||
double dy = query_pose.position.y - goal_pose.position.y;
|
||||
|
||||
bool position_reached = (dx * dx + dy * dy <= xy_goal_tolerance_sq_);
|
||||
|
||||
// If stateful, remember that we reached the position
|
||||
if (stateful_ && position_reached) {
|
||||
position_reached_ = true;
|
||||
}
|
||||
|
||||
return position_reached;
|
||||
}
|
||||
|
||||
bool PositionGoalChecker::getTolerances(
|
||||
geometry_msgs::msg::Pose & pose_tolerance,
|
||||
geometry_msgs::msg::Twist & vel_tolerance)
|
||||
{
|
||||
double invalid_field = std::numeric_limits<double>::lowest();
|
||||
|
||||
pose_tolerance.position.x = xy_goal_tolerance_;
|
||||
pose_tolerance.position.y = xy_goal_tolerance_;
|
||||
pose_tolerance.position.z = invalid_field;
|
||||
|
||||
// Return zero orientation tolerance as we don't check it
|
||||
pose_tolerance.orientation.x = 0.0;
|
||||
pose_tolerance.orientation.y = 0.0;
|
||||
pose_tolerance.orientation.z = 0.0;
|
||||
pose_tolerance.orientation.w = 1.0;
|
||||
|
||||
vel_tolerance.linear.x = invalid_field;
|
||||
vel_tolerance.linear.y = invalid_field;
|
||||
vel_tolerance.linear.z = invalid_field;
|
||||
|
||||
vel_tolerance.angular.x = invalid_field;
|
||||
vel_tolerance.angular.y = invalid_field;
|
||||
vel_tolerance.angular.z = invalid_field;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void nav2_controller::PositionGoalChecker::setXYGoalTolerance(double tolerance)
|
||||
{
|
||||
xy_goal_tolerance_ = tolerance;
|
||||
xy_goal_tolerance_sq_ = tolerance * tolerance;
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
PositionGoalChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
for (auto & parameter : parameters) {
|
||||
const auto & type = parameter.get_type();
|
||||
const auto & name = parameter.get_name();
|
||||
|
||||
if (type == ParameterType::PARAMETER_DOUBLE) {
|
||||
if (name == plugin_name_ + ".xy_goal_tolerance") {
|
||||
xy_goal_tolerance_ = parameter.as_double();
|
||||
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_BOOL) {
|
||||
if (name == plugin_name_ + ".stateful") {
|
||||
stateful_ = parameter.as_bool();
|
||||
}
|
||||
}
|
||||
}
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace nav2_controller
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_controller::PositionGoalChecker, nav2_core::GoalChecker)
|
||||
@@ -0,0 +1,170 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
#include "nav2_controller/plugins/simple_goal_checker.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "angles/angles.h"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wpedantic"
|
||||
#include "tf2/utils.h"
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
using std::placeholders::_1;
|
||||
|
||||
namespace nav2_controller
|
||||
{
|
||||
|
||||
SimpleGoalChecker::SimpleGoalChecker()
|
||||
: xy_goal_tolerance_(0.25),
|
||||
yaw_goal_tolerance_(0.25),
|
||||
stateful_(true),
|
||||
check_xy_(true),
|
||||
xy_goal_tolerance_sq_(0.0625)
|
||||
{
|
||||
}
|
||||
|
||||
void SimpleGoalChecker::initialize(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
const std::string & plugin_name,
|
||||
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS>/*costmap_ros*/)
|
||||
{
|
||||
plugin_name_ = plugin_name;
|
||||
auto node = parent.lock();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
plugin_name + ".yaw_goal_tolerance", rclcpp::ParameterValue(0.25));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
plugin_name + ".stateful", rclcpp::ParameterValue(true));
|
||||
|
||||
node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_);
|
||||
node->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_);
|
||||
node->get_parameter(plugin_name + ".stateful", stateful_);
|
||||
|
||||
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
|
||||
|
||||
// Add callback for dynamic parameters
|
||||
dyn_params_handler_ = node->add_on_set_parameters_callback(
|
||||
std::bind(&SimpleGoalChecker::dynamicParametersCallback, this, _1));
|
||||
}
|
||||
|
||||
void SimpleGoalChecker::reset()
|
||||
{
|
||||
check_xy_ = true;
|
||||
}
|
||||
|
||||
bool SimpleGoalChecker::isGoalReached(
|
||||
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
|
||||
const geometry_msgs::msg::Twist &)
|
||||
{
|
||||
if (check_xy_) {
|
||||
double dx = query_pose.position.x - goal_pose.position.x,
|
||||
dy = query_pose.position.y - goal_pose.position.y;
|
||||
if (dx * dx + dy * dy > xy_goal_tolerance_sq_) {
|
||||
return false;
|
||||
}
|
||||
// We are within the window
|
||||
// If we are stateful, change the state.
|
||||
if (stateful_) {
|
||||
check_xy_ = false;
|
||||
}
|
||||
}
|
||||
double dyaw = angles::shortest_angular_distance(
|
||||
tf2::getYaw(query_pose.orientation),
|
||||
tf2::getYaw(goal_pose.orientation));
|
||||
return fabs(dyaw) < yaw_goal_tolerance_;
|
||||
}
|
||||
|
||||
bool SimpleGoalChecker::getTolerances(
|
||||
geometry_msgs::msg::Pose & pose_tolerance,
|
||||
geometry_msgs::msg::Twist & vel_tolerance)
|
||||
{
|
||||
double invalid_field = std::numeric_limits<double>::lowest();
|
||||
|
||||
pose_tolerance.position.x = xy_goal_tolerance_;
|
||||
pose_tolerance.position.y = xy_goal_tolerance_;
|
||||
pose_tolerance.position.z = invalid_field;
|
||||
pose_tolerance.orientation =
|
||||
nav2_util::geometry_utils::orientationAroundZAxis(yaw_goal_tolerance_);
|
||||
|
||||
vel_tolerance.linear.x = invalid_field;
|
||||
vel_tolerance.linear.y = invalid_field;
|
||||
vel_tolerance.linear.z = invalid_field;
|
||||
|
||||
vel_tolerance.angular.x = invalid_field;
|
||||
vel_tolerance.angular.y = invalid_field;
|
||||
vel_tolerance.angular.z = invalid_field;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
SimpleGoalChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
for (auto & parameter : parameters) {
|
||||
const auto & type = parameter.get_type();
|
||||
const auto & name = parameter.get_name();
|
||||
|
||||
if (type == ParameterType::PARAMETER_DOUBLE) {
|
||||
if (name == plugin_name_ + ".xy_goal_tolerance") {
|
||||
xy_goal_tolerance_ = parameter.as_double();
|
||||
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
|
||||
} else if (name == plugin_name_ + ".yaw_goal_tolerance") {
|
||||
yaw_goal_tolerance_ = parameter.as_double();
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_BOOL) {
|
||||
if (name == plugin_name_ + ".stateful") {
|
||||
stateful_ = parameter.as_bool();
|
||||
}
|
||||
}
|
||||
}
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace nav2_controller
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleGoalChecker, nav2_core::GoalChecker)
|
||||
@@ -0,0 +1,119 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_controller/plugins/simple_progress_checker.hpp"
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include "nav2_core/exceptions.hpp"
|
||||
#include "nav_2d_utils/conversions.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "geometry_msgs/msg/pose2_d.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
using std::placeholders::_1;
|
||||
|
||||
namespace nav2_controller
|
||||
{
|
||||
void SimpleProgressChecker::initialize(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
const std::string & plugin_name)
|
||||
{
|
||||
plugin_name_ = plugin_name;
|
||||
auto node = parent.lock();
|
||||
|
||||
clock_ = node->get_clock();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0));
|
||||
// Scale is set to 0 by default, so if it was not set otherwise, set to 0
|
||||
node->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5);
|
||||
double time_allowance_param = 0.0;
|
||||
node->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0);
|
||||
time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param);
|
||||
|
||||
// Add callback for dynamic parameters
|
||||
dyn_params_handler_ = node->add_on_set_parameters_callback(
|
||||
std::bind(&SimpleProgressChecker::dynamicParametersCallback, this, _1));
|
||||
}
|
||||
|
||||
bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose)
|
||||
{
|
||||
// relies on short circuit evaluation to not call is_robot_moved_enough if
|
||||
// baseline_pose is not set.
|
||||
geometry_msgs::msg::Pose2D current_pose2d;
|
||||
current_pose2d = nav_2d_utils::poseToPose2D(current_pose.pose);
|
||||
|
||||
if ((!baseline_pose_set_) || (isRobotMovedEnough(current_pose2d))) {
|
||||
resetBaselinePose(current_pose2d);
|
||||
return true;
|
||||
}
|
||||
return !((clock_->now() - baseline_time_) > time_allowance_);
|
||||
}
|
||||
|
||||
void SimpleProgressChecker::reset()
|
||||
{
|
||||
baseline_pose_set_ = false;
|
||||
}
|
||||
|
||||
void SimpleProgressChecker::resetBaselinePose(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
baseline_pose_ = pose;
|
||||
baseline_time_ = clock_->now();
|
||||
baseline_pose_set_ = true;
|
||||
}
|
||||
|
||||
bool SimpleProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose)
|
||||
{
|
||||
return pose_distance(pose, baseline_pose_) > radius_;
|
||||
}
|
||||
|
||||
double SimpleProgressChecker::pose_distance(
|
||||
const geometry_msgs::msg::Pose2D & pose1,
|
||||
const geometry_msgs::msg::Pose2D & pose2)
|
||||
{
|
||||
double dx = pose1.x - pose2.x;
|
||||
double dy = pose1.y - pose2.y;
|
||||
|
||||
return std::hypot(dx, dy);
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
SimpleProgressChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
for (auto parameter : parameters) {
|
||||
const auto & type = parameter.get_type();
|
||||
const auto & name = parameter.get_name();
|
||||
|
||||
if (type == ParameterType::PARAMETER_DOUBLE) {
|
||||
if (name == plugin_name_ + ".required_movement_radius") {
|
||||
radius_ = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".movement_time_allowance") {
|
||||
time_allowance_ = rclcpp::Duration::from_seconds(parameter.as_double());
|
||||
}
|
||||
}
|
||||
}
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace nav2_controller
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleProgressChecker, nav2_core::ProgressChecker)
|
||||
@@ -0,0 +1,139 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
#include "nav2_controller/plugins/stopped_goal_checker.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
using std::hypot;
|
||||
using std::fabs;
|
||||
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
using std::placeholders::_1;
|
||||
|
||||
namespace nav2_controller
|
||||
{
|
||||
|
||||
StoppedGoalChecker::StoppedGoalChecker()
|
||||
: SimpleGoalChecker(), rot_stopped_velocity_(0.25), trans_stopped_velocity_(0.25)
|
||||
{
|
||||
}
|
||||
|
||||
void StoppedGoalChecker::initialize(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
const std::string & plugin_name,
|
||||
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
|
||||
{
|
||||
plugin_name_ = plugin_name;
|
||||
SimpleGoalChecker::initialize(parent, plugin_name, costmap_ros);
|
||||
|
||||
auto node = parent.lock();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
plugin_name + ".rot_stopped_velocity", rclcpp::ParameterValue(0.25));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node,
|
||||
plugin_name + ".trans_stopped_velocity", rclcpp::ParameterValue(0.25));
|
||||
|
||||
node->get_parameter(plugin_name + ".rot_stopped_velocity", rot_stopped_velocity_);
|
||||
node->get_parameter(plugin_name + ".trans_stopped_velocity", trans_stopped_velocity_);
|
||||
|
||||
// Add callback for dynamic parameters
|
||||
dyn_params_handler_ = node->add_on_set_parameters_callback(
|
||||
std::bind(&StoppedGoalChecker::dynamicParametersCallback, this, _1));
|
||||
}
|
||||
|
||||
bool StoppedGoalChecker::isGoalReached(
|
||||
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
|
||||
const geometry_msgs::msg::Twist & velocity)
|
||||
{
|
||||
bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity);
|
||||
if (!ret) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
return fabs(velocity.angular.z) <= rot_stopped_velocity_ &&
|
||||
hypot(velocity.linear.x, velocity.linear.y) <= trans_stopped_velocity_;
|
||||
}
|
||||
|
||||
bool StoppedGoalChecker::getTolerances(
|
||||
geometry_msgs::msg::Pose & pose_tolerance,
|
||||
geometry_msgs::msg::Twist & vel_tolerance)
|
||||
{
|
||||
double invalid_field = std::numeric_limits<double>::lowest();
|
||||
|
||||
// populate the poses
|
||||
bool rtn = SimpleGoalChecker::getTolerances(pose_tolerance, vel_tolerance);
|
||||
|
||||
// override the velocities
|
||||
vel_tolerance.linear.x = trans_stopped_velocity_;
|
||||
vel_tolerance.linear.y = trans_stopped_velocity_;
|
||||
vel_tolerance.linear.z = invalid_field;
|
||||
|
||||
vel_tolerance.angular.x = invalid_field;
|
||||
vel_tolerance.angular.y = invalid_field;
|
||||
vel_tolerance.angular.z = rot_stopped_velocity_;
|
||||
|
||||
return true && rtn;
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
StoppedGoalChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
for (auto parameter : parameters) {
|
||||
const auto & type = parameter.get_type();
|
||||
const auto & name = parameter.get_name();
|
||||
|
||||
if (type == ParameterType::PARAMETER_DOUBLE) {
|
||||
if (name == plugin_name_ + ".rot_stopped_velocity") {
|
||||
rot_stopped_velocity_ = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".trans_stopped_velocity") {
|
||||
trans_stopped_velocity_ = parameter.as_double();
|
||||
}
|
||||
}
|
||||
}
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace nav2_controller
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_controller::StoppedGoalChecker, nav2_core::GoalChecker)
|
||||
@@ -0,0 +1,4 @@
|
||||
ament_add_gtest(pctest progress_checker.cpp)
|
||||
target_link_libraries(pctest simple_progress_checker pose_progress_checker)
|
||||
ament_add_gtest(gctest goal_checker.cpp)
|
||||
target_link_libraries(gctest simple_goal_checker stopped_goal_checker)
|
||||
@@ -0,0 +1,245 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "nav2_controller/plugins/simple_goal_checker.hpp"
|
||||
#include "nav2_controller/plugins/stopped_goal_checker.hpp"
|
||||
#include "nav_2d_utils/conversions.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
|
||||
using nav2_controller::SimpleGoalChecker;
|
||||
using nav2_controller::StoppedGoalChecker;
|
||||
|
||||
void checkMacro(
|
||||
nav2_core::GoalChecker & gc,
|
||||
double x0, double y0, double theta0,
|
||||
double x1, double y1, double theta1,
|
||||
double xv, double yv, double thetav,
|
||||
bool expected_result)
|
||||
{
|
||||
gc.reset();
|
||||
geometry_msgs::msg::Pose2D pose0, pose1;
|
||||
pose0.x = x0;
|
||||
pose0.y = y0;
|
||||
pose0.theta = theta0;
|
||||
pose1.x = x1;
|
||||
pose1.y = y1;
|
||||
pose1.theta = theta1;
|
||||
nav_2d_msgs::msg::Twist2D v;
|
||||
v.x = xv;
|
||||
v.y = yv;
|
||||
v.theta = thetav;
|
||||
if (expected_result) {
|
||||
EXPECT_TRUE(
|
||||
gc.isGoalReached(
|
||||
nav_2d_utils::pose2DToPose(pose0),
|
||||
nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v)));
|
||||
} else {
|
||||
EXPECT_FALSE(
|
||||
gc.isGoalReached(
|
||||
nav_2d_utils::pose2DToPose(pose0),
|
||||
nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v)));
|
||||
}
|
||||
}
|
||||
|
||||
void sameResult(
|
||||
nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1,
|
||||
double x0, double y0, double theta0,
|
||||
double x1, double y1, double theta1,
|
||||
double xv, double yv, double thetav,
|
||||
bool expected_result)
|
||||
{
|
||||
checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
|
||||
checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
|
||||
}
|
||||
|
||||
void trueFalse(
|
||||
nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1,
|
||||
double x0, double y0, double theta0,
|
||||
double x1, double y1, double theta1,
|
||||
double xv, double yv, double thetav)
|
||||
{
|
||||
checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, true);
|
||||
checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, false);
|
||||
}
|
||||
class TestLifecycleNode : public nav2_util::LifecycleNode
|
||||
{
|
||||
public:
|
||||
explicit TestLifecycleNode(const std::string & name)
|
||||
: nav2_util::LifecycleNode(name)
|
||||
{
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &)
|
||||
{
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &)
|
||||
{
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
|
||||
{
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
|
||||
{
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &)
|
||||
{
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &)
|
||||
{
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
};
|
||||
|
||||
TEST(VelocityIterator, goal_checker_reset)
|
||||
{
|
||||
auto x = std::make_shared<TestLifecycleNode>("goal_checker");
|
||||
|
||||
nav2_core::GoalChecker * gc = new SimpleGoalChecker;
|
||||
gc->reset();
|
||||
delete gc;
|
||||
EXPECT_TRUE(true);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, stopped_goal_checker_reset)
|
||||
{
|
||||
auto x = std::make_shared<TestLifecycleNode>("stopped_goal_checker");
|
||||
|
||||
nav2_core::GoalChecker * sgc = new StoppedGoalChecker;
|
||||
sgc->reset();
|
||||
delete sgc;
|
||||
EXPECT_TRUE(true);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, two_checks)
|
||||
{
|
||||
auto x = std::make_shared<TestLifecycleNode>("goal_checker");
|
||||
|
||||
SimpleGoalChecker gc;
|
||||
StoppedGoalChecker sgc;
|
||||
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
|
||||
|
||||
gc.initialize(x, "nav2_controller", costmap);
|
||||
sgc.initialize(x, "nav2_controller", costmap);
|
||||
sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true);
|
||||
sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false);
|
||||
sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false);
|
||||
sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, false);
|
||||
sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, true);
|
||||
trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0);
|
||||
trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0);
|
||||
trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0);
|
||||
trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1);
|
||||
}
|
||||
|
||||
TEST(StoppedGoalChecker, get_tol_and_dynamic_params)
|
||||
{
|
||||
auto x = std::make_shared<TestLifecycleNode>("goal_checker");
|
||||
|
||||
SimpleGoalChecker gc;
|
||||
StoppedGoalChecker sgc;
|
||||
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
|
||||
|
||||
sgc.initialize(x, "test", costmap);
|
||||
gc.initialize(x, "test2", costmap);
|
||||
geometry_msgs::msg::Pose pose_tol;
|
||||
geometry_msgs::msg::Twist vel_tol;
|
||||
|
||||
// Test stopped goal checker's tolerance API
|
||||
EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol));
|
||||
EXPECT_EQ(vel_tol.linear.x, 0.25);
|
||||
EXPECT_EQ(vel_tol.linear.y, 0.25);
|
||||
EXPECT_EQ(vel_tol.angular.z, 0.25);
|
||||
|
||||
// Test Stopped goal checker's dynamic parameters
|
||||
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
|
||||
x->get_node_base_interface(), x->get_node_topics_interface(),
|
||||
x->get_node_graph_interface(),
|
||||
x->get_node_services_interface());
|
||||
|
||||
auto results = rec_param->set_parameters_atomically(
|
||||
{rclcpp::Parameter("test.rot_stopped_velocity", 100.0),
|
||||
rclcpp::Parameter("test.trans_stopped_velocity", 100.0)});
|
||||
|
||||
rclcpp::spin_until_future_complete(
|
||||
x->get_node_base_interface(),
|
||||
results);
|
||||
|
||||
EXPECT_EQ(x->get_parameter("test.rot_stopped_velocity").as_double(), 100.0);
|
||||
EXPECT_EQ(x->get_parameter("test.trans_stopped_velocity").as_double(), 100.0);
|
||||
|
||||
// Test normal goal checker's dynamic parameters
|
||||
results = rec_param->set_parameters_atomically(
|
||||
{rclcpp::Parameter("test2.xy_goal_tolerance", 200.0),
|
||||
rclcpp::Parameter("test2.yaw_goal_tolerance", 200.0),
|
||||
rclcpp::Parameter("test2.stateful", true)});
|
||||
|
||||
rclcpp::spin_until_future_complete(
|
||||
x->get_node_base_interface(),
|
||||
results);
|
||||
|
||||
EXPECT_EQ(x->get_parameter("test2.xy_goal_tolerance").as_double(), 200.0);
|
||||
EXPECT_EQ(x->get_parameter("test2.yaw_goal_tolerance").as_double(), 200.0);
|
||||
EXPECT_EQ(x->get_parameter("test2.stateful").as_bool(), true);
|
||||
|
||||
// Test the dynamic parameters impacted the tolerances
|
||||
EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol));
|
||||
EXPECT_EQ(vel_tol.linear.x, 100.0);
|
||||
EXPECT_EQ(vel_tol.linear.y, 100.0);
|
||||
EXPECT_EQ(vel_tol.angular.z, 100.0);
|
||||
|
||||
EXPECT_TRUE(gc.getTolerances(pose_tol, vel_tol));
|
||||
EXPECT_EQ(pose_tol.position.x, 200.0);
|
||||
EXPECT_EQ(pose_tol.position.y, 200.0);
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
@@ -0,0 +1,244 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "nav2_controller/plugins/simple_progress_checker.hpp"
|
||||
#include "nav2_controller/plugins/pose_progress_checker.hpp"
|
||||
#include "nav_2d_utils/conversions.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
using nav2_controller::SimpleProgressChecker;
|
||||
using nav2_controller::PoseProgressChecker;
|
||||
|
||||
class TestLifecycleNode : public nav2_util::LifecycleNode
|
||||
{
|
||||
public:
|
||||
explicit TestLifecycleNode(const std::string & name)
|
||||
: nav2_util::LifecycleNode(name)
|
||||
{
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &)
|
||||
{
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &)
|
||||
{
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
|
||||
{
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
|
||||
{
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &)
|
||||
{
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &)
|
||||
{
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
};
|
||||
|
||||
void checkMacro(
|
||||
nav2_core::ProgressChecker & pc,
|
||||
double x0, double y0, double theta0,
|
||||
double x1, double y1, double theta1,
|
||||
int delay,
|
||||
bool expected_result)
|
||||
{
|
||||
pc.reset();
|
||||
geometry_msgs::msg::PoseStamped pose0, pose1;
|
||||
pose0.pose.position.x = x0;
|
||||
pose0.pose.position.y = y0;
|
||||
pose0.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta0);
|
||||
pose1.pose.position.x = x1;
|
||||
pose1.pose.position.y = y1;
|
||||
pose1.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1);
|
||||
EXPECT_TRUE(pc.check(pose0));
|
||||
rclcpp::sleep_for(std::chrono::milliseconds(delay));
|
||||
if (expected_result) {
|
||||
EXPECT_TRUE(pc.check(pose1));
|
||||
} else {
|
||||
EXPECT_FALSE(pc.check(pose1));
|
||||
}
|
||||
}
|
||||
|
||||
TEST(SimpleProgressChecker, progress_checker_reset)
|
||||
{
|
||||
auto x = std::make_shared<TestLifecycleNode>("progress_checker");
|
||||
|
||||
nav2_core::ProgressChecker * pc = new SimpleProgressChecker;
|
||||
pc->reset();
|
||||
delete pc;
|
||||
EXPECT_TRUE(true);
|
||||
}
|
||||
|
||||
TEST(SimpleProgressChecker, unit_tests)
|
||||
{
|
||||
auto x = std::make_shared<TestLifecycleNode>("progress_checker");
|
||||
|
||||
SimpleProgressChecker pc;
|
||||
pc.initialize(x, "nav2_controller");
|
||||
|
||||
double time_allowance = 0.5;
|
||||
int half_time_allowance_ms = static_cast<int>(time_allowance * 0.5 * 1000);
|
||||
int twice_time_allowance_ms = static_cast<int>(time_allowance * 2.0 * 1000);
|
||||
|
||||
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
|
||||
x->get_node_base_interface(), x->get_node_topics_interface(),
|
||||
x->get_node_graph_interface(),
|
||||
x->get_node_services_interface());
|
||||
|
||||
auto results = rec_param->set_parameters_atomically(
|
||||
{rclcpp::Parameter("nav2_controller.movement_time_allowance", time_allowance)});
|
||||
|
||||
rclcpp::spin_until_future_complete(
|
||||
x->get_node_base_interface(),
|
||||
results);
|
||||
|
||||
EXPECT_EQ(
|
||||
x->get_parameter("nav2_controller.movement_time_allowance").as_double(),
|
||||
time_allowance);
|
||||
|
||||
// BELOW time allowance (set to time_allowance)
|
||||
// no movement
|
||||
checkMacro(pc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true);
|
||||
// translation below required_movement_radius (default 0.5)
|
||||
checkMacro(pc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true);
|
||||
checkMacro(pc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true);
|
||||
// translation above required_movement_radius (default 0.5)
|
||||
checkMacro(pc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true);
|
||||
checkMacro(pc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true);
|
||||
|
||||
// ABOVE time allowance (set to time_allowance)
|
||||
// no movement
|
||||
checkMacro(pc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false);
|
||||
// translation below required_movement_radius (default 0.5)
|
||||
checkMacro(pc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false);
|
||||
checkMacro(pc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false);
|
||||
// translation above required_movement_radius (default 0.5)
|
||||
checkMacro(pc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true);
|
||||
checkMacro(pc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true);
|
||||
}
|
||||
|
||||
TEST(PoseProgressChecker, pose_progress_checker_reset)
|
||||
{
|
||||
auto x = std::make_shared<TestLifecycleNode>("pose_progress_checker");
|
||||
|
||||
PoseProgressChecker * rpc = new PoseProgressChecker;
|
||||
rpc->reset();
|
||||
delete rpc;
|
||||
EXPECT_TRUE(true);
|
||||
}
|
||||
|
||||
TEST(PoseProgressChecker, unit_tests)
|
||||
{
|
||||
auto x = std::make_shared<TestLifecycleNode>("pose_progress_checker");
|
||||
|
||||
PoseProgressChecker rpc;
|
||||
rpc.initialize(x, "nav2_controller");
|
||||
|
||||
double time_allowance = 0.5;
|
||||
int half_time_allowance_ms = static_cast<int>(time_allowance * 0.5 * 1000);
|
||||
int twice_time_allowance_ms = static_cast<int>(time_allowance * 2.0 * 1000);
|
||||
|
||||
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
|
||||
x->get_node_base_interface(), x->get_node_topics_interface(),
|
||||
x->get_node_graph_interface(),
|
||||
x->get_node_services_interface());
|
||||
|
||||
auto results = rec_param->set_parameters_atomically(
|
||||
{rclcpp::Parameter("nav2_controller.movement_time_allowance", time_allowance)});
|
||||
|
||||
rclcpp::spin_until_future_complete(
|
||||
x->get_node_base_interface(),
|
||||
results);
|
||||
|
||||
EXPECT_EQ(
|
||||
x->get_parameter("nav2_controller.movement_time_allowance").as_double(),
|
||||
time_allowance);
|
||||
|
||||
// BELOW time allowance (set to time_allowance)
|
||||
// no movement
|
||||
checkMacro(rpc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true);
|
||||
// translation below required_movement_radius (default 0.5)
|
||||
checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true);
|
||||
checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true);
|
||||
// rotation below required_movement_angle (default 0.5)
|
||||
checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, half_time_allowance_ms, true);
|
||||
checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, half_time_allowance_ms, true);
|
||||
// translation above required_movement_radius (default 0.5)
|
||||
checkMacro(rpc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true);
|
||||
checkMacro(rpc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true);
|
||||
// rotation above required_movement_angle (default 0.5)
|
||||
checkMacro(rpc, 0, 0, 0, 0, 0, 1, half_time_allowance_ms, true);
|
||||
checkMacro(rpc, 0, 0, 0, 0, 0, -1, half_time_allowance_ms, true);
|
||||
|
||||
// ABOVE time allowance (set to time_allowance)
|
||||
// no movement
|
||||
checkMacro(rpc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false);
|
||||
// translation below required_movement_radius (default 0.5)
|
||||
checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false);
|
||||
checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false);
|
||||
// rotation below required_movement_angle (default 0.5)
|
||||
checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, twice_time_allowance_ms, false);
|
||||
checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, twice_time_allowance_ms, false);
|
||||
// translation above required_movement_radius (default 0.5)
|
||||
checkMacro(rpc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true);
|
||||
checkMacro(rpc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true);
|
||||
// rotation above required_movement_angle (default 0.5)
|
||||
checkMacro(rpc, 0, 0, 0, 0, 0, 1, twice_time_allowance_ms, true);
|
||||
checkMacro(rpc, 0, 0, 0, 0, 0, -1, twice_time_allowance_ms, true);
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
Reference in New Issue
Block a user