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/xy_theta_iterator.h>
00036 #include <nav_2d_utils/parameters.h>
00037
00038 namespace dwb_plugins
00039 {
00040 void XYThetaIterator::initialize(ros::NodeHandle& nh, KinematicParameters::Ptr kinematics)
00041 {
00042 kinematics_ = kinematics;
00043 nh.param("vx_samples", vx_samples_, 20);
00044 nh.param("vy_samples", vy_samples_, 5);
00045 vtheta_samples_ = nav_2d_utils::loadParameterWithDeprecation(nh, "vtheta_samples", "vth_samples", 20);
00046 }
00047
00048 void XYThetaIterator::startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt)
00049 {
00050 x_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.x, kinematics_->getMinX(), kinematics_->getMaxX(),
00051 kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_);
00052 y_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.y, kinematics_->getMinY(), kinematics_->getMaxY(),
00053 kinematics_->getAccY(), kinematics_->getDecelY(), dt, vy_samples_);
00054 th_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.theta,
00055 kinematics_->getMinTheta(), kinematics_->getMaxTheta(),
00056 kinematics_->getAccTheta(), kinematics_->getDecelTheta(),
00057 dt, vtheta_samples_);
00058 if (!isValidVelocity())
00059 {
00060 iterateToValidVelocity();
00061 }
00062 }
00063
00064 bool XYThetaIterator::isValidVelocity()
00065 {
00066 return kinematics_->isValidSpeed(x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity());
00067 }
00068
00069 bool XYThetaIterator::hasMoreTwists()
00070 {
00071 return x_it_ && !x_it_->isFinished();
00072 }
00073
00074
00075 nav_2d_msgs::Twist2D XYThetaIterator::nextTwist()
00076 {
00077 nav_2d_msgs::Twist2D velocity;
00078 velocity.x = x_it_->getVelocity();
00079 velocity.y = y_it_->getVelocity();
00080 velocity.theta = th_it_->getVelocity();
00081
00082 iterateToValidVelocity();
00083
00084 return velocity;
00085 }
00086
00087 void XYThetaIterator::iterateToValidVelocity()
00088 {
00089 bool valid = false;
00090 while (!valid && hasMoreTwists())
00091 {
00092 ++(*th_it_);
00093 if (th_it_->isFinished())
00094 {
00095 th_it_->reset();
00096 ++(*y_it_);
00097 if (y_it_->isFinished())
00098 {
00099 y_it_->reset();
00100 ++(*x_it_);
00101 }
00102 }
00103 valid = isValidVelocity();
00104 }
00105 }
00106
00107 }