/ru/navigation/trajectoryplanner.cpp

  1#include "trajectoryplanner.h"
  2
  3bool isAngleBetweenTwoAngles(float angle, float start_angle, float end_angle)
  4{
  5    bool angle_match = false;
  6
  7    // ?? ???????
  8    if ((angle>=start_angle)&&(angle<=end_angle)) {
  9        angle_match = true;
 10    } else {
 11        angle_match = false;
 12    }
 13
 14    return angle_match;
 15}
 16
 17float TrajectoryElement::getDistanceToPoint(const RobotPosition &element_pos, const QPointF &point)
 18{
 19    float result = 10000000.0f;
 20    if (type==ElementType_Line) {
 21        // ?????????? ?? ???????
 22        QPointF end_pos = getEndpointPosition(element_pos);
 23        QVector2D v1(point-element_pos);
 24        QVector2D v2(end_pos-element_pos);
 25        float product = QVector2D::dotProduct(v1,v2);
 26
 27        QPointF p = QPointF(element_pos) + (end_pos-element_pos)*(product/v2.lengthSquared());
 28
 29        float line_sign = sign(float(v1.x()*v2.y()-v1.y()*v2.x()));
 30
 31        if ((product>=0.0)&&((product/v2.lengthSquared())<=1.0)) {
 32            // ????? ????? ??????
 33            result = line_sign*QVector2D(p-point).length();
 34        } else {
 35            // ????? ???????
 36            if (product<0) // ?????
 37                result = line_sign*QVector2D(element_pos-point).length();
 38            else //  ??????
 39                result = line_sign*QVector2D(end_pos-point).length();
 40        }
 41    } else if (type==ElementType_Arc) {
 42        // ?????????? ?? ????
 43        // ?????????? ????? ??????????
 44        QPointF rot = getArcCenter(element_pos);
 45        // ??????? ????? ? ?????? ????????? ? rot
 46        QPointF pos = (point-rot);
 47        QPointF start_pos = (element_pos-rot);
 48        // ?????????? ????????? ???? ????
 49        float start_angle_offset = Angle(atan2(start_pos.y(), start_pos.x()));
 50        float start_angle = Angle(atan2(start_pos.y(), start_pos.x())-start_angle_offset).to2PIFormat();
 51        start_angle = 0;
 52        // ???????? ????
 53        float angle_wise = sign(angle)*sign(R);
 54        float end_angle = Angle(fabs(angle)).to2PIFormat();
 55
 56
 57        // ?????????? ???? ????? ?????? ? ???????
 58        float point_angle1 = 0;
 59        float point_angle2 = 0;
 60        if (angle_wise>0) {
 61            point_angle1 = PIx2-Angle(atan2(pos.y(), pos.x()) - start_angle_offset).to2PIFormat();  // ???? ?? ?????? ? ?????
 62            point_angle2 = PIx2-Angle(atan2(pos.y(), pos.x())+PI - start_angle_offset).to2PIFormat();   // ??????????????? ????
 63        } else {
 64            point_angle1 = Angle(atan2(pos.y(), pos.x()) - start_angle_offset).to2PIFormat();  // ???? ?? ?????? ? ?????
 65            point_angle2 = Angle(atan2(pos.y(), pos.x())+PI - start_angle_offset).to2PIFormat();   // ??????????????? ????
 66        }
 67        //qDebug()<<point_angle1;
 68
 69
 70        // ??????? ???????? ?? ??? ???? ? ????????
 71
 72        bool angle_match_short = false;
 73        bool angle_match_long = false;
 74        if (fabs(angle)<PIx2) {
 75            //
 76            angle_match_long = isAngleBetweenTwoAngles(point_angle1, start_angle, end_angle);
 77            angle_match_short = isAngleBetweenTwoAngles(point_angle2, start_angle, end_angle);
 78        } else { // ???? ?????? ??? PI*2 ?? 100% ???????????
 79            angle_match_short = true;
 80        }
 81
 82        //qDebug()<<angle_match_long;
 83
 84        // ?????????? ??????????
 85        if (angle_match_short) {
 86            // ?????????? ?? ?????? - ??????
 87            result = R+angle_wise*QVector2D(pos).length();
 88        } else if (angle_match_long) { // ?????????? ?? ?????? + ??????
 89            result = R-angle_wise*QVector2D(pos).length();
 90        } else {
 91            // ????????? ?????????? ?? ????? ?? ???? ?????
 92            // ??? ?? ????????????
 93        }
 94    }
 95    return result;
 96}
 97
 98RobotPosition TrajectoryElement::getEndpointPosition(const RobotPosition &start_pos)
 99{
100    RobotPosition pos;
101    if (type==ElementType_Line) {
102        pos.a = start_pos.a;
103        pos.x = start_pos.x + line_length*cos(pos.a);
104        pos.y = start_pos.y + line_length*sin(pos.a);
105
106    } else if (type==ElementType_Arc) {
107        pos.a = start_pos.a - angle*sign(R);
108        // ????? ?????? ??????? ???????
109        QPointF rot = getArcCenter(start_pos);
110        //qDebug()<<rot;
111
112        float an = -angle*sign(R);
113        //qDebug()<<an;
114        // ???????
115        float tx = rot.x()+(start_pos.x-rot.x())*cos(an)-(start_pos.y-rot.y())*sin(an);
116        float ty = rot.y()+(start_pos.x-rot.x())*sin(an)+(start_pos.y-rot.y())*cos(an);
117
118        pos.x = tx;
119        pos.y = ty;
120    } else if (type==ElementType_Rotation) {
121        pos.a = start_pos.a+angle;
122        pos.x = start_pos.x;
123        pos.y = start_pos.y;
124    }
125    pos.speed = endpoint_speed;
126    return pos;
127}
128
129RobotPosition TrajectoryElement::interpolatePosition(const RobotPosition &start_pos, float value)
130{
131    RobotPosition pos;
132    if (type==ElementType_Line) {
133        pos.a = start_pos.a;
134        pos.x = start_pos.x + line_length*cos(pos.a)*value;
135        pos.y = start_pos.y + line_length*sin(pos.a)*value;
136
137    } else if (type==ElementType_Arc) {
138        float an = -angle*sign(R)*value;
139        pos.a = start_pos.a - an;
140        // ????? ?????? ??????? ???????
141        QPointF rot = getArcCenter(start_pos);
142        //qDebug()<<rot;
143
144        //qDebug()<<an;
145        // ???????
146        float tx = rot.x()+(start_pos.x-rot.x())*cos(an)-(start_pos.y-rot.y())*sin(an);
147        float ty = rot.y()+(start_pos.x-rot.x())*sin(an)+(start_pos.y-rot.y())*cos(an);
148
149        pos.x = tx;
150        pos.y = ty;
151    } else if (type==ElementType_Rotation) {
152        pos.a = start_pos.a+angle*value;
153        pos.x = start_pos.x;
154        pos.y = start_pos.y;
155    }
156    pos.speed = startpoint_speed*(1.0-value)+endpoint_speed*value;
157    return pos;
158}
159
160QPointF TrajectoryElement::getArcCenter(const RobotPosition &start_pos)
161{
162    return QPointF(start_pos.x-R*cos(start_pos.a+PId2), start_pos.y-R*sin(start_pos.a+PId2));
163}
164
165float ITrajectoryPlanner::getTrajectoryLength(const Trajectory &trajectory)
166{
167    float res = 0;
168    for (int i=0; i<trajectory.size(); i++) {
169        if (trajectory.elements[i].type==ElementType_Arc) {
170            res += sqr((trajectory.elements[i].R * trajectory.elements[i].angle * 10.0f));  // ???? ????? ????????????????, ??? ??????, ??????? ??? ??????
171            //res += abs(trajectory[i].R * trajectory[i].angle );
172        } else
173            if (trajectory.elements[i].type==ElementType_Line) {
174                res += fabs(trajectory.elements[i].line_length);
175            }
176    }
177
178    return res;
179}
180
181float TrajectoryElement::getElementLength()
182{
183    if (type==ElementType_Line)
184        return fabs(line_length);
185    if (type==ElementType_Arc)
186        return fabs(R*angle);
187    return 0.0f;
188}
189
190float Trajectory::getDistanceToPoint(const RobotPosition &start_pos, const QPointF &point)
191{
192    RobotPosition element_position = start_pos;
193    int size = elements.size();
194    float min_size = 10000000.0;
195
196    for (int i=0; i<size; i++) {
197        float dist = elements[i].getDistanceToPoint(element_position, point);
198        if (fabs(dist)<fabs(min_size)) {
199            min_size = dist;
200        }
201        element_position = elements[i].getEndpointPosition(element_position);
202        //qDebug()<<QString("%1  %2;%3").arg(i).arg(element_position.x).arg(element_position.y);
203    }
204    return min_size;
205}
206
207
208void Trajectory::appendTrajectory(const Trajectory &trajectory)
209{
210    for (int i=0; i<trajectory.size(); i++) {
211        elements.push_back(trajectory.elements[i]);
212    }
213}