Program Listing for File dwb_local_planner.hpp
↰ Return to documentation for file (include/dwb_core/dwb_local_planner.hpp
)
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CORE__DWB_LOCAL_PLANNER_HPP_
#define DWB_CORE__DWB_LOCAL_PLANNER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "nav2_core/controller.hpp"
#include "nav2_core/goal_checker.hpp"
#include "dwb_core/publisher.hpp"
#include "dwb_core/trajectory_critic.hpp"
#include "dwb_core/trajectory_generator.hpp"
#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
#include "nav_2d_msgs/msg/twist2_d_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
namespace dwb_core
{
class DWBLocalPlanner : public nav2_core::Controller
{
public:
DWBLocalPlanner();
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
virtual ~DWBLocalPlanner() {}
void activate() override;
void deactivate() override;
void cleanup() override;
void setPlan(const nav_msgs::msg::Path & path) override;
geometry_msgs::msg::TwistStamped computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * /*goal_checker*/) override;
virtual dwb_msgs::msg::TrajectoryScore scoreTrajectory(
const dwb_msgs::msg::Trajectory2D & traj,
double best_score = -1);
virtual nav_2d_msgs::msg::Twist2DStamped computeVelocityCommands(
const nav_2d_msgs::msg::Pose2DStamped & pose,
const nav_2d_msgs::msg::Twist2D & velocity,
std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results);
void setSpeedLimit(const double & speed_limit, const bool & percentage) override
{
if (traj_generator_) {
traj_generator_->setSpeedLimit(speed_limit, percentage);
}
}
protected:
void prepareGlobalPlan(
const nav_2d_msgs::msg::Pose2DStamped & pose, nav_2d_msgs::msg::Path2D & transformed_plan,
nav_2d_msgs::msg::Pose2DStamped & goal_pose, bool publish_plan = true);
virtual dwb_msgs::msg::TrajectoryScore coreScoringAlgorithm(
const geometry_msgs::msg::Pose2D & pose,
const nav_2d_msgs::msg::Twist2D velocity,
std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results);
virtual nav_2d_msgs::msg::Path2D transformGlobalPlan(
const nav_2d_msgs::msg::Pose2DStamped & pose);
nav_2d_msgs::msg::Path2D global_plan_;
bool prune_plan_;
double prune_distance_;
bool debug_trajectory_details_;
rclcpp::Duration transform_tolerance_{0, 0};
bool shorten_transformed_plan_;
double forward_prune_distance_;
std::string resolveCriticClassName(std::string base_name);
virtual void loadCritics();
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
rclcpp::Clock::SharedPtr clock_;
rclcpp::Logger logger_{rclcpp::get_logger("DWBLocalPlanner")};
std::shared_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
std::unique_ptr<DWBPublisher> pub_;
std::vector<std::string> default_critic_namespaces_;
// Plugin handling
pluginlib::ClassLoader<TrajectoryGenerator> traj_gen_loader_;
TrajectoryGenerator::Ptr traj_generator_;
pluginlib::ClassLoader<TrajectoryCritic> critic_loader_;
std::vector<TrajectoryCritic::Ptr> critics_;
std::string dwb_plugin_name_;
bool short_circuit_trajectory_evaluation_;
};
} // namespace dwb_core
#endif // DWB_CORE__DWB_LOCAL_PLANNER_HPP_