Alex Pirciu
/
BFMC
a
src/BezierMotionPlanner/beziermotionplanner.cpp
- Committer:
- alexpirciu
- Date:
- 2019-03-28
- Revision:
- 1:ceee5a608e7c
File content as of revision 1:ceee5a608e7c:
/** ****************************************************************************** * @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