Alex Pirciu
/
BFMC
a
Diff: src/BezierMotionPlanner/beziermotionplanner.cpp
- Revision:
- 1:ceee5a608e7c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/BezierMotionPlanner/beziermotionplanner.cpp Thu Mar 28 07:44:42 2019 +0000 @@ -0,0 +1,168 @@ +/** + ****************************************************************************** + * @file BezierMotionPlanner.cpp + * @author RBRO/PJ-IU + * @version V1.0.0 + * @date day-month-year + * @brief This file contains the class definition for the Bezier Motion + * Planner methods. + ****************************************************************************** + */ + +#include <iostream> +#include <BezierMotionPlanner/bezierMotionplanner.hpp> + +namespace planner{ + + /** + * @brief Construct a new CBezierMotionPlanner::CBezierMotionPlanner object + * + */ + CBezierMotionPlanner::CBezierMotionPlanner() + { + this->isInitialized=false; + } + + /** + * @brief Construct a new CBezierMotionPlanner::CBezierMotionPlanner object + * + * @param isForward Forward movement flag + * @param a Point A + * @param b Point B + * @param c Point C + * @param d Point D + * @param motion_duration_i The motion duration in second. + * @param timestep_i The base period of the planner. (Sample time) + */ + CBezierMotionPlanner::CBezierMotionPlanner(bool isForward, + std::complex<float> a, + std::complex<float> b, + std::complex<float> c, + std::complex<float> d, + float motion_duration_i, + float timestep_i) + : isForward(isForward) + , bezierCurve(a,b,c,d) + , motion_duration(motion_duration_i) + , time_step(timestep_i) + { + this->bezierValueInput_step=1.0/(int)(this->motion_duration/this->time_step); + this->next_bezierValueInput=0.0; + this->isInitialized=true; + } + + + /** + * @brief Set motion planner parameters + * + * @param isForward Forward movement flag + * @param a Point A + * @param b Point B + * @param c Point C + * @param d Point D + * @param motion_duration_i The motion duration in second. + * @param timestep_i The base period of the planner. (Sample time) + */ + void CBezierMotionPlanner::setMotionPlannerParameters(bool isForward, + std::complex<float> a, + std::complex<float> b, + std::complex<float> c, + std::complex<float> d, + float motion_duration_i, + float timestep_i) + { + this->isForward=isForward; + this->motion_duration=motion_duration_i; + this->time_step=timestep_i; + this->bezierCurve.setBezierCurve(a,b,c,d); + this->bezierValueInput_step=1.0/(int)(this->motion_duration/this->time_step); + this->next_bezierValueInput=0.0; + this->isInitialized=true; + } + + /** + * @brief Destroy the CBezierMotionPlanner::CBezierMotionPlanner object + * + */ + CBezierMotionPlanner::~CBezierMotionPlanner() + { + } + + + /** + * @brief Get the Bezier curve. + * + */ + math::BezierCurve<float> CBezierMotionPlanner::getBezierCurve() + { + return this->bezierCurve; + } + + /** + * @brief Get the next control parameters. It calculates the velocity and angle, and increase input value. + * + * @return It returns the next forward velocity and direction angle. + */ + std::pair<float,float> CBezierMotionPlanner::getNextVelocity(){ + std::pair<float,float> commands=this->getVelocity(next_bezierValueInput); + this->next_bezierValueInput+=this->bezierValueInput_step; + return commands; + } + + /** + * @brief Get the forward velocity and steering angle, base on the given input value. + * + * @param input_value The input value have to belong to interval [0,1]. + * @return It returns a pair of number, where the fist variable and the second contains the forward velocity and the steering angular, respectively. + */ + std::pair<float,float> CBezierMotionPlanner::getVelocity(float input_value) + { + if(!this->isInitialized) return std::pair<float,float>(0,0); + + std::complex<float> dS=this->bezierCurve.get_FO_DerivateValue(input_value); + + float dl_absolute=sqrt((dS*std::conj(dS)).real());//[0,1]//Length of the vector + float dl_real=dl_absolute/this->motion_duration; + + std::complex<float> ddS=this->bezierCurve.get_SO_DerivateValue(input_value); + + std::complex<float> correctorValue(0,-2); + std::complex<float> temp1=(dS*std::conj(ddS)-std::conj(dS)*ddS)/correctorValue; + float num=temp1.real(); + if(dl_absolute==0) + { + return std::pair<float,float>(dl_real,0); + } + + float k=num/pow(dl_absolute,3); + + float angle_rad=atan(k*WHEELBASE); + float angle_deg=(180.f/M_PI)*angle_rad; + + std::pair<float,float> commands(dl_real,angle_deg); + return commands; + } + + /** + * @brief Get the state of the planner. + * + * @return true The planner has valid value. + * @return false The planner finished the last given curve, it cannnot get correct parameters. + */ + bool CBezierMotionPlanner::hasValidValue() + { + return (next_bezierValueInput>=0 && next_bezierValueInput<=1); + } + + /** @brief Get the direction of the motion + * + * + * @return true The motion direction is forward. + * @return false The motion direction is backward. + */ + bool CBezierMotionPlanner::getForward() + { + return this->isForward; + } + +}; // namespace planner