A version of DWBLocalPlanner with ROS services for the major components. More...
#include <debug_dwb_local_planner.h>
Public Member Functions | |
void | initialize (const ros::NodeHandle &parent, const std::string &name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override |
Override the DWB constructor to also advertise the services. More... | |
![]() | |
nav_2d_msgs::Twist2DStamped | computeVelocityCommands (const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override |
nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity More... | |
virtual nav_2d_msgs::Twist2DStamped | computeVelocityCommands (const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, std::shared_ptr< dwb_msgs::LocalPlanEvaluation > &results) |
Compute the best command given the current pose and velocity, with possible debug information. More... | |
DWBLocalPlanner () | |
Constructor that brings up pluginlib loaders. More... | |
void | initialize (const ros::NodeHandle &parent, const std::string &name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override |
nav_core2 initialization More... | |
bool | isGoalReached (const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override |
nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. More... | |
virtual dwb_msgs::TrajectoryScore | scoreTrajectory (const dwb_msgs::Trajectory2D &traj, double best_score=-1) |
Score a given command. Can be used for testing. More... | |
void | setGoalPose (const nav_2d_msgs::Pose2DStamped &goal_pose) override |
nav_core2 setGoalPose - Sets the global goal pose More... | |
void | setPlan (const nav_2d_msgs::Path2D &path) override |
nav_core2 setPlan - Sets the global plan More... | |
virtual | ~DWBLocalPlanner () |
![]() | |
virtual | ~LocalPlanner () |
Protected Member Functions | |
bool | debugLocalPlanService (dwb_msgs::DebugLocalPlan::Request &req, dwb_msgs::DebugLocalPlan::Response &res) |
bool | generateTrajectoryService (dwb_msgs::GenerateTrajectory::Request &req, dwb_msgs::GenerateTrajectory::Response &res) |
bool | generateTwistsService (dwb_msgs::GenerateTwists::Request &req, dwb_msgs::GenerateTwists::Response &res) |
TrajectoryCritic::Ptr | getCritic (std::string name) |
bool | getCriticScoreService (dwb_msgs::GetCriticScore::Request &req, dwb_msgs::GetCriticScore::Response &res) |
bool | scoreTrajectoryService (dwb_msgs::ScoreTrajectory::Request &req, dwb_msgs::ScoreTrajectory::Response &res) |
![]() | |
virtual dwb_msgs::TrajectoryScore | coreScoringAlgorithm (const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D velocity, std::shared_ptr< dwb_msgs::LocalPlanEvaluation > &results) |
Iterate through all the twists and find the best one. More... | |
virtual void | loadCritics (const std::string name) |
Load the critic parameters from the namespace. More... | |
virtual void | prepare (const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) |
Helper method for preparing for the core scoring algorithm. More... | |
std::string | resolveCriticClassName (std::string base_name) |
try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic" More... | |
virtual nav_2d_msgs::Path2D | transformGlobalPlan (const nav_2d_msgs::Pose2DStamped &pose) |
Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses. More... | |
geometry_msgs::Pose2D | transformPoseToLocal (const nav_2d_msgs::Pose2DStamped &pose) |
Helper method to transform a given pose to the local costmap frame. More... | |
Protected Attributes | |
ros::ServiceServer | critic_service_ |
ros::ServiceServer | debug_service_ |
ros::ServiceServer | generate_traj_service_ |
ros::ServiceServer | score_service_ |
ros::ServiceServer | twist_gen_service_ |
![]() | |
nav_core2::Costmap::Ptr | costmap_ |
pluginlib::ClassLoader< TrajectoryCritic > | critic_loader_ |
std::vector< TrajectoryCritic::Ptr > | critics_ |
bool | debug_trajectory_details_ |
std::vector< std::string > | default_critic_namespaces_ |
nav_2d_msgs::Path2D | global_plan_ |
Saved Global Plan. More... | |
GoalChecker::Ptr | goal_checker_ |
pluginlib::ClassLoader< GoalChecker > | goal_checker_loader_ |
nav_2d_msgs::Pose2DStamped | goal_pose_ |
Saved Goal Pose. More... | |
ros::NodeHandle | planner_nh_ |
double | prune_distance_ |
bool | prune_plan_ |
DWBPublisher | pub_ |
bool | short_circuit_trajectory_evaluation_ |
TFListenerPtr | tf_ |
pluginlib::ClassLoader< TrajectoryGenerator > | traj_gen_loader_ |
TrajectoryGenerator::Ptr | traj_generator_ |
bool | update_costmap_before_planning_ |
A version of DWBLocalPlanner with ROS services for the major components.
Advertises three services: GenerateTwists, GenerateTrajectory and DebugLocalPlan
Definition at line 54 of file debug_dwb_local_planner.h.
|
protected |
Definition at line 125 of file debug_dwb_local_planner.cpp.
|
protected |
Definition at line 69 of file debug_dwb_local_planner.cpp.
|
protected |
Definition at line 62 of file debug_dwb_local_planner.cpp.
|
protected |
Definition at line 89 of file debug_dwb_local_planner.cpp.
|
protected |
Definition at line 99 of file debug_dwb_local_planner.cpp.
|
overridevirtual |
Override the DWB constructor to also advertise the services.
Implements nav_core2::LocalPlanner.
Definition at line 45 of file debug_dwb_local_planner.cpp.
|
protected |
Definition at line 76 of file debug_dwb_local_planner.cpp.
|
protected |
Definition at line 76 of file debug_dwb_local_planner.h.
|
protected |
Definition at line 76 of file debug_dwb_local_planner.h.
|
protected |
Definition at line 76 of file debug_dwb_local_planner.h.
|
protected |
Definition at line 76 of file debug_dwb_local_planner.h.
|
protected |
Definition at line 76 of file debug_dwb_local_planner.h.