Standard DWA-like trajectory generator. More...
#include <standard_traj_generator.h>
Public Member Functions | |
dwb_msgs::Trajectory2D | generateTrajectory (const geometry_msgs::Pose2D &start_pose, const nav_2d_msgs::Twist2D &start_vel, const nav_2d_msgs::Twist2D &cmd_vel) override |
bool | hasMoreTwists () override |
void | initialize (ros::NodeHandle &nh) override |
nav_2d_msgs::Twist2D | nextTwist () override |
void | startNewIteration (const nav_2d_msgs::Twist2D ¤t_velocity) override |
Protected Member Functions | |
virtual void | checkUseDwaParam (const ros::NodeHandle &nh) |
Check if the deprecated use_dwa parameter is set to the functionality that matches this class. | |
virtual geometry_msgs::Pose2D | computeNewPosition (const geometry_msgs::Pose2D start_pose, const nav_2d_msgs::Twist2D &vel, const double dt) |
Use the robot's kinematic model to predict new positions for the robot. | |
virtual nav_2d_msgs::Twist2D | computeNewVelocity (const nav_2d_msgs::Twist2D &cmd_vel, const nav_2d_msgs::Twist2D &start_vel, const double dt) |
Calculate the velocity after a set period of time, given the desired velocity and acceleration limits. | |
virtual std::vector< double > | getTimeSteps (const nav_2d_msgs::Twist2D &cmd_vel) |
Compute an array of time deltas between the points in the generated trajectory. | |
virtual void | initializeIterator (ros::NodeHandle &nh) |
Initialize the VelocityIterator pointer. Put in its own function for easy overriding. | |
Protected Attributes | |
double | angular_granularity_ |
If not discretizing by time, the amount of angular space between points. | |
bool | discretize_by_time_ |
bool | include_last_point_ |
KinematicParameters::Ptr | kinematics_ |
double | linear_granularity_ |
If not discretizing by time, the amount of linear space between points. | |
double | sim_time_ |
double | time_granularity_ |
If discretizing by time, the amount of time between each point in the traj. | |
std::shared_ptr< VelocityIterator > | velocity_iterator_ |
Standard DWA-like trajectory generator.
Definition at line 52 of file standard_traj_generator.h.
void dwb_plugins::StandardTrajectoryGenerator::checkUseDwaParam | ( | const ros::NodeHandle & | nh | ) | [protected, virtual] |
Check if the deprecated use_dwa parameter is set to the functionality that matches this class.
The functionality guarded by the use_dwa parameter has been split between this class and the derived LimitedAccelGenerator. If use_dwa was false, this class should be used. If it was true, then LimitedAccelGenerator. If this is NOT the case, this function will throw an exception.
Reimplemented in dwb_plugins::LimitedAccelGenerator.
Definition at line 86 of file standard_traj_generator.cpp.
geometry_msgs::Pose2D dwb_plugins::StandardTrajectoryGenerator::computeNewPosition | ( | const geometry_msgs::Pose2D | start_pose, |
const nav_2d_msgs::Twist2D & | vel, | ||
const double | dt | ||
) | [protected, virtual] |
Use the robot's kinematic model to predict new positions for the robot.
start_pose | Starting pose |
vel | Actual robot velocity (assumed to be within acceleration limits) |
dt | amount of time in seconds |
Definition at line 189 of file standard_traj_generator.cpp.
nav_2d_msgs::Twist2D dwb_plugins::StandardTrajectoryGenerator::computeNewVelocity | ( | const nav_2d_msgs::Twist2D & | cmd_vel, |
const nav_2d_msgs::Twist2D & | start_vel, | ||
const double | dt | ||
) | [protected, virtual] |
Calculate the velocity after a set period of time, given the desired velocity and acceleration limits.
cmd_vel | Desired velocity |
start_vel | starting velocity |
dt | amount of time in seconds |
change vel using acceleration limits to converge towards sample_target-vel
Reimplemented in dwb_plugins::LimitedAccelGenerator.
Definition at line 178 of file standard_traj_generator.cpp.
dwb_msgs::Trajectory2D dwb_plugins::StandardTrajectoryGenerator::generateTrajectory | ( | const geometry_msgs::Pose2D & | start_pose, |
const nav_2d_msgs::Twist2D & | start_vel, | ||
const nav_2d_msgs::Twist2D & | cmd_vel | ||
) | [override, virtual] |
Implements dwb_local_planner::TrajectoryGenerator.
Definition at line 142 of file standard_traj_generator.cpp.
std::vector< double > dwb_plugins::StandardTrajectoryGenerator::getTimeSteps | ( | const nav_2d_msgs::Twist2D & | cmd_vel | ) | [protected, virtual] |
Compute an array of time deltas between the points in the generated trajectory.
cmd_vel | The desired command velocity |
If we are discretizing by time, the returned vector will be the same constant time_granularity for all cmd_vels. Otherwise, you will get times based on the linear/angular granularity.
Right now the vector contains a single value repeated many times, but this method could be overridden to allow for dynamic spacing
Definition at line 112 of file standard_traj_generator.cpp.
bool dwb_plugins::StandardTrajectoryGenerator::hasMoreTwists | ( | ) | [override, virtual] |
Implements dwb_local_planner::TrajectoryGenerator.
Definition at line 102 of file standard_traj_generator.cpp.
void dwb_plugins::StandardTrajectoryGenerator::initialize | ( | ros::NodeHandle & | nh | ) | [override, virtual] |
Implements dwb_local_planner::TrajectoryGenerator.
Reimplemented in dwb_plugins::LimitedAccelGenerator.
Definition at line 49 of file standard_traj_generator.cpp.
void dwb_plugins::StandardTrajectoryGenerator::initializeIterator | ( | ros::NodeHandle & | nh | ) | [protected, virtual] |
Initialize the VelocityIterator pointer. Put in its own function for easy overriding.
Definition at line 80 of file standard_traj_generator.cpp.
nav_2d_msgs::Twist2D dwb_plugins::StandardTrajectoryGenerator::nextTwist | ( | ) | [override, virtual] |
Implements dwb_local_planner::TrajectoryGenerator.
Definition at line 107 of file standard_traj_generator.cpp.
void dwb_plugins::StandardTrajectoryGenerator::startNewIteration | ( | const nav_2d_msgs::Twist2D & | current_velocity | ) | [override, virtual] |
Implements dwb_local_planner::TrajectoryGenerator.
Reimplemented in dwb_plugins::LimitedAccelGenerator.
Definition at line 97 of file standard_traj_generator.cpp.
double dwb_plugins::StandardTrajectoryGenerator::angular_granularity_ [protected] |
If not discretizing by time, the amount of angular space between points.
Definition at line 127 of file standard_traj_generator.h.
bool dwb_plugins::StandardTrajectoryGenerator::discretize_by_time_ [protected] |
Definition at line 124 of file standard_traj_generator.h.
bool dwb_plugins::StandardTrajectoryGenerator::include_last_point_ [protected] |
Definition at line 142 of file standard_traj_generator.h.
KinematicParameters::Ptr dwb_plugins::StandardTrajectoryGenerator::kinematics_ [protected] |
Definition at line 118 of file standard_traj_generator.h.
double dwb_plugins::StandardTrajectoryGenerator::linear_granularity_ [protected] |
If not discretizing by time, the amount of linear space between points.
Definition at line 126 of file standard_traj_generator.h.
double dwb_plugins::StandardTrajectoryGenerator::sim_time_ [protected] |
Definition at line 121 of file standard_traj_generator.h.
double dwb_plugins::StandardTrajectoryGenerator::time_granularity_ [protected] |
If discretizing by time, the amount of time between each point in the traj.
Definition at line 125 of file standard_traj_generator.h.
std::shared_ptr<VelocityIterator> dwb_plugins::StandardTrajectoryGenerator::velocity_iterator_ [protected] |
Definition at line 119 of file standard_traj_generator.h.