Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <dwb_plugins/limited_accel_generator.h>
00036 #include <nav_2d_utils/parameters.h>
00037 #include <pluginlib/class_list_macros.h>
00038 #include <nav_core2/exceptions.h>
00039 #include <vector>
00040
00041 namespace dwb_plugins
00042 {
00043
00044 void LimitedAccelGenerator::initialize(ros::NodeHandle& nh)
00045 {
00046 StandardTrajectoryGenerator::initialize(nh);
00047 if (nh.hasParam("sim_period"))
00048 {
00049 nh.getParam("sim_period", acceleration_time_);
00050 }
00051 else
00052 {
00053 double controller_frequency = nav_2d_utils::searchAndGetParam(nh, "controller_frequency", 20.0);
00054 if (controller_frequency > 0)
00055 {
00056 acceleration_time_ = 1.0 / controller_frequency;
00057 }
00058 else
00059 {
00060 ROS_WARN_NAMED("LimitedAccelGenerator", "A controller_frequency less than or equal to 0 has been set. "
00061 "Ignoring the parameter, assuming a rate of 20Hz");
00062 acceleration_time_ = 0.05;
00063 }
00064 }
00065 }
00066
00067 void LimitedAccelGenerator::checkUseDwaParam(const ros::NodeHandle& nh)
00068 {
00069 bool use_dwa;
00070 nh.param("use_dwa", use_dwa, true);
00071 if (!use_dwa)
00072 {
00073 throw nav_core2::PlannerException("Deprecated parameter use_dwa set to false. "
00074 "Please use StandardTrajectoryGenerator for that functionality.");
00075 }
00076 }
00077
00078 void LimitedAccelGenerator::startNewIteration(const nav_2d_msgs::Twist2D& current_velocity)
00079 {
00080
00081 velocity_iterator_->startNewIteration(current_velocity, acceleration_time_);
00082 }
00083
00084 nav_2d_msgs::Twist2D LimitedAccelGenerator::computeNewVelocity(const nav_2d_msgs::Twist2D& cmd_vel,
00085 const nav_2d_msgs::Twist2D& start_vel, const double dt)
00086 {
00087 return cmd_vel;
00088 }
00089
00090 }
00091
00092 PLUGINLIB_EXPORT_CLASS(dwb_plugins::LimitedAccelGenerator, dwb_local_planner::TrajectoryGenerator)