Penalize trajectories with rotational velocities. More...
#include <twirling.h>
Public Member Functions | |
void | onInit () override |
double | scoreTrajectory (const dwb_msgs::Trajectory2D &traj) override |
Public Member Functions inherited from dwb_local_planner::TrajectoryCritic | |
virtual void | addCriticVisualization (sensor_msgs::PointCloud &pc) |
virtual void | debrief (const nav_2d_msgs::Twist2D &cmd_vel) |
std::string | getName () |
virtual double | getScale () const |
void | initialize (const ros::NodeHandle &planner_nh, std::string name, nav_core2::Costmap::Ptr costmap) |
virtual bool | prepare (const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel, const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan) |
virtual void | reset () |
void | setScale (const double scale) |
virtual | ~TrajectoryCritic () |
Additional Inherited Members | |
Public Types inherited from dwb_local_planner::TrajectoryCritic | |
typedef std::shared_ptr< dwb_local_planner::TrajectoryCritic > | Ptr |
Protected Attributes inherited from dwb_local_planner::TrajectoryCritic | |
nav_core2::Costmap::Ptr | costmap_ |
ros::NodeHandle | critic_nh_ |
std::string | name_ |
ros::NodeHandle | planner_nh_ |
double | scale_ |
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.h.
|
overridevirtual |
Reimplemented from dwb_local_planner::TrajectoryCritic.
Definition at line 40 of file twirling.cpp.
|
overridevirtual |
Implements dwb_local_planner::TrajectoryCritic.
Definition at line 49 of file twirling.cpp.