Nav2 Navigation Stack - humble
humble
ROS 2 Navigation Stack
|
Penalize trajectories with rotational velocities. More...
#include <nav2_dwb_controller/dwb_critics/include/dwb_critics/twirling.hpp>
Public Member Functions | |
void | onInit () override |
double | scoreTrajectory (const dwb_msgs::msg::Trajectory2D &traj) override |
Return a raw score for the given trajectory. More... | |
![]() | |
void | initialize (const nav2_util::LifecycleNode::SharedPtr &nh, const std::string &name, const std::string &ns, std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros) |
Initialize the critic with appropriate pointers and parameters. More... | |
virtual void | reset () |
Reset the state of the critic. More... | |
virtual bool | prepare (const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &, const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Path2D &) |
Prior to evaluating any trajectories, look at contextual information constant across all trajectories. More... | |
virtual void | debrief (const nav_2d_msgs::msg::Twist2D &) |
debrief informs the critic what the chosen cmd_vel was (if it cares) | |
virtual void | addCriticVisualization (std::vector< std::pair< std::string, std::vector< float >>> &) |
Add information to the given pointcloud for debugging costmap-grid based scores. More... | |
std::string | getName () |
virtual double | getScale () const |
void | setScale (const double scale) |
Additional Inherited Members | |
![]() | |
using | Ptr = std::shared_ptr< dwb_core::TrajectoryCritic > |
![]() | |
std::string | name_ |
std::string | dwb_plugin_name_ |
std::shared_ptr< nav2_costmap_2d::Costmap2DROS > | costmap_ros_ |
double | scale_ |
rclcpp_lifecycle::LifecycleNode::WeakPtr | node_ |
Penalize trajectories with rotational velocities.
This class provides a cost based on how much a robot "twirls" on its way to the goal. With differential-drive robots, there isn't a choice, but with holonomic or near-holonomic robots, sometimes a robot spins more than you'd like on its way to a goal. This class provides a way to assign a penalty purely to rotational velocities.
Definition at line 51 of file twirling.hpp.
|
overridevirtual |
Return a raw score for the given trajectory.
scores < 0 are considered invalid/errors, such as collisions This is the raw score in that the scale should not be applied to it.
Implements dwb_core::TrajectoryCritic.
Definition at line 50 of file twirling.cpp.