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/standard_traj_generator.h>
00036 #include <dwb_plugins/xy_theta_iterator.h>
00037 #include <nav_2d_utils/parameters.h>
00038 #include <pluginlib/class_list_macros.h>
00039 #include <nav_core2/exceptions.h>
00040 #include <string>
00041 #include <vector>
00042 #include <algorithm>
00043
00044 using nav_2d_utils::loadParameterWithDeprecation;
00045
00046 namespace dwb_plugins
00047 {
00048
00049 void StandardTrajectoryGenerator::initialize(ros::NodeHandle& nh)
00050 {
00051 kinematics_ = std::make_shared<KinematicParameters>();
00052 kinematics_->initialize(nh);
00053 initializeIterator(nh);
00054
00055 nh.param("sim_time", sim_time_, 1.7);
00056 checkUseDwaParam(nh);
00057
00058 nh.param("include_last_point", include_last_point_, true);
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068 nh.param("discretize_by_time", discretize_by_time_, false);
00069 if (discretize_by_time_)
00070 {
00071 time_granularity_ = loadParameterWithDeprecation(nh, "time_granularity", "sim_granularity", 0.025);
00072 }
00073 else
00074 {
00075 linear_granularity_ = loadParameterWithDeprecation(nh, "linear_granularity", "sim_granularity", 0.025);
00076 angular_granularity_ = loadParameterWithDeprecation(nh, "angular_granularity", "angular_sim_granularity", 0.1);
00077 }
00078 }
00079
00080 void StandardTrajectoryGenerator::initializeIterator(ros::NodeHandle& nh)
00081 {
00082 velocity_iterator_ = std::make_shared<XYThetaIterator>();
00083 velocity_iterator_->initialize(nh, kinematics_);
00084 }
00085
00086 void StandardTrajectoryGenerator::checkUseDwaParam(const ros::NodeHandle& nh)
00087 {
00088 bool use_dwa;
00089 nh.param("use_dwa", use_dwa, false);
00090 if (use_dwa)
00091 {
00092 throw nav_core2::PlannerException("Deprecated parameter use_dwa set to true. "
00093 "Please use LimitedAccelGenerator for that functionality.");
00094 }
00095 }
00096
00097 void StandardTrajectoryGenerator::startNewIteration(const nav_2d_msgs::Twist2D& current_velocity)
00098 {
00099 velocity_iterator_->startNewIteration(current_velocity, sim_time_);
00100 }
00101
00102 bool StandardTrajectoryGenerator::hasMoreTwists()
00103 {
00104 return velocity_iterator_->hasMoreTwists();
00105 }
00106
00107 nav_2d_msgs::Twist2D StandardTrajectoryGenerator::nextTwist()
00108 {
00109 return velocity_iterator_->nextTwist();
00110 }
00111
00112 std::vector<double> StandardTrajectoryGenerator::getTimeSteps(const nav_2d_msgs::Twist2D& cmd_vel)
00113 {
00114 std::vector<double> steps;
00115 if (discretize_by_time_)
00116 {
00117 steps.resize(ceil(sim_time_ / time_granularity_));
00118 }
00119 else
00120 {
00121 double vmag = hypot(cmd_vel.x, cmd_vel.y);
00122
00123
00124 double projected_linear_distance = vmag * sim_time_;
00125
00126
00127 double projected_angular_distance = fabs(cmd_vel.theta) * sim_time_;
00128
00129
00130 int num_steps = ceil(std::max(projected_linear_distance / linear_granularity_,
00131 projected_angular_distance / angular_granularity_));
00132 steps.resize(num_steps);
00133 }
00134 if (steps.size() == 0)
00135 {
00136 steps.resize(1);
00137 }
00138 std::fill(steps.begin(), steps.end(), sim_time_ / steps.size());
00139 return steps;
00140 }
00141
00142 dwb_msgs::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(const geometry_msgs::Pose2D& start_pose,
00143 const nav_2d_msgs::Twist2D& start_vel,
00144 const nav_2d_msgs::Twist2D& cmd_vel)
00145 {
00146 dwb_msgs::Trajectory2D traj;
00147 traj.velocity = cmd_vel;
00148
00149
00150 geometry_msgs::Pose2D pose = start_pose;
00151 nav_2d_msgs::Twist2D vel = start_vel;
00152 double running_time = 0.0;
00153 std::vector<double> steps = getTimeSteps(cmd_vel);
00154 for (double dt : steps)
00155 {
00156 traj.poses.push_back(pose);
00157 traj.time_offsets.push_back(ros::Duration(running_time));
00158
00159 vel = computeNewVelocity(cmd_vel, vel, dt);
00160
00161
00162 pose = computeNewPosition(pose, vel, dt);
00163 running_time += dt;
00164 }
00165
00166 if (include_last_point_)
00167 {
00168 traj.poses.push_back(pose);
00169 traj.time_offsets.push_back(ros::Duration(running_time));
00170 }
00171
00172 return traj;
00173 }
00174
00178 nav_2d_msgs::Twist2D StandardTrajectoryGenerator::computeNewVelocity(const nav_2d_msgs::Twist2D& cmd_vel,
00179 const nav_2d_msgs::Twist2D& start_vel, const double dt)
00180 {
00181 nav_2d_msgs::Twist2D new_vel;
00182 new_vel.x = projectVelocity(start_vel.x, kinematics_->getAccX(), kinematics_->getDecelX(), dt, cmd_vel.x);
00183 new_vel.y = projectVelocity(start_vel.y, kinematics_->getAccY(), kinematics_->getDecelY(), dt, cmd_vel.y);
00184 new_vel.theta = projectVelocity(start_vel.theta, kinematics_->getAccTheta(), kinematics_->getDecelTheta(),
00185 dt, cmd_vel.theta);
00186 return new_vel;
00187 }
00188
00189 geometry_msgs::Pose2D StandardTrajectoryGenerator::computeNewPosition(const geometry_msgs::Pose2D start_pose,
00190 const nav_2d_msgs::Twist2D& vel, const double dt)
00191 {
00192 geometry_msgs::Pose2D new_pose;
00193 new_pose.x = start_pose.x + (vel.x * cos(start_pose.theta) + vel.y * cos(M_PI_2 + start_pose.theta)) * dt;
00194 new_pose.y = start_pose.y + (vel.x * sin(start_pose.theta) + vel.y * sin(M_PI_2 + start_pose.theta)) * dt;
00195 new_pose.theta = start_pose.theta + vel.theta * dt;
00196 return new_pose;
00197 }
00198
00199 }
00200
00201 PLUGINLIB_EXPORT_CLASS(dwb_plugins::StandardTrajectoryGenerator, dwb_local_planner::TrajectoryGenerator)