m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add humble-navigation2

Browse Source
This commit is contained in:
X-lanni
2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
navigation2/nav2_smoother/CMakeLists.txt
+119
View File
@@ -0,0 +1,119 @@
cmake_minimum_required(VERSION 3.5)
project(nav2_smoother)
find_package(ament_cmake REQUIRED)
find_package(nav2_core REQUIRED)
find_package(nav2_common REQUIRED)
find_package(angles REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(std_msgs REQUIRED)
find_package(nav2_util REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(nav_2d_utils REQUIRED)
find_package(nav_2d_msgs REQUIRED)
find_package(pluginlib REQUIRED)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
nav2_package()
include_directories(
include
)
set(executable_name smoother_server)
set(library_name ${executable_name}_core)
set(dependencies
angles
rclcpp
rclcpp_components
rclcpp_action
rclcpp_components
std_msgs
nav2_msgs
nav_2d_utils
nav_2d_msgs
nav2_util
nav2_core
pluginlib
)
# Main library
add_library(${library_name} SHARED
src/nav2_smoother.cpp
)
ament_target_dependencies(${library_name}
${dependencies}
)
# Main executable
add_executable(${executable_name}
src/main.cpp
)
ament_target_dependencies(${executable_name}
${dependencies}
)
target_link_libraries(${executable_name} ${library_name})
# Simple Smoother plugin
add_library(simple_smoother SHARED
src/simple_smoother.cpp
)
ament_target_dependencies(simple_smoother
${dependencies}
)
# Savitzky Golay Smoother plugin
add_library(savitzky_golay_smoother SHARED
src/savitzky_golay_smoother.cpp
)
ament_target_dependencies(savitzky_golay_smoother
${dependencies}
)
pluginlib_export_plugin_description_file(nav2_core plugins.xml)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
set(ament_cmake_copyright_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_gtest REQUIRED)
endif()
rclcpp_components_register_nodes(${library_name} "nav2_smoother::SmootherServer")
install(
TARGETS ${library_name} simple_smoother savitzky_golay_smoother
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(TARGETS ${executable_name}
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY include/
DESTINATION include/
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_gtest REQUIRED)
add_subdirectory(test)
endif()
ament_export_include_directories(include)
ament_export_libraries(${library_name} simple_smoother savitzky_golay_smoother)
ament_export_dependencies(${dependencies})
ament_package()
navigation2/nav2_smoother/README.md
+11
View File
@@ -0,0 +1,11 @@
# Nav2 Smoother
The Nav2 smoother is a Task Server in Nav2 that implements the `nav2_behavior_tree::SmoothPath` interface.
A smoothing module implementing the `nav2_behavior_tree::SmoothPath` interface is responsible for improving path smoothness and/or quality, typically given an unsmoothed path from the planner module in `nav2_planner`. It loads a map of potential smoother plugins to do the path smoothing in different user-defined situations.
See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available smoother plugins.
See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions.
This package contains the Simple Smoother and Savitzky-Golay Smoother plugins.
navigation2/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp
+165
View File
@@ -0,0 +1,165 @@
// Copyright (c) 2021 RoboTech Vision
// 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.
#ifndef NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_
#define NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_
#include <memory>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>
#include "nav2_core/smoother.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
#include "nav2_costmap_2d/footprint_subscriber.hpp"
#include "nav2_msgs/action/smooth_path.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/simple_action_server.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "pluginlib/class_loader.hpp"
namespace nav2_smoother
{
/**
* @class nav2_smoother::SmootherServer
* @brief This class hosts variety of plugins of different algorithms to
* smooth or refine a path from the exposed SmoothPath action server.
*/
class SmootherServer : public nav2_util::LifecycleNode
{
public:
using SmootherMap = std::unordered_map<std::string, nav2_core::Smoother::Ptr>;
/**
* @brief A constructor for nav2_smoother::SmootherServer
* @param options Additional options to control creation of the node.
*/
explicit SmootherServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
/**
* @brief Destructor for nav2_smoother::SmootherServer
*/
~SmootherServer();
protected:
/**
* @brief Configures smoother parameters and member variables
*
* Configures smoother plugin and costmap; Initialize odom subscriber,
* velocity publisher and smooth path action server.
* @param state LifeCycle Node's state
* @return Success or Failure
* @throw pluginlib::PluginlibException When failed to initialize smoother
* plugin
*/
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
/**
* @brief Loads smoother plugins from parameter file
* @return bool if successfully loaded the plugins
*/
bool loadSmootherPlugins();
/**
* @brief Activates member variables
*
* Activates smoother, costmap, velocity publisher and smooth path action
* server
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
/**
* @brief Deactivates member variables
*
* Deactivates smooth path action server, smoother, costmap and velocity
* publisher. Before calling deactivate state, velocity is being set to zero.
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
/**
* @brief Calls clean up states and resets member variables.
*
* Smoother and costmap clean up state is called, and resets rest of the
* variables
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
/**
* @brief Called when in Shutdown state
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
using Action = nav2_msgs::action::SmoothPath;
using ActionServer = nav2_util::SimpleActionServer<Action>;
/**
* @brief SmoothPath action server callback. Handles action server updates and
* spins server until goal is reached
*
* Provides global path to smoother received from action client. Local
* section of the path is optimized using smoother.
* @throw nav2_core::PlannerException
*/
void smoothPlan();
/**
* @brief Find the valid smoother ID name for the given request
*
* @param c_name The requested smoother name
* @param name Reference to the name to use for control if any valid available
* @return bool Whether it found a valid smoother to use
*/
bool findSmootherId(const std::string & c_name, std::string & name);
// Our action server implements the SmoothPath action
std::unique_ptr<ActionServer> action_server_;
// Transforms
std::shared_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
// Publishers and subscribers
rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
// Smoother Plugins
pluginlib::ClassLoader<nav2_core::Smoother> lp_loader_;
SmootherMap smoothers_;
std::vector<std::string> default_ids_;
std::vector<std::string> default_types_;
std::vector<std::string> smoother_ids_;
std::vector<std::string> smoother_types_;
std::string smoother_ids_concat_, current_smoother_;
// Utilities
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
};
} // namespace nav2_smoother
#endif // NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_
navigation2/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp
+108
View File
@@ -0,0 +1,108 @@
// Copyright (c) 2022, Samsung Research America
//
// 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. Reserved.
#ifndef NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_
#define NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_
#include <cmath>
#include <vector>
#include <string>
#include <iostream>
#include <memory>
#include <queue>
#include <utility>
#include "nav2_core/smoother.hpp"
#include "nav2_smoother/smoother_utils.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "angles/angles.h"
#include "tf2/utils.h"
namespace nav2_smoother
{
/**
* @class nav2_smoother::SavitzkyGolaySmoother
* @brief A path smoother implementation using Savitzky Golay filters
*/
class SavitzkyGolaySmoother : public nav2_core::Smoother
{
public:
/**
* @brief A constructor for nav2_smoother::SavitzkyGolaySmoother
*/
SavitzkyGolaySmoother() = default;
/**
* @brief A destructor for nav2_smoother::SavitzkyGolaySmoother
*/
~SavitzkyGolaySmoother() override = default;
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr &,
std::string name, std::shared_ptr<tf2_ros::Buffer>,
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>,
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>) override;
/**
* @brief Method to cleanup resources.
*/
void cleanup() override {}
/**
* @brief Method to activate smoother and any threads involved in execution.
*/
void activate() override {}
/**
* @brief Method to deactivate smoother and any threads involved in execution.
*/
void deactivate() override {}
/**
* @brief Method to smooth given path
*
* @param path In-out path to be smoothed
* @param max_time Maximum duration smoothing should take
* @return If smoothing was completed (true) or interrupted by time limit (false)
*/
bool smooth(
nav_msgs::msg::Path & path,
const rclcpp::Duration & max_time) override;
protected:
/**
* @brief Smoother method - does the smoothing on a segment
* @param path Reference to path
* @param reversing_segment Return if this is a reversing segment
* @param costmap Pointer to minimal costmap
* @param max_time Maximum time to compute, stop early if over limit
* @return If smoothing was successful
*/
bool smoothImpl(
nav_msgs::msg::Path & path,
bool & reversing_segment);
bool do_refinement_;
int refinement_num_;
rclcpp::Logger logger_{rclcpp::get_logger("SGSmoother")};
};
} // namespace nav2_smoother
#endif // NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_
navigation2/nav2_smoother/include/nav2_smoother/simple_smoother.hpp
+132
View File
@@ -0,0 +1,132 @@
// Copyright (c) 2022, Samsung Research America
//
// 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. Reserved.
#ifndef NAV2_SMOOTHER__SIMPLE_SMOOTHER_HPP_
#define NAV2_SMOOTHER__SIMPLE_SMOOTHER_HPP_
#include <cmath>
#include <vector>
#include <string>
#include <iostream>
#include <memory>
#include <queue>
#include <utility>
#include "nav2_core/smoother.hpp"
#include "nav2_smoother/smoother_utils.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "angles/angles.h"
#include "tf2/utils.h"
namespace nav2_smoother
{
/**
* @class nav2_smoother::SimpleSmoother
* @brief A path smoother implementation
*/
class SimpleSmoother : public nav2_core::Smoother
{
public:
/**
* @brief A constructor for nav2_smoother::SimpleSmoother
*/
SimpleSmoother() = default;
/**
* @brief A destructor for nav2_smoother::SimpleSmoother
*/
~SimpleSmoother() override = default;
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr &,
std::string name, std::shared_ptr<tf2_ros::Buffer>,
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>,
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>) override;
/**
* @brief Method to cleanup resources.
*/
void cleanup() override {costmap_sub_.reset();}
/**
* @brief Method to activate smoother and any threads involved in execution.
*/
void activate() override {}
/**
* @brief Method to deactivate smoother and any threads involved in execution.
*/
void deactivate() override {}
/**
* @brief Method to smooth given path
*
* @param path In-out path to be smoothed
* @param max_time Maximum duration smoothing should take
* @return If smoothing was completed (true) or interrupted by time limit (false)
*/
bool smooth(
nav_msgs::msg::Path & path,
const rclcpp::Duration & max_time) override;
protected:
/**
* @brief Smoother method - does the smoothing on a segment
* @param path Reference to path
* @param reversing_segment Return if this is a reversing segment
* @param costmap Pointer to minimal costmap
* @param max_time Maximum time to compute, stop early if over limit
* @return If smoothing was successful
*/
bool smoothImpl(
nav_msgs::msg::Path & path,
bool & reversing_segment,
const nav2_costmap_2d::Costmap2D * costmap,
const double & max_time);
/**
* @brief Get the field value for a given dimension
* @param msg Current pose to sample
* @param dim Dimension ID of interest
* @return dim value
*/
inline double getFieldByDim(
const geometry_msgs::msg::PoseStamped & msg,
const unsigned int & dim);
/**
* @brief Set the field value for a given dimension
* @param msg Current pose to sample
* @param dim Dimension ID of interest
* @param value to set the dimention to for the pose
*/
inline void setFieldByDim(
geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
const double & value);
double tolerance_, data_w_, smooth_w_;
int max_its_, refinement_ctr_;
bool do_refinement_;
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
rclcpp::Logger logger_{rclcpp::get_logger("SimpleSmoother")};
};
} // namespace nav2_smoother
#endif // NAV2_SMOOTHER__SIMPLE_SMOOTHER_HPP_
navigation2/nav2_smoother/include/nav2_smoother/smoother_utils.hpp
+132
View File
@@ -0,0 +1,132 @@
// Copyright (c) 2022, Samsung Research America
//
// 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. Reserved.
#ifndef NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_
#define NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_
#include <cmath>
#include <vector>
#include <string>
#include <iostream>
#include <memory>
#include <queue>
#include <utility>
#include "nav2_core/smoother.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "angles/angles.h"
#include "tf2/utils.h"
namespace smoother_utils
{
/**
* @class nav2_smoother::PathSegment
* @brief A segment of a path in start/end indices
*/
struct PathSegment
{
unsigned int start;
unsigned int end;
};
typedef std::vector<geometry_msgs::msg::PoseStamped>::iterator PathIterator;
typedef std::vector<geometry_msgs::msg::PoseStamped>::reverse_iterator ReversePathIterator;
inline std::vector<PathSegment> findDirectionalPathSegments(
const nav_msgs::msg::Path & path)
{
std::vector<PathSegment> segments;
PathSegment curr_segment;
curr_segment.start = 0;
// Iterating through the path to determine the position of the cusp
for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
// We have two vectors for the dot product OA and AB. Determining the vectors.
double oa_x = path.poses[idx].pose.position.x -
path.poses[idx - 1].pose.position.x;
double oa_y = path.poses[idx].pose.position.y -
path.poses[idx - 1].pose.position.y;
double ab_x = path.poses[idx + 1].pose.position.x -
path.poses[idx].pose.position.x;
double ab_y = path.poses[idx + 1].pose.position.y -
path.poses[idx].pose.position.y;
// Checking for the existance of cusp, in the path, using the dot product.
double dot_product = (oa_x * ab_x) + (oa_y * ab_y);
if (dot_product < 0.0) {
curr_segment.end = idx;
segments.push_back(curr_segment);
curr_segment.start = idx;
}
// Checking for the existance of a differential rotation in place.
double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation);
double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation);
double dtheta = angles::shortest_angular_distance(cur_theta, next_theta);
if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) {
curr_segment.end = idx;
segments.push_back(curr_segment);
curr_segment.start = idx;
}
}
curr_segment.end = path.poses.size() - 1;
segments.push_back(curr_segment);
return segments;
}
inline void updateApproximatePathOrientations(
nav_msgs::msg::Path & path,
bool & reversing_segment)
{
double dx, dy, theta, pt_yaw;
reversing_segment = false;
// Find if this path segment is in reverse
dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x;
dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y;
theta = atan2(dy, dx);
pt_yaw = tf2::getYaw(path.poses[1].pose.orientation);
if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) {
reversing_segment = true;
}
// Find the angle relative the path position vectors
for (unsigned int i = 0; i != path.poses.size() - 1; i++) {
dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x;
dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y;
theta = atan2(dy, dx);
// If points are overlapping, pass
if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) {
continue;
}
// Flip the angle if this path segment is in reverse
if (reversing_segment) {
theta += M_PI; // orientationAroundZAxis will normalize
}
path.poses[i].pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta);
}
}
} // namespace smoother_utils
#endif // NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_
navigation2/nav2_smoother/package.xml
+33
View File
@@ -0,0 +1,33 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_smoother</name>
<version>1.1.18</version>
<description>Smoother action interface</description>
<maintainer email="vargovcik@robotechvision.com">Matej Vargovcik</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>nav2_common</build_depend>
<depend>angles</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>rclcpp_action</depend>
<depend>std_msgs</depend>
<depend>nav2_util</depend>
<depend>nav2_msgs</depend>
<depend>nav_2d_utils</depend>
<depend>nav_2d_msgs</depend>
<depend>nav2_core</depend>
<depend>pluginlib</depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_pytest</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
navigation2/nav2_smoother/plugins.xml
+13
View File
@@ -0,0 +1,13 @@
<class_libraries>
<library path="simple_smoother">
<class type="nav2_smoother::SimpleSmoother" base_class_type="nav2_core::Smoother">
<description>Does a simple smoothing process with collision checking</description>
</class>
</library>
<library path="savitzky_golay_smoother">
<class type="nav2_smoother::SavitzkyGolaySmoother" base_class_type="nav2_core::Smoother">
<description>Does Savitzky-Golay smoothing</description>
</class>
</library>
</class_libraries>
navigation2/nav2_smoother/src/main.cpp
+29
View File
@@ -0,0 +1,29 @@
// Copyright (c) 2021 RoboTech Vision
// 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 <memory>
#include "nav2_smoother/nav2_smoother.hpp"
#include "rclcpp/rclcpp.hpp"
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<nav2_smoother::SmootherServer>();
rclcpp::spin(node->get_node_base_interface());
rclcpp::shutdown();
return 0;
}
navigation2/nav2_smoother/src/nav2_smoother.cpp
+338
View File
@@ -0,0 +1,338 @@
// Copyright (c) 2019 RoboTech Vision
// Copyright (c) 2019 Intel Corporation
// Copyright (c) 2022 Samsung Research America
//
// 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 <chrono>
#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "nav2_core/exceptions.hpp"
#include "nav2_smoother/nav2_smoother.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "nav_2d_utils/tf_help.hpp"
#include "tf2_ros/create_timer_ros.h"
using namespace std::chrono_literals;
namespace nav2_smoother
{
SmootherServer::SmootherServer(const rclcpp::NodeOptions & options)
: LifecycleNode("smoother_server", "", options),
lp_loader_("nav2_core", "nav2_core::Smoother"),
default_ids_{"simple_smoother"},
default_types_{"nav2_smoother::SimpleSmoother"}
{
RCLCPP_INFO(get_logger(), "Creating smoother server");
declare_parameter(
"costmap_topic", rclcpp::ParameterValue(
std::string(
"global_costmap/costmap_raw")));
declare_parameter(
"footprint_topic",
rclcpp::ParameterValue(
std::string("global_costmap/published_footprint")));
declare_parameter(
"robot_base_frame",
rclcpp::ParameterValue(std::string("base_link")));
declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1));
declare_parameter("smoother_plugins", default_ids_);
}
SmootherServer::~SmootherServer()
{
smoothers_.clear();
}
nav2_util::CallbackReturn
SmootherServer::on_configure(const rclcpp_lifecycle::State &)
{
RCLCPP_INFO(get_logger(), "Configuring smoother server");
auto node = shared_from_this();
get_parameter("smoother_plugins", smoother_ids_);
if (smoother_ids_ == default_ids_) {
for (size_t i = 0; i < default_ids_.size(); ++i) {
nav2_util::declare_parameter_if_not_declared(
node, default_ids_[i] + ".plugin",
rclcpp::ParameterValue(default_types_[i]));
}
}
tf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
get_node_base_interface(), get_node_timers_interface());
tf_->setCreateTimerInterface(timer_interface);
transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
std::string costmap_topic, footprint_topic, robot_base_frame;
double transform_tolerance;
this->get_parameter("costmap_topic", costmap_topic);
this->get_parameter("footprint_topic", footprint_topic);
this->get_parameter("transform_tolerance", transform_tolerance);
this->get_parameter("robot_base_frame", robot_base_frame);
costmap_sub_ = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(
shared_from_this(), costmap_topic);
footprint_sub_ = std::make_shared<nav2_costmap_2d::FootprintSubscriber>(
shared_from_this(), footprint_topic, *tf_, robot_base_frame, transform_tolerance);
collision_checker_ =
std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
*costmap_sub_, *footprint_sub_, this->get_name());
if (!loadSmootherPlugins()) {
return nav2_util::CallbackReturn::FAILURE;
}
// Initialize pubs & subs
plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan_smoothed", 1);
// Create the action server that we implement with our smoothPath method
action_server_ = std::make_unique<ActionServer>(
shared_from_this(),
"smooth_path",
std::bind(&SmootherServer::smoothPlan, this),
nullptr,
std::chrono::milliseconds(500),
true);
return nav2_util::CallbackReturn::SUCCESS;
}
bool SmootherServer::loadSmootherPlugins()
{
auto node = shared_from_this();
smoother_types_.resize(smoother_ids_.size());
for (size_t i = 0; i != smoother_ids_.size(); i++) {
try {
smoother_types_[i] =
nav2_util::get_plugin_type_param(node, smoother_ids_[i]);
nav2_core::Smoother::Ptr smoother =
lp_loader_.createUniqueInstance(smoother_types_[i]);
RCLCPP_INFO(
get_logger(), "Created smoother : %s of type %s",
smoother_ids_[i].c_str(), smoother_types_[i].c_str());
smoother->configure(
node, smoother_ids_[i], tf_, costmap_sub_,
footprint_sub_);
smoothers_.insert({smoother_ids_[i], smoother});
} catch (const pluginlib::PluginlibException & ex) {
RCLCPP_FATAL(
get_logger(), "Failed to create smoother. Exception: %s",
ex.what());
return false;
}
}
for (size_t i = 0; i != smoother_ids_.size(); i++) {
smoother_ids_concat_ += smoother_ids_[i] + std::string(" ");
}
RCLCPP_INFO(
get_logger(), "Smoother Server has %s smoothers available.",
smoother_ids_concat_.c_str());
return true;
}
nav2_util::CallbackReturn
SmootherServer::on_activate(const rclcpp_lifecycle::State &)
{
RCLCPP_INFO(get_logger(), "Activating");
plan_publisher_->on_activate();
SmootherMap::iterator it;
for (it = smoothers_.begin(); it != smoothers_.end(); ++it) {
it->second->activate();
}
action_server_->activate();
// create bond connection
createBond();
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
SmootherServer::on_deactivate(const rclcpp_lifecycle::State &)
{
RCLCPP_INFO(get_logger(), "Deactivating");
action_server_->deactivate();
SmootherMap::iterator it;
for (it = smoothers_.begin(); it != smoothers_.end(); ++it) {
it->second->deactivate();
}
plan_publisher_->on_deactivate();
// destroy bond connection
destroyBond();
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
SmootherServer::on_cleanup(const rclcpp_lifecycle::State &)
{
RCLCPP_INFO(get_logger(), "Cleaning up");
// Cleanup the helper classes
SmootherMap::iterator it;
for (it = smoothers_.begin(); it != smoothers_.end(); ++it) {
it->second->cleanup();
}
smoothers_.clear();
// Release any allocated resources
action_server_.reset();
plan_publisher_.reset();
transform_listener_.reset();
tf_.reset();
footprint_sub_.reset();
costmap_sub_.reset();
collision_checker_.reset();
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
SmootherServer::on_shutdown(const rclcpp_lifecycle::State &)
{
RCLCPP_INFO(get_logger(), "Shutting down");
return nav2_util::CallbackReturn::SUCCESS;
}
bool SmootherServer::findSmootherId(
const std::string & c_name,
std::string & current_smoother)
{
if (smoothers_.find(c_name) == smoothers_.end()) {
if (smoothers_.size() == 1 && c_name.empty()) {
RCLCPP_WARN_ONCE(
get_logger(),
"No smoother was specified in action call."
" Server will use only plugin loaded %s. "
"This warning will appear once.",
smoother_ids_concat_.c_str());
current_smoother = smoothers_.begin()->first;
} else {
RCLCPP_ERROR(
get_logger(),
"SmoothPath called with smoother name %s, "
"which does not exist. Available smoothers are: %s.",
c_name.c_str(), smoother_ids_concat_.c_str());
return false;
}
} else {
RCLCPP_DEBUG(get_logger(), "Selected smoother: %s.", c_name.c_str());
current_smoother = c_name;
}
return true;
}
void SmootherServer::smoothPlan()
{
auto start_time = this->now();
RCLCPP_INFO(get_logger(), "Received a path to smooth.");
auto result = std::make_shared<Action::Result>();
try {
auto goal = action_server_->get_current_goal();
if (!goal) {
return; // if action_server_ is inactivate, goal would be a nullptr
}
std::string c_name = goal->smoother_id;
std::string current_smoother;
if (findSmootherId(c_name, current_smoother)) {
current_smoother_ = current_smoother;
} else {
action_server_->terminate_current();
return;
}
// Perform smoothing
result->path = goal->path;
result->was_completed = smoothers_[current_smoother_]->smooth(
result->path, goal->max_smoothing_duration);
result->smoothing_duration = this->now() - start_time;
if (!result->was_completed) {
RCLCPP_INFO(
get_logger(),
"Smoother %s did not complete smoothing in specified time limit"
"(%lf seconds) and was interrupted after %lf seconds",
current_smoother_.c_str(),
rclcpp::Duration(goal->max_smoothing_duration).seconds(),
rclcpp::Duration(result->smoothing_duration).seconds());
}
plan_publisher_->publish(result->path);
// Check for collisions
if (goal->check_for_collisions) {
geometry_msgs::msg::Pose2D pose2d;
bool fetch_data = true;
for (const auto & pose : result->path.poses) {
pose2d.x = pose.pose.position.x;
pose2d.y = pose.pose.position.y;
pose2d.theta = tf2::getYaw(pose.pose.orientation);
if (!collision_checker_->isCollisionFree(pose2d, fetch_data)) {
RCLCPP_ERROR(
get_logger(),
"Smoothed path leads to a collision at x: %lf, y: %lf, theta: %lf",
pose2d.x, pose2d.y, pose2d.theta);
action_server_->terminate_current(result);
return;
}
fetch_data = false;
}
}
RCLCPP_DEBUG(
get_logger(), "Smoother succeeded (time: %lf), setting result",
rclcpp::Duration(result->smoothing_duration).seconds());
action_server_->succeeded_current(result);
} catch (nav2_core::PlannerException & e) {
RCLCPP_ERROR(this->get_logger(), "%s", e.what());
action_server_->terminate_current();
return;
} catch (std::exception & ex) {
RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
action_server_->terminate_current(result);
return;
}
}
} // namespace nav2_smoother
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(nav2_smoother::SmootherServer)
navigation2/nav2_smoother/src/savitzky_golay_smoother.cpp
+198
View File
@@ -0,0 +1,198 @@
// Copyright (c) 2022, Samsung Research America
//
// 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. Reserved.
#include <vector>
#include <memory>
#include "nav2_smoother/savitzky_golay_smoother.hpp"
namespace nav2_smoother
{
using namespace smoother_utils; // NOLINT
using namespace nav2_util::geometry_utils; // NOLINT
using namespace std::chrono; // NOLINT
using nav2_util::declare_parameter_if_not_declared;
void SavitzkyGolaySmoother::configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>/*costmap_sub*/,
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>/*footprint_sub*/)
{
auto node = parent.lock();
logger_ = node->get_logger();
declare_parameter_if_not_declared(
node, name + ".do_refinement", rclcpp::ParameterValue(true));
declare_parameter_if_not_declared(
node, name + ".refinement_num", rclcpp::ParameterValue(2));
node->get_parameter(name + ".do_refinement", do_refinement_);
node->get_parameter(name + ".refinement_num", refinement_num_);
}
bool SavitzkyGolaySmoother::smooth(
nav_msgs::msg::Path & path,
const rclcpp::Duration & max_time)
{
steady_clock::time_point start = steady_clock::now();
double time_remaining = max_time.seconds();
bool success = true, reversing_segment;
nav_msgs::msg::Path curr_path_segment;
curr_path_segment.header = path.header;
std::vector<PathSegment> path_segments = findDirectionalPathSegments(path);
for (unsigned int i = 0; i != path_segments.size(); i++) {
if (path_segments[i].end - path_segments[i].start > 9) {
// Populate path segment
curr_path_segment.poses.clear();
std::copy(
path.poses.begin() + path_segments[i].start,
path.poses.begin() + path_segments[i].end + 1,
std::back_inserter(curr_path_segment.poses));
// Make sure we're still able to smooth with time remaining
steady_clock::time_point now = steady_clock::now();
time_remaining = max_time.seconds() - duration_cast<duration<double>>(now - start).count();
if (time_remaining <= 0.0) {
RCLCPP_WARN(
logger_,
"Smoothing time exceeded allowed duration of %0.2f.", max_time.seconds());
return false;
}
// Smooth path segment
success = success && smoothImpl(curr_path_segment, reversing_segment);
// Assemble the path changes to the main path
std::copy(
curr_path_segment.poses.begin(),
curr_path_segment.poses.end(),
path.poses.begin() + path_segments[i].start);
}
}
return success;
}
bool SavitzkyGolaySmoother::smoothImpl(
nav_msgs::msg::Path & path,
bool & reversing_segment)
{
// Must be at least 10 in length to enter function
const unsigned int & path_size = path.poses.size();
// 7-point SG filter
const std::array<double, 7> filter = {
-2.0 / 21.0,
3.0 / 21.0,
6.0 / 21.0,
7.0 / 21.0,
6.0 / 21.0,
3.0 / 21.0,
-2.0 / 21.0};
auto applyFilter = [&](const std::vector<geometry_msgs::msg::Point> & data)
-> geometry_msgs::msg::Point
{
geometry_msgs::msg::Point val;
for (unsigned int i = 0; i != filter.size(); i++) {
val.x += filter[i] * data[i].x;
val.y += filter[i] * data[i].y;
}
return val;
};
auto applyFilterOverAxes =
[&](std::vector<geometry_msgs::msg::PoseStamped> & plan_pts) -> void
{
// Handle initial boundary conditions, first point is fixed
unsigned int idx = 1;
plan_pts[idx].pose.position = applyFilter(
{
plan_pts[idx - 1].pose.position,
plan_pts[idx - 1].pose.position,
plan_pts[idx - 1].pose.position,
plan_pts[idx].pose.position,
plan_pts[idx + 1].pose.position,
plan_pts[idx + 2].pose.position,
plan_pts[idx + 3].pose.position});
idx++;
plan_pts[idx].pose.position = applyFilter(
{
plan_pts[idx - 2].pose.position,
plan_pts[idx - 2].pose.position,
plan_pts[idx - 1].pose.position,
plan_pts[idx].pose.position,
plan_pts[idx + 1].pose.position,
plan_pts[idx + 2].pose.position,
plan_pts[idx + 3].pose.position});
// Apply nominal filter
for (idx = 3; idx < path_size - 4; ++idx) {
plan_pts[idx].pose.position = applyFilter(
{
plan_pts[idx - 3].pose.position,
plan_pts[idx - 2].pose.position,
plan_pts[idx - 1].pose.position,
plan_pts[idx].pose.position,
plan_pts[idx + 1].pose.position,
plan_pts[idx + 2].pose.position,
plan_pts[idx + 3].pose.position});
}
// Handle terminal boundary conditions, last point is fixed
idx++;
plan_pts[idx].pose.position = applyFilter(
{
plan_pts[idx - 3].pose.position,
plan_pts[idx - 2].pose.position,
plan_pts[idx - 1].pose.position,
plan_pts[idx].pose.position,
plan_pts[idx + 1].pose.position,
plan_pts[idx + 2].pose.position,
plan_pts[idx + 2].pose.position});
idx++;
plan_pts[idx].pose.position = applyFilter(
{
plan_pts[idx - 3].pose.position,
plan_pts[idx - 2].pose.position,
plan_pts[idx - 1].pose.position,
plan_pts[idx].pose.position,
plan_pts[idx + 1].pose.position,
plan_pts[idx + 1].pose.position,
plan_pts[idx + 1].pose.position});
};
applyFilterOverAxes(path.poses);
// Lets do additional refinement, it shouldn't take more than a couple milliseconds
if (do_refinement_) {
for (int i = 0; i < refinement_num_; i++) {
applyFilterOverAxes(path.poses);
}
}
updateApproximatePathOrientations(path, reversing_segment);
return true;
}
} // namespace nav2_smoother
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_smoother::SavitzkyGolaySmoother, nav2_core::Smoother)
navigation2/nav2_smoother/src/simple_smoother.cpp
+221
View File
@@ -0,0 +1,221 @@
// Copyright (c) 2022, Samsung Research America
//
// 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. Reserved.
#include <vector>
#include <memory>
#include "nav2_smoother/simple_smoother.hpp"
namespace nav2_smoother
{
using namespace smoother_utils; // NOLINT
using namespace nav2_util::geometry_utils; // NOLINT
using namespace std::chrono; // NOLINT
using nav2_util::declare_parameter_if_not_declared;
void SimpleSmoother::configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub,
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>/*footprint_sub*/)
{
costmap_sub_ = costmap_sub;
auto node = parent.lock();
logger_ = node->get_logger();
declare_parameter_if_not_declared(
node, name + ".tolerance", rclcpp::ParameterValue(1e-10));
declare_parameter_if_not_declared(
node, name + ".max_its", rclcpp::ParameterValue(1000));
declare_parameter_if_not_declared(
node, name + ".w_data", rclcpp::ParameterValue(0.2));
declare_parameter_if_not_declared(
node, name + ".w_smooth", rclcpp::ParameterValue(0.3));
declare_parameter_if_not_declared(
node, name + ".do_refinement", rclcpp::ParameterValue(true));
node->get_parameter(name + ".tolerance", tolerance_);
node->get_parameter(name + ".max_its", max_its_);
node->get_parameter(name + ".w_data", data_w_);
node->get_parameter(name + ".w_smooth", smooth_w_);
node->get_parameter(name + ".do_refinement", do_refinement_);
}
bool SimpleSmoother::smooth(
nav_msgs::msg::Path & path,
const rclcpp::Duration & max_time)
{
auto costmap = costmap_sub_->getCostmap();
refinement_ctr_ = 0;
steady_clock::time_point start = steady_clock::now();
double time_remaining = max_time.seconds();
bool success = true, reversing_segment;
nav_msgs::msg::Path curr_path_segment;
curr_path_segment.header = path.header;
std::vector<PathSegment> path_segments = findDirectionalPathSegments(path);
for (unsigned int i = 0; i != path_segments.size(); i++) {
if (path_segments[i].end - path_segments[i].start > 9) {
// Populate path segment
curr_path_segment.poses.clear();
std::copy(
path.poses.begin() + path_segments[i].start,
path.poses.begin() + path_segments[i].end + 1,
std::back_inserter(curr_path_segment.poses));
// Make sure we're still able to smooth with time remaining
steady_clock::time_point now = steady_clock::now();
time_remaining = max_time.seconds() - duration_cast<duration<double>>(now - start).count();
// Smooth path segment naively
success = success && smoothImpl(
curr_path_segment, reversing_segment, costmap.get(), time_remaining);
// Assemble the path changes to the main path
std::copy(
curr_path_segment.poses.begin(),
curr_path_segment.poses.end(),
path.poses.begin() + path_segments[i].start);
}
}
return success;
}
bool SimpleSmoother::smoothImpl(
nav_msgs::msg::Path & path,
bool & reversing_segment,
const nav2_costmap_2d::Costmap2D * costmap,
const double & max_time)
{
steady_clock::time_point a = steady_clock::now();
rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(max_time);
int its = 0;
double change = tolerance_;
const unsigned int & path_size = path.poses.size();
double x_i, y_i, y_m1, y_ip1, y_i_org;
unsigned int mx, my;
nav_msgs::msg::Path new_path = path;
nav_msgs::msg::Path last_path = path;
while (change >= tolerance_) {
its += 1;
change = 0.0;
// Make sure the smoothing function will converge
if (its >= max_its_) {
RCLCPP_WARN(
logger_,
"Number of iterations has exceeded limit of %i.", max_its_);
path = last_path;
updateApproximatePathOrientations(path, reversing_segment);
return false;
}
// Make sure still have time left to process
steady_clock::time_point b = steady_clock::now();
rclcpp::Duration timespan(duration_cast<duration<double>>(b - a));
if (timespan > max_dur) {
RCLCPP_WARN(
logger_,
"Smoothing time exceeded allowed duration of %0.2f.", max_time);
path = last_path;
updateApproximatePathOrientations(path, reversing_segment);
return false;
}
for (unsigned int i = 1; i != path_size - 1; i++) {
for (unsigned int j = 0; j != 2; j++) {
x_i = getFieldByDim(path.poses[i], j);
y_i = getFieldByDim(new_path.poses[i], j);
y_m1 = getFieldByDim(new_path.poses[i - 1], j);
y_ip1 = getFieldByDim(new_path.poses[i + 1], j);
y_i_org = y_i;
// Smooth based on local 3 point neighborhood and original data locations
y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_ip1 + y_m1 - (2.0 * y_i));
setFieldByDim(new_path.poses[i], j, y_i);
change += abs(y_i - y_i_org);
}
// validate update is admissible, only checks cost if a valid costmap pointer is provided
float cost = 0.0;
if (costmap) {
costmap->worldToMap(
getFieldByDim(new_path.poses[i], 0),
getFieldByDim(new_path.poses[i], 1),
mx, my);
cost = static_cast<float>(costmap->getCost(mx, my));
}
if (cost > nav2_costmap_2d::MAX_NON_OBSTACLE && cost != nav2_costmap_2d::NO_INFORMATION) {
RCLCPP_DEBUG(
rclcpp::get_logger("SmacPlannerSmoother"),
"Smoothing process resulted in an infeasible collision. "
"Returning the last path before the infeasibility was introduced.");
path = last_path;
updateApproximatePathOrientations(path, reversing_segment);
return false;
}
}
last_path = new_path;
}
// Lets do additional refinement, it shouldn't take more than a couple milliseconds
// but really puts the path quality over the top.
if (do_refinement_ && refinement_ctr_ < 4) {
refinement_ctr_++;
smoothImpl(new_path, reversing_segment, costmap, max_time);
}
updateApproximatePathOrientations(new_path, reversing_segment);
path = new_path;
return true;
}
double SimpleSmoother::getFieldByDim(
const geometry_msgs::msg::PoseStamped & msg, const unsigned int & dim)
{
if (dim == 0) {
return msg.pose.position.x;
} else if (dim == 1) {
return msg.pose.position.y;
} else {
return msg.pose.position.z;
}
}
void SimpleSmoother::setFieldByDim(
geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
const double & value)
{
if (dim == 0) {
msg.pose.position.x = value;
} else if (dim == 1) {
msg.pose.position.y = value;
} else {
msg.pose.position.z = value;
}
}
} // namespace nav2_smoother
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_smoother::SimpleSmoother, nav2_core::Smoother)
navigation2/nav2_smoother/test/CMakeLists.txt
+36
View File
@@ -0,0 +1,36 @@
ament_add_gtest(test_smoother_server
test_smoother_server.cpp
)
target_link_libraries(test_smoother_server
${library_name}
)
ament_target_dependencies(test_smoother_server
${dependencies}
)
ament_add_gtest(test_simple_smoother
test_simple_smoother.cpp
)
target_link_libraries(test_simple_smoother
simple_smoother
)
ament_target_dependencies(test_simple_smoother
${dependencies}
)
ament_add_gtest(test_savitzky_golay_smoother
test_savitzky_golay_smoother.cpp
)
target_link_libraries(test_savitzky_golay_smoother
savitzky_golay_smoother
)
ament_target_dependencies(test_savitzky_golay_smoother
${dependencies}
)
navigation2/nav2_smoother/test/test_savitzky_golay_smoother.cpp
+331
View File
@@ -0,0 +1,331 @@
// Copyright (c) 2022, Samsung Research America
//
// 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. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include <chrono>
#include <limits>
#include <random>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_msgs/msg/costmap.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_smoother/savitzky_golay_smoother.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
using namespace smoother_utils; // NOLINT
using namespace nav2_smoother; // NOLINT
using namespace std::chrono_literals; // NOLINT
class RclCppFixture
{
public:
RclCppFixture() {rclcpp::init(0, nullptr);}
~RclCppFixture() {rclcpp::shutdown();}
};
RclCppFixture g_rclcppfixture;
TEST(SmootherTest, test_sg_smoother_basics)
{
rclcpp_lifecycle::LifecycleNode::SharedPtr node =
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSGSmootherTest");
std::shared_ptr<nav2_msgs::msg::Costmap> costmap_msg =
std::make_shared<nav2_msgs::msg::Costmap>();
costmap_msg->header.stamp = node->now();
costmap_msg->header.frame_id = "map";
costmap_msg->data.resize(100 * 100);
costmap_msg->metadata.resolution = 0.05;
costmap_msg->metadata.size_x = 100;
costmap_msg->metadata.size_y = 100;
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> parent = node;
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> dummy_costmap;
dummy_costmap = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(parent, "dummy_topic");
dummy_costmap->costmapCallback(costmap_msg);
// Make smoother
std::shared_ptr<tf2_ros::Buffer> dummy_tf;
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> dummy_footprint;
node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false));
auto smoother = std::make_unique<nav2_smoother::SavitzkyGolaySmoother>();
smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint);
smoother->activate();
rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds
// Test regular path, should see no effective change
nav_msgs::msg::Path straight_regular_path, straight_regular_path_baseline;
straight_regular_path.header.frame_id = "map";
straight_regular_path.header.stamp = node->now();
straight_regular_path.poses.resize(11);
straight_regular_path.poses[0].pose.position.x = 0.5;
straight_regular_path.poses[0].pose.position.y = 0.1;
straight_regular_path.poses[1].pose.position.x = 0.5;
straight_regular_path.poses[1].pose.position.y = 0.2;
straight_regular_path.poses[2].pose.position.x = 0.5;
straight_regular_path.poses[2].pose.position.y = 0.3;
straight_regular_path.poses[3].pose.position.x = 0.5;
straight_regular_path.poses[3].pose.position.y = 0.4;
straight_regular_path.poses[4].pose.position.x = 0.5;
straight_regular_path.poses[4].pose.position.y = 0.5;
straight_regular_path.poses[5].pose.position.x = 0.5;
straight_regular_path.poses[5].pose.position.y = 0.6;
straight_regular_path.poses[6].pose.position.x = 0.5;
straight_regular_path.poses[6].pose.position.y = 0.7;
straight_regular_path.poses[7].pose.position.x = 0.5;
straight_regular_path.poses[7].pose.position.y = 0.8;
straight_regular_path.poses[8].pose.position.x = 0.5;
straight_regular_path.poses[8].pose.position.y = 0.9;
straight_regular_path.poses[9].pose.position.x = 0.5;
straight_regular_path.poses[9].pose.position.y = 1.0;
straight_regular_path.poses[10].pose.position.x = 0.5;
straight_regular_path.poses[10].pose.position.y = 1.1;
straight_regular_path_baseline = straight_regular_path;
EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time));
for (uint i = 0; i != straight_regular_path.poses.size() - 1; i++) {
// Check distances are still the same
EXPECT_NEAR(
fabs(
straight_regular_path.poses[i].pose.position.y -
straight_regular_path_baseline.poses[i].pose.position.y), 0.0, 0.011);
}
// Attempt smoothing with no time given, should fail
rclcpp::Duration no_time = rclcpp::Duration::from_seconds(-1.0); // 0 seconds
EXPECT_FALSE(smoother->smooth(straight_regular_path, no_time));
smoother->deactivate();
smoother->cleanup();
}
TEST(SmootherTest, test_sg_smoother_noisey_path)
{
rclcpp_lifecycle::LifecycleNode::SharedPtr node =
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSGSmootherTest");
std::shared_ptr<nav2_msgs::msg::Costmap> costmap_msg =
std::make_shared<nav2_msgs::msg::Costmap>();
costmap_msg->header.stamp = node->now();
costmap_msg->header.frame_id = "map";
costmap_msg->data.resize(100 * 100);
costmap_msg->metadata.resolution = 0.05;
costmap_msg->metadata.size_x = 100;
costmap_msg->metadata.size_y = 100;
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> parent = node;
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> dummy_costmap;
dummy_costmap = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(parent, "dummy_topic");
dummy_costmap->costmapCallback(costmap_msg);
// Make smoother
std::shared_ptr<tf2_ros::Buffer> dummy_tf;
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> dummy_footprint;
node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false));
auto smoother = std::make_unique<nav2_smoother::SavitzkyGolaySmoother>();
smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint);
rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds
// Given nominal irregular/noisey path, test that the output is shorter and smoother
nav_msgs::msg::Path noisey_path, noisey_path_baseline;
noisey_path.header.frame_id = "map";
noisey_path.header.stamp = node->now();
noisey_path.poses.resize(11);
noisey_path.poses[0].pose.position.x = 0.5;
noisey_path.poses[0].pose.position.y = 0.1;
noisey_path.poses[1].pose.position.x = 0.5;
noisey_path.poses[1].pose.position.y = 0.2;
noisey_path.poses[2].pose.position.x = 0.5;
noisey_path.poses[2].pose.position.y = 0.3;
noisey_path.poses[3].pose.position.x = 0.5;
noisey_path.poses[3].pose.position.y = 0.4;
noisey_path.poses[4].pose.position.x = 0.5;
noisey_path.poses[4].pose.position.y = 0.5;
noisey_path.poses[5].pose.position.x = 0.5;
noisey_path.poses[5].pose.position.y = 0.6;
noisey_path.poses[6].pose.position.x = 0.5;
noisey_path.poses[6].pose.position.y = 0.7;
noisey_path.poses[7].pose.position.x = 0.5;
noisey_path.poses[7].pose.position.y = 0.8;
noisey_path.poses[8].pose.position.x = 0.5;
noisey_path.poses[8].pose.position.y = 0.9;
noisey_path.poses[9].pose.position.x = 0.5;
noisey_path.poses[9].pose.position.y = 1.0;
noisey_path.poses[10].pose.position.x = 0.5;
noisey_path.poses[10].pose.position.y = 1.1;
// Add random but deterministic noises
std::random_device rd{};
std::mt19937 gen{rd()};
std::normal_distribution<> normal_distribution{0.0, 0.02};
for (unsigned int i = 0; i != noisey_path.poses.size(); i++) {
auto noise = normal_distribution(gen);
noisey_path.poses[i].pose.position.x += noise;
}
noisey_path_baseline = noisey_path;
EXPECT_TRUE(smoother->smooth(noisey_path, max_time));
// Compute metric, should be shorter if smoother
double length = 0;
double base_length = 0;
for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) {
length += std::hypot(
noisey_path.poses[i + 1].pose.position.x - noisey_path.poses[i].pose.position.x,
noisey_path.poses[i + 1].pose.position.y - noisey_path.poses[i].pose.position.y);
base_length += std::hypot(
noisey_path_baseline.poses[i + 1].pose.position.x -
noisey_path_baseline.poses[i].pose.position.x,
noisey_path_baseline.poses[i + 1].pose.position.y -
noisey_path_baseline.poses[i].pose.position.y);
}
EXPECT_LT(length, base_length);
// Test again with refinement, even shorter and smoother
node->set_parameter(rclcpp::Parameter("test.do_refinement", rclcpp::ParameterValue(true)));
smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint);
nav_msgs::msg::Path noisey_path_refined = noisey_path_baseline;
EXPECT_TRUE(smoother->smooth(noisey_path_refined, max_time));
length = 0;
for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) {
length += std::hypot(
noisey_path_refined.poses[i + 1].pose.position.x -
noisey_path_refined.poses[i].pose.position.x,
noisey_path_refined.poses[i + 1].pose.position.y -
noisey_path_refined.poses[i].pose.position.y);
// std::hypot(
// noisey_path.poses[i + 1].pose.position.x - noisey_path_baseline.poses[i].pose.position.x,
// noisey_path.poses[i + 1].pose.position.y - noisey_path_baseline.poses[i].pose.position.y);
}
EXPECT_LT(length, base_length);
}
TEST(SmootherTest, test_sg_smoother_reversing)
{
rclcpp_lifecycle::LifecycleNode::SharedPtr node =
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSGSmootherTest");
std::shared_ptr<nav2_msgs::msg::Costmap> costmap_msg =
std::make_shared<nav2_msgs::msg::Costmap>();
costmap_msg->header.stamp = node->now();
costmap_msg->header.frame_id = "map";
costmap_msg->data.resize(100 * 100);
costmap_msg->metadata.resolution = 0.05;
costmap_msg->metadata.size_x = 100;
costmap_msg->metadata.size_y = 100;
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> parent = node;
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> dummy_costmap;
dummy_costmap = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(parent, "dummy_topic");
dummy_costmap->costmapCallback(costmap_msg);
// Make smoother
std::shared_ptr<tf2_ros::Buffer> dummy_tf;
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> dummy_footprint;
node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false));
auto smoother = std::make_unique<nav2_smoother::SavitzkyGolaySmoother>();
smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint);
rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds
// Test reversing / multiple segments via a cusp
nav_msgs::msg::Path cusp_path, cusp_path_baseline;
cusp_path.header.frame_id = "map";
cusp_path.header.stamp = node->now();
cusp_path.poses.resize(22);
cusp_path.poses[0].pose.position.x = 0.5;
cusp_path.poses[0].pose.position.y = 0.1;
cusp_path.poses[1].pose.position.x = 0.5;
cusp_path.poses[1].pose.position.y = 0.2;
cusp_path.poses[2].pose.position.x = 0.5;
cusp_path.poses[2].pose.position.y = 0.3;
cusp_path.poses[3].pose.position.x = 0.5;
cusp_path.poses[3].pose.position.y = 0.4;
cusp_path.poses[4].pose.position.x = 0.5;
cusp_path.poses[4].pose.position.y = 0.5;
cusp_path.poses[5].pose.position.x = 0.5;
cusp_path.poses[5].pose.position.y = 0.6;
cusp_path.poses[6].pose.position.x = 0.5;
cusp_path.poses[6].pose.position.y = 0.7;
cusp_path.poses[7].pose.position.x = 0.5;
cusp_path.poses[7].pose.position.y = 0.8;
cusp_path.poses[8].pose.position.x = 0.5;
cusp_path.poses[8].pose.position.y = 0.9;
cusp_path.poses[9].pose.position.x = 0.5;
cusp_path.poses[9].pose.position.y = 1.0;
cusp_path.poses[10].pose.position.x = 0.5;
cusp_path.poses[10].pose.position.y = 1.1;
cusp_path.poses[11].pose.position.x = 0.5;
cusp_path.poses[11].pose.position.y = 1.0;
cusp_path.poses[12].pose.position.x = 0.5;
cusp_path.poses[12].pose.position.y = 0.9;
cusp_path.poses[13].pose.position.x = 0.5;
cusp_path.poses[13].pose.position.y = 0.8;
cusp_path.poses[14].pose.position.x = 0.5;
cusp_path.poses[14].pose.position.y = 0.7;
cusp_path.poses[15].pose.position.x = 0.5;
cusp_path.poses[15].pose.position.y = 0.6;
cusp_path.poses[16].pose.position.x = 0.5;
cusp_path.poses[16].pose.position.y = 0.5;
cusp_path.poses[17].pose.position.x = 0.5;
cusp_path.poses[17].pose.position.y = 0.4;
cusp_path.poses[18].pose.position.x = 0.5;
cusp_path.poses[18].pose.position.y = 0.3;
cusp_path.poses[19].pose.position.x = 0.5;
cusp_path.poses[19].pose.position.y = 0.2;
cusp_path.poses[20].pose.position.x = 0.5;
cusp_path.poses[20].pose.position.y = 0.1;
cusp_path.poses[21].pose.position.x = 0.5;
cusp_path.poses[21].pose.position.y = 0.0;
// Add random but deterministic noises
std::random_device rd{};
std::mt19937 gen{rd()};
std::normal_distribution<> normal_distribution{0.0, 0.02};
for (unsigned int i = 0; i != cusp_path.poses.size(); i++) {
auto noise = normal_distribution(gen);
cusp_path.poses[i].pose.position.x += noise;
}
cusp_path_baseline = cusp_path;
EXPECT_TRUE(smoother->smooth(cusp_path, max_time));
// If it detected the cusp, the cusp point should be fixed
EXPECT_EQ(cusp_path.poses[10].pose.position.x, cusp_path_baseline.poses[10].pose.position.x);
EXPECT_EQ(cusp_path.poses[10].pose.position.y, cusp_path_baseline.poses[10].pose.position.y);
// But the path also should be smoother / shorter
double length = 0;
double base_length = 0;
for (unsigned int i = 0; i != cusp_path.poses.size() - 1; i++) {
length += std::hypot(
cusp_path.poses[i + 1].pose.position.x - cusp_path.poses[i].pose.position.x,
cusp_path.poses[i + 1].pose.position.y - cusp_path.poses[i].pose.position.y);
base_length += std::hypot(
cusp_path_baseline.poses[i + 1].pose.position.x -
cusp_path_baseline.poses[i].pose.position.x,
cusp_path_baseline.poses[i + 1].pose.position.y -
cusp_path_baseline.poses[i].pose.position.y);
}
EXPECT_LT(length, base_length);
}
navigation2/nav2_smoother/test/test_simple_smoother.cpp
+280
View File
@@ -0,0 +1,280 @@
// Copyright (c) 2022, Samsung Research America
//
// 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. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include <chrono>
#include <limits>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_msgs/msg/costmap.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_smoother/simple_smoother.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
using namespace smoother_utils; // NOLINT
using namespace nav2_smoother; // NOLINT
using namespace std::chrono_literals; // NOLINT
class RclCppFixture
{
public:
RclCppFixture() {rclcpp::init(0, nullptr);}
~RclCppFixture() {rclcpp::shutdown();}
};
RclCppFixture g_rclcppfixture;
class SmootherWrapper : public nav2_smoother::SimpleSmoother
{
public:
SmootherWrapper()
: nav2_smoother::SimpleSmoother()
{
}
std::vector<PathSegment> findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path)
{
return findDirectionalPathSegments(path);
}
void setMaxItsToInvalid()
{
max_its_ = 0;
}
};
TEST(SmootherTest, test_simple_smoother)
{
rclcpp_lifecycle::LifecycleNode::SharedPtr node =
std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSmootherTest");
std::shared_ptr<nav2_msgs::msg::Costmap> costmap_msg =
std::make_shared<nav2_msgs::msg::Costmap>();
costmap_msg->header.stamp = node->now();
costmap_msg->header.frame_id = "map";
costmap_msg->data.resize(100 * 100);
costmap_msg->metadata.resolution = 0.05;
costmap_msg->metadata.size_x = 100;
costmap_msg->metadata.size_y = 100;
// island in the middle of lethal cost to cross
for (unsigned int i = 20; i <= 30; ++i) {
for (unsigned int j = 20; j <= 30; ++j) {
costmap_msg->data[j * 100 + i] = 254;
}
}
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> parent = node;
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> dummy_costmap;
dummy_costmap = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(parent, "dummy_topic");
dummy_costmap->costmapCallback(costmap_msg);
// Make smoother
std::shared_ptr<tf2_ros::Buffer> dummy_tf;
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> dummy_footprint;
auto smoother = std::make_unique<SmootherWrapper>();
smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint);
// Test that an irregular distributed path becomes more distributed
nav_msgs::msg::Path straight_irregular_path;
straight_irregular_path.header.frame_id = "map";
straight_irregular_path.header.stamp = node->now();
straight_irregular_path.poses.resize(11);
straight_irregular_path.poses[0].pose.position.x = 0.5;
straight_irregular_path.poses[0].pose.position.y = 0.0;
straight_irregular_path.poses[1].pose.position.x = 0.5;
straight_irregular_path.poses[1].pose.position.y = 0.1;
straight_irregular_path.poses[2].pose.position.x = 0.5;
straight_irregular_path.poses[2].pose.position.y = 0.2;
straight_irregular_path.poses[3].pose.position.x = 0.5;
straight_irregular_path.poses[3].pose.position.y = 0.35;
straight_irregular_path.poses[4].pose.position.x = 0.5;
straight_irregular_path.poses[4].pose.position.y = 0.4;
straight_irregular_path.poses[5].pose.position.x = 0.5;
straight_irregular_path.poses[5].pose.position.y = 0.56;
straight_irregular_path.poses[6].pose.position.x = 0.5;
straight_irregular_path.poses[6].pose.position.y = 0.9;
straight_irregular_path.poses[7].pose.position.x = 0.5;
straight_irregular_path.poses[7].pose.position.y = 0.95;
straight_irregular_path.poses[8].pose.position.x = 0.5;
straight_irregular_path.poses[8].pose.position.y = 1.3;
straight_irregular_path.poses[9].pose.position.x = 0.5;
straight_irregular_path.poses[9].pose.position.y = 2.0;
straight_irregular_path.poses[10].pose.position.x = 0.5;
straight_irregular_path.poses[10].pose.position.y = 2.5;
rclcpp::Duration no_time = rclcpp::Duration::from_seconds(0.0); // 0 seconds
rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1); // 1 second
EXPECT_FALSE(smoother->smooth(straight_irregular_path, no_time));
EXPECT_TRUE(smoother->smooth(straight_irregular_path, max_time));
for (uint i = 0; i != straight_irregular_path.poses.size() - 1; i++) {
// Check distances are more evenly spaced out now
EXPECT_LT(
fabs(
straight_irregular_path.poses[i].pose.position.y -
straight_irregular_path.poses[i + 1].pose.position.y), 0.38);
}
// Test regular path, should see no effective change
nav_msgs::msg::Path straight_regular_path;
straight_regular_path.header = straight_irregular_path.header;
straight_regular_path.poses.resize(11);
straight_regular_path.poses[0].pose.position.x = 0.5;
straight_regular_path.poses[0].pose.position.y = 0.0;
straight_regular_path.poses[1].pose.position.x = 0.5;
straight_regular_path.poses[1].pose.position.y = 0.1;
straight_regular_path.poses[2].pose.position.x = 0.5;
straight_regular_path.poses[2].pose.position.y = 0.2;
straight_regular_path.poses[3].pose.position.x = 0.5;
straight_regular_path.poses[3].pose.position.y = 0.3;
straight_regular_path.poses[4].pose.position.x = 0.5;
straight_regular_path.poses[4].pose.position.y = 0.4;
straight_regular_path.poses[5].pose.position.x = 0.5;
straight_regular_path.poses[5].pose.position.y = 0.5;
straight_regular_path.poses[6].pose.position.x = 0.5;
straight_regular_path.poses[6].pose.position.y = 0.6;
straight_regular_path.poses[7].pose.position.x = 0.5;
straight_regular_path.poses[7].pose.position.y = 0.7;
straight_regular_path.poses[8].pose.position.x = 0.5;
straight_regular_path.poses[8].pose.position.y = 0.8;
straight_regular_path.poses[9].pose.position.x = 0.5;
straight_regular_path.poses[9].pose.position.y = 0.9;
straight_regular_path.poses[10].pose.position.x = 0.5;
straight_regular_path.poses[10].pose.position.y = 1.0;
EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time));
for (uint i = 0; i != straight_regular_path.poses.size() - 1; i++) {
// Check distances are still very evenly spaced
EXPECT_NEAR(
fabs(
straight_regular_path.poses[i].pose.position.y -
straight_regular_path.poses[i + 1].pose.position.y), 0.1, 0.001);
}
// test shorter and curved if given a right angle
nav_msgs::msg::Path right_angle_path;
right_angle_path = straight_regular_path;
straight_regular_path.poses[6].pose.position.x = 0.6;
straight_regular_path.poses[6].pose.position.y = 0.5;
straight_regular_path.poses[7].pose.position.x = 0.7;
straight_regular_path.poses[7].pose.position.y = 0.5;
straight_regular_path.poses[8].pose.position.x = 0.8;
straight_regular_path.poses[8].pose.position.y = 0.5;
straight_regular_path.poses[9].pose.position.x = 0.9;
straight_regular_path.poses[9].pose.position.y = 0.5;
straight_regular_path.poses[10].pose.position.x = 0.95;
straight_regular_path.poses[10].pose.position.y = 0.5;
EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time));
EXPECT_NEAR(straight_regular_path.poses[5].pose.position.x, 0.637, 0.01);
EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.353, 0.01);
// Test that collisions are rejected
nav_msgs::msg::Path collision_path;
collision_path.poses.resize(11);
collision_path.poses[0].pose.position.x = 0.0;
collision_path.poses[0].pose.position.y = 0.0;
collision_path.poses[1].pose.position.x = 0.2;
collision_path.poses[1].pose.position.y = 0.2;
collision_path.poses[2].pose.position.x = 0.4;
collision_path.poses[2].pose.position.y = 0.4;
collision_path.poses[3].pose.position.x = 0.6;
collision_path.poses[3].pose.position.y = 0.6;
collision_path.poses[4].pose.position.x = 0.8;
collision_path.poses[4].pose.position.y = 0.8;
collision_path.poses[5].pose.position.x = 1.0;
collision_path.poses[5].pose.position.y = 1.0;
collision_path.poses[6].pose.position.x = 1.1;
collision_path.poses[6].pose.position.y = 1.1;
collision_path.poses[7].pose.position.x = 1.2;
collision_path.poses[7].pose.position.y = 1.2;
collision_path.poses[8].pose.position.x = 1.3;
collision_path.poses[8].pose.position.y = 1.3;
collision_path.poses[9].pose.position.x = 1.4;
collision_path.poses[9].pose.position.y = 1.4;
collision_path.poses[10].pose.position.x = 1.5;
collision_path.poses[10].pose.position.y = 1.5;
EXPECT_FALSE(smoother->smooth(collision_path, max_time));
// test cusp / reversing segments
nav_msgs::msg::Path reversing_path;
reversing_path.poses.resize(11);
reversing_path.poses[0].pose.position.x = 0.5;
reversing_path.poses[0].pose.position.y = 0.0;
reversing_path.poses[1].pose.position.x = 0.5;
reversing_path.poses[1].pose.position.y = 0.1;
reversing_path.poses[2].pose.position.x = 0.5;
reversing_path.poses[2].pose.position.y = 0.2;
reversing_path.poses[3].pose.position.x = 0.5;
reversing_path.poses[3].pose.position.y = 0.3;
reversing_path.poses[4].pose.position.x = 0.5;
reversing_path.poses[4].pose.position.y = 0.4;
reversing_path.poses[5].pose.position.x = 0.5;
reversing_path.poses[5].pose.position.y = 0.5;
reversing_path.poses[6].pose.position.x = 0.5;
reversing_path.poses[6].pose.position.y = 0.4;
reversing_path.poses[7].pose.position.x = 0.5;
reversing_path.poses[7].pose.position.y = 0.3;
reversing_path.poses[8].pose.position.x = 0.5;
reversing_path.poses[8].pose.position.y = 0.2;
reversing_path.poses[9].pose.position.x = 0.5;
reversing_path.poses[9].pose.position.y = 0.1;
reversing_path.poses[10].pose.position.x = 0.5;
reversing_path.poses[10].pose.position.y = 0.0;
EXPECT_TRUE(smoother->smooth(reversing_path, max_time));
// // test rotate in place
tf2::Quaternion quat1, quat2;
quat1.setRPY(0.0, 0.0, 0.0);
quat2.setRPY(0.0, 0.0, 1.0);
straight_irregular_path.poses[5].pose.position.x = 0.5;
straight_irregular_path.poses[5].pose.position.y = 0.5;
straight_irregular_path.poses[5].pose.orientation = tf2::toMsg(quat1);
straight_irregular_path.poses[6].pose.position.x = 0.5;
straight_irregular_path.poses[6].pose.position.y = 0.5;
straight_irregular_path.poses[6].pose.orientation = tf2::toMsg(quat2);
EXPECT_TRUE(smoother->smooth(straight_irregular_path, max_time));
// test max iterations
smoother->setMaxItsToInvalid();
nav_msgs::msg::Path max_its_path;
max_its_path.poses.resize(11);
max_its_path.poses[0].pose.position.x = 0.5;
max_its_path.poses[0].pose.position.y = 0.0;
max_its_path.poses[1].pose.position.x = 0.5;
max_its_path.poses[1].pose.position.y = 0.1;
max_its_path.poses[2].pose.position.x = 0.5;
max_its_path.poses[2].pose.position.y = 0.2;
max_its_path.poses[3].pose.position.x = 0.5;
max_its_path.poses[3].pose.position.y = 0.3;
max_its_path.poses[4].pose.position.x = 0.5;
max_its_path.poses[4].pose.position.y = 0.4;
max_its_path.poses[5].pose.position.x = 0.5;
max_its_path.poses[5].pose.position.y = 0.5;
max_its_path.poses[6].pose.position.x = 0.5;
max_its_path.poses[6].pose.position.y = 0.6;
max_its_path.poses[7].pose.position.x = 0.5;
max_its_path.poses[7].pose.position.y = 0.7;
max_its_path.poses[8].pose.position.x = 0.5;
max_its_path.poses[8].pose.position.y = 0.8;
max_its_path.poses[9].pose.position.x = 0.5;
max_its_path.poses[9].pose.position.y = 0.9;
max_its_path.poses[10].pose.position.x = 0.5;
max_its_path.poses[10].pose.position.y = 1.0;
EXPECT_FALSE(smoother->smooth(max_its_path, max_time));
}
navigation2/nav2_smoother/test/test_smoother_server.cpp
+440
View File
@@ -0,0 +1,440 @@
// Copyright (c) 2021 RoboTech Vision
//
// 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. Reserved.
#include <string>
#include <memory>
#include <chrono>
#include <iostream>
#include <future>
#include <thread>
#include <algorithm>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_core/smoother.hpp"
#include "nav2_core/exceptions.hpp"
#include "nav2_msgs/action/smooth_path.hpp"
#include "nav2_smoother/nav2_smoother.hpp"
#include "tf2_ros/create_timer_ros.h"
using SmoothAction = nav2_msgs::action::SmoothPath;
using ClientGoalHandle = rclcpp_action::ClientGoalHandle<SmoothAction>;
using namespace std::chrono_literals;
// A smoother for testing the base class
class DummySmoother : public nav2_core::Smoother
{
public:
DummySmoother() {}
~DummySmoother() {}
virtual void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr &,
std::string, std::shared_ptr<tf2_ros::Buffer>,
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>,
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>) {}
virtual void cleanup() {}
virtual void activate() {}
virtual void deactivate() {}
virtual bool smooth(
nav_msgs::msg::Path & path,
const rclcpp::Duration & max_time)
{
assert(path.poses.size() == 2);
if (path.poses.front() == path.poses.back()) {
throw nav2_core::PlannerException("Start and goal pose must differ");
}
auto max_time_ms = max_time.to_chrono<std::chrono::milliseconds>();
std::this_thread::sleep_for(std::min(max_time_ms, 100ms));
// place dummy pose in the middle of the path
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x =
(path.poses.front().pose.position.x + path.poses.back().pose.position.x) / 2;
pose.pose.position.y =
(path.poses.front().pose.position.y + path.poses.back().pose.position.y) / 2;
pose.pose.orientation.w = 1.0;
path.poses.push_back(pose);
return max_time_ms > 100ms;
}
private:
std::string command_;
std::chrono::system_clock::time_point start_time_;
};
// Mocked class loader
void onPluginDeletion(nav2_core::Smoother * obj)
{
if (nullptr != obj) {
delete (obj);
}
}
template<>
pluginlib::UniquePtr<nav2_core::Smoother> pluginlib::ClassLoader<nav2_core::Smoother>::
createUniqueInstance(const std::string & lookup_name)
{
if (lookup_name != "DummySmoother") {
// original method body
if (!isClassLoaded(lookup_name)) {
loadLibraryForClass(lookup_name);
}
try {
std::string class_type = getClassType(lookup_name);
pluginlib::UniquePtr<nav2_core::Smoother> obj =
lowlevel_class_loader_.createUniqueInstance<nav2_core::Smoother>(class_type);
return obj;
} catch (const class_loader::CreateClassException & ex) {
throw pluginlib::CreateClassException(ex.what());
}
}
// mocked plugin creation
return std::unique_ptr<nav2_core::Smoother,
class_loader::ClassLoader::DeleterType<nav2_core::Smoother>>(
new DummySmoother(),
onPluginDeletion);
}
class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
{
public:
DummyCostmapSubscriber(
nav2_util::LifecycleNode::SharedPtr node,
const std::string & topic_name)
: CostmapSubscriber(node, topic_name)
{
auto costmap = std::make_shared<nav2_msgs::msg::Costmap>();
costmap->metadata.size_x = 100;
costmap->metadata.size_y = 100;
costmap->metadata.resolution = 0.1;
costmap->metadata.origin.position.x = -5.0;
costmap->metadata.origin.position.y = -5.0;
costmap->data.resize(costmap->metadata.size_x * costmap->metadata.size_y, 0);
for (unsigned int i = 0; i < costmap->metadata.size_y; ++i) {
for (unsigned int j = 20; j < 40; ++j) {
costmap->data[i * costmap->metadata.size_x + j] = 254;
}
}
setCostmap(costmap);
}
void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg)
{
costmap_msg_ = msg;
costmap_received_ = true;
}
};
class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber
{
public:
DummyFootprintSubscriber(
nav2_util::LifecycleNode::SharedPtr node,
const std::string & topic_name,
tf2_ros::Buffer & tf_)
: FootprintSubscriber(node, topic_name, tf_)
{
auto footprint = std::make_shared<geometry_msgs::msg::PolygonStamped>();
footprint->header.frame_id = "base_link"; // global frame = robot frame to avoid tf lookup
footprint->header.stamp = node->get_clock()->now();
geometry_msgs::msg::Point32 point;
point.x = -0.2f;
point.y = -0.2f;
footprint->polygon.points.push_back(point);
point.y = 0.2f;
footprint->polygon.points.push_back(point);
point.x = 0.2f;
point.y = 0.0f;
footprint->polygon.points.push_back(point);
setFootprint(footprint);
}
void setFootprint(geometry_msgs::msg::PolygonStamped::SharedPtr msg)
{
footprint_ = msg;
footprint_received_ = true;
}
};
class DummySmootherServer : public nav2_smoother::SmootherServer
{
public:
DummySmootherServer()
{
// Override defaults
default_ids_.clear();
default_ids_.resize(1, "SmoothPath");
set_parameter(rclcpp::Parameter("smoother_plugins", default_ids_));
default_types_.clear();
default_types_.resize(1, "DummySmoother");
}
nav2_util::CallbackReturn
on_configure(const rclcpp_lifecycle::State & state)
{
auto result = SmootherServer::on_configure(state);
if (result != nav2_util::CallbackReturn::SUCCESS) {
return result;
}
// Create dummy subscribers and collision checker
auto node = shared_from_this();
costmap_sub_ =
std::make_shared<DummyCostmapSubscriber>(
node, "costmap_topic");
footprint_sub_ =
std::make_shared<DummyFootprintSubscriber>(
node, "footprint_topic", *tf_);
collision_checker_ =
std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
*costmap_sub_, *footprint_sub_,
node->get_name());
return result;
}
};
// Define a test class to hold the context for the tests
class SmootherTest : public ::testing::Test
{
public:
SmootherTest() {}
~SmootherTest() {}
void SetUp() override
{
node_ =
std::make_shared<rclcpp::Node>(
"LifecycleSmootherTestNode", rclcpp::NodeOptions());
smoother_server_ = std::make_shared<DummySmootherServer>();
smoother_server_->set_parameter(
rclcpp::Parameter(
"smoother_plugins",
rclcpp::ParameterValue(std::vector<std::string>(1, "DummySmoothPath"))));
smoother_server_->declare_parameter(
"DummySmoothPath.plugin",
rclcpp::ParameterValue(std::string("DummySmoother")));
smoother_server_->configure();
smoother_server_->activate();
client_ = rclcpp_action::create_client<SmoothAction>(
node_->get_node_base_interface(),
node_->get_node_graph_interface(),
node_->get_node_logging_interface(),
node_->get_node_waitables_interface(), "smooth_path");
std::cout << "Setup complete." << std::endl;
}
void TearDown() override
{
smoother_server_->deactivate();
smoother_server_->cleanup();
smoother_server_->shutdown();
smoother_server_.reset();
client_.reset();
node_.reset();
}
bool sendGoal(
std::string smoother_id, double x_start, double y_start, double x_goal,
double y_goal, std::chrono::milliseconds max_time, bool check_for_collisions)
{
if (!client_->wait_for_action_server(4s)) {
std::cout << "Server not up" << std::endl;
return false;
}
geometry_msgs::msg::PoseStamped pose;
pose.pose.orientation.w = 1.0;
auto goal = SmoothAction::Goal();
goal.smoother_id = smoother_id;
pose.pose.position.x = x_start;
pose.pose.position.y = y_start;
goal.path.poses.push_back(pose);
pose.pose.position.x = x_goal;
pose.pose.position.y = y_goal;
goal.path.poses.push_back(pose);
goal.check_for_collisions = check_for_collisions;
goal.max_smoothing_duration = rclcpp::Duration(max_time);
auto future_goal = client_->async_send_goal(goal);
if (rclcpp::spin_until_future_complete(node_, future_goal) !=
rclcpp::FutureReturnCode::SUCCESS)
{
std::cout << "failed sending goal" << std::endl;
// failed sending the goal
return false;
}
goal_handle_ = future_goal.get();
if (!goal_handle_) {
std::cout << "goal was rejected" << std::endl;
// goal was rejected by the action server
return false;
}
return true;
}
ClientGoalHandle::WrappedResult getResult()
{
std::cout << "Getting async result..." << std::endl;
auto future_result = client_->async_get_result(goal_handle_);
std::cout << "Waiting on future..." << std::endl;
rclcpp::spin_until_future_complete(node_, future_result);
std::cout << "future received!" << std::endl;
return future_result.get();
}
std::shared_ptr<rclcpp::Node> node_;
std::shared_ptr<DummySmootherServer> smoother_server_;
std::shared_ptr<rclcpp_action::Client<SmoothAction>> client_;
std::shared_ptr<rclcpp_action::ClientGoalHandle<SmoothAction>> goal_handle_;
};
// Define the tests
TEST_F(SmootherTest, testingSuccess)
{
ASSERT_TRUE(sendGoal("DummySmoothPath", 0.0, 0.0, 1.0, 0.0, 500ms, true));
auto result = getResult();
EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
EXPECT_EQ(result.result->path.poses.size(), (std::size_t)3);
EXPECT_TRUE(result.result->was_completed);
SUCCEED();
}
TEST_F(SmootherTest, testingFailureOnInvalidSmootherId)
{
ASSERT_TRUE(sendGoal("InvalidSmoother", 0.0, 0.0, 1.0, 0.0, 500ms, true));
auto result = getResult();
EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED);
SUCCEED();
}
TEST_F(SmootherTest, testingSuccessOnEmptyPlugin)
{
ASSERT_TRUE(sendGoal("", 0.0, 0.0, 1.0, 0.0, 500ms, true));
auto result = getResult();
EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
SUCCEED();
}
TEST_F(SmootherTest, testingIncomplete)
{
ASSERT_TRUE(sendGoal("DummySmoothPath", 0.0, 0.0, 1.0, 0.0, 50ms, true));
auto result = getResult();
EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
EXPECT_FALSE(result.result->was_completed);
SUCCEED();
}
TEST_F(SmootherTest, testingFailureOnException)
{
ASSERT_TRUE(sendGoal("DummySmoothPath", 0.0, 0.0, 0.0, 0.0, 500ms, true));
auto result = getResult();
EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED);
SUCCEED();
}
TEST_F(SmootherTest, testingFailureOnCollision)
{
ASSERT_TRUE(sendGoal("DummySmoothPath", -4.0, 0.0, 0.0, 0.0, 500ms, true));
auto result = getResult();
EXPECT_EQ(result.code, rclcpp_action::ResultCode::ABORTED);
SUCCEED();
}
TEST_F(SmootherTest, testingCollisionCheckDisabled)
{
ASSERT_TRUE(sendGoal("DummySmoothPath", -4.0, 0.0, 0.0, 0.0, 500ms, false));
auto result = getResult();
EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
SUCCEED();
}
TEST(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin)
{
auto smoother_server = std::make_shared<DummySmootherServer>();
smoother_server->set_parameter(
rclcpp::Parameter(
"smoother_plugins",
rclcpp::ParameterValue(std::vector<std::string>(1, "DummySmoothPath"))));
smoother_server->declare_parameter(
"DummySmoothPath.plugin",
rclcpp::ParameterValue(std::string("DummySmoother")));
auto state = smoother_server->configure();
EXPECT_EQ(state.id(), 2); // 1 on failure, 2 on success
SUCCEED();
}
TEST(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin)
{
auto smoother_server = std::make_shared<DummySmootherServer>();
smoother_server->set_parameter(
rclcpp::Parameter(
"smoother_plugins",
rclcpp::ParameterValue(std::vector<std::string>(1, "DummySmoothPath"))));
smoother_server->declare_parameter(
"DummySmoothPath.plugin",
rclcpp::ParameterValue(std::string("InvalidSmootherPlugin")));
auto state = smoother_server->configure();
EXPECT_EQ(state.id(), 1); // 1 on failure, 2 on success
SUCCEED();
}
TEST(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin)
{
auto smoother_server = std::make_shared<DummySmootherServer>();
auto state = smoother_server->configure();
EXPECT_EQ(state.id(), 2); // 1 on failure, 2 on success
SUCCEED();
}
int main(int argc, char ** argv)
{
::testing::InitGoogleTest(&argc, argv);
// initialize ROS
rclcpp::init(argc, argv);
bool all_successful = RUN_ALL_TESTS();
// shutdown ROS
rclcpp::shutdown();
return all_successful;
}