beziermotionplanner.cpp

00001 /**
00002   ******************************************************************************
00003   * @file    BezierMotionPlanner.cpp
00004   * @author  RBRO/PJ-IU
00005   * @version V1.0.0
00006   * @date    day-month-year
00007   * @brief   This file contains the class definition for the Bezier Motion
00008   *          Planner methods.
00009   ******************************************************************************
00010  */
00011 
00012 #include <iostream>
00013 #include <BezierMotionPlanner/bezierMotionplanner.hpp>
00014 
00015 namespace planner{
00016     
00017     /**
00018      * @brief Construct a new CBezierMotionPlanner::CBezierMotionPlanner object
00019      * 
00020      */
00021     CBezierMotionPlanner::CBezierMotionPlanner()
00022     {
00023         this->isInitialized=false;
00024     }
00025 
00026     /**
00027      * @brief Construct a new CBezierMotionPlanner::CBezierMotionPlanner object
00028      * 
00029      * @param isForward             Forward movement flag
00030      * @param a                     Point A
00031      * @param b                     Point B
00032      * @param c                     Point C
00033      * @param d                     Point D
00034      * @param motion_duration_i     The motion duration in second. 
00035      * @param timestep_i            The base period of the planner. (Sample time) 
00036      */
00037     CBezierMotionPlanner::CBezierMotionPlanner(bool                    isForward,
00038                                             std::complex<float>     a,
00039                                             std::complex<float>     b,
00040                                             std::complex<float>     c,
00041                                             std::complex<float>     d,
00042                                             float                   motion_duration_i,
00043                                             float                   timestep_i)
00044         : isForward(isForward)
00045         , bezierCurve(a,b,c,d)
00046         , motion_duration(motion_duration_i)
00047         , time_step(timestep_i)
00048     {
00049         this->bezierValueInput_step=1.0/(int)(this->motion_duration/this->time_step);
00050         this->next_bezierValueInput=0.0;
00051         this->isInitialized=true;
00052     }
00053 
00054 
00055     /**
00056      * @brief Set motion planner parameters
00057      * 
00058      * @param isForward             Forward movement flag
00059      * @param a                     Point A
00060      * @param b                     Point B
00061      * @param c                     Point C
00062      * @param d                     Point D
00063      * @param motion_duration_i     The motion duration in second. 
00064      * @param timestep_i            The base period of the planner. (Sample time) 
00065      */
00066     void CBezierMotionPlanner::setMotionPlannerParameters(bool                    isForward,
00067                                                         std::complex<float>     a,
00068                                                         std::complex<float>     b,
00069                                                         std::complex<float>     c,
00070                                                         std::complex<float>     d,
00071                                                         float                   motion_duration_i,
00072                                                         float                   timestep_i)
00073     {
00074         this->isForward=isForward;
00075         this->motion_duration=motion_duration_i;
00076         this->time_step=timestep_i;
00077         this->bezierCurve.setBezierCurve(a,b,c,d);
00078         this->bezierValueInput_step=1.0/(int)(this->motion_duration/this->time_step);
00079         this->next_bezierValueInput=0.0;
00080         this->isInitialized=true;                        
00081     }
00082 
00083     /**
00084      * @brief Destroy the CBezierMotionPlanner::CBezierMotionPlanner object
00085      * 
00086      */
00087     CBezierMotionPlanner::~CBezierMotionPlanner()
00088     {
00089     }
00090 
00091 
00092     /**
00093      * @brief Get the Bezier curve.
00094      * 
00095      */
00096     math::BezierCurve<float>  CBezierMotionPlanner::getBezierCurve()
00097     {
00098         return this->bezierCurve;
00099     }
00100 
00101     /**
00102      * @brief Get the next control parameters. It calculates the velocity and angle, and increase input value. 
00103      * 
00104      * @return It returns the next forward velocity and direction angle. 
00105      */
00106     std::pair<float,float> CBezierMotionPlanner::getNextVelocity(){
00107         std::pair<float,float> commands=this->getVelocity(next_bezierValueInput);
00108         this->next_bezierValueInput+=this->bezierValueInput_step;
00109         return commands;
00110     }
00111 
00112     /**
00113      * @brief Get the forward velocity and steering angle, base on the given input value.
00114      * 
00115      * @param input_value The input value have to belong to interval [0,1].
00116      * @return It returns a pair of number, where the fist variable and the second contains the forward velocity and the steering angular, respectively.
00117      */
00118     std::pair<float,float> CBezierMotionPlanner::getVelocity(float input_value)
00119     {
00120         if(!this->isInitialized) return std::pair<float,float>(0,0);
00121         
00122         std::complex<float> dS=this->bezierCurve.get_FO_DerivateValue(input_value);
00123 
00124         float dl_absolute=sqrt((dS*std::conj(dS)).real());//[0,1]//Length of the vector
00125         float dl_real=dl_absolute/this->motion_duration;
00126 
00127         std::complex<float> ddS=this->bezierCurve.get_SO_DerivateValue(input_value);
00128 
00129         std::complex<float> correctorValue(0,-2);
00130         std::complex<float> temp1=(dS*std::conj(ddS)-std::conj(dS)*ddS)/correctorValue;
00131         float num=temp1.real();
00132         if(dl_absolute==0)
00133         {
00134             return std::pair<float,float>(dl_real,0);
00135         }
00136 
00137         float k=num/pow(dl_absolute,3);
00138 
00139         float angle_rad=atan(k*WHEELBASE);
00140         float angle_deg=(180.f/M_PI)*angle_rad;
00141 
00142         std::pair<float,float> commands(dl_real,angle_deg);
00143         return commands;
00144     }
00145 
00146     /**
00147      * @brief Get the state of the planner.
00148      * 
00149      * @return true The planner has valid value. 
00150      * @return false The planner finished the last given curve, it cannnot get correct parameters. 
00151      */
00152     bool CBezierMotionPlanner::hasValidValue()
00153     {
00154         return (next_bezierValueInput>=0 && next_bezierValueInput<=1);
00155     }
00156 
00157     /** @brief  Get the direction of the motion
00158      *
00159      *  
00160      * @return true The motion direction is forward. 
00161      * @return false The motion direction is backward. 
00162      */
00163     bool CBezierMotionPlanner::getForward()
00164     {
00165         return this->isForward;
00166     }
00167 
00168 }; // namespace planner