add humble-navigation2
This commit is contained in:
X-lanni
@@ -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;
|
||||
}
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user