Merge pull request #81 from IRobot-Algorithm/navigation-branch

fix : 提高控制频率

src/navigation/move_base/param/global_costmap_params.yaml

@@ -2,7 +2,7 @@ global_costmap:
   global_frame: map
   robot_base_frame: base_link
 
-  inflation_radius: 0.18
+  inflation_radius: 0.22
 
   update_frequency: 1.0
   publish_frequency: 1.0

src/navigation/move_base/param/move_base_params.yaml

@@ -1,5 +1,5 @@
 shutdown_costmaps: false
-controller_frequency: 5.0
+controller_frequency: 15.0
 planner_patience: 5.0
 controller_patience: 15.0
 conservative_reset_dist: 3.0

src/navigation/teb_local_planner/include/cubic_spline/cpprobotics_types.h

@@ -0,0 +1,30 @@
+/*************************************************************************
+	> File Name: cpprobotics_types.h
+	> Author: TAI Lei
+	> Mail: [email protected]
+	> Created Time: Mon Apr 22 13:25:12 2019
+ ************************************************************************/
+
+#ifndef _CPPROBOTICS_TYPES_H
+#define _CPPROBOTICS_TYPES_H
+
+#include <iterator>
+#include <vector>
+#include <array>
+#include <string>
+#include <iostream>
+
+namespace cpprobotics{
+
+using Vec_f=std::vector<float>;
+using Poi_f=std::array<float, 2>;
+using Vec_Poi=std::vector<Poi_f>;
+
+using Matrix5f = Eigen::Matrix<float, 5, 5>;
+using Matrix52f = Eigen::Matrix<float, 5, 2>;
+using Matrix25f = Eigen::Matrix<float, 2, 5>;
+using RowVector5f = Eigen::Matrix<float, 1, 5>;
+using Vector5f = Eigen::Matrix<float, 5, 1>;
+};
+
+#endif

src/navigation/teb_local_planner/include/cubic_spline/cubic_spline.h

@@ -0,0 +1,180 @@
+/*************************************************************************
+	> File Name: csv_reader.h
+	> Author: TAI Lei
+	> Mail: [email protected]
+	> Created Time: Wed Mar 27 12:49:12 2019
+ ************************************************************************/
+
+#ifndef _CUBIC_SPLINE_H
+#define _CUBIC_SPLINE_H
+
+#include <iostream>
+#include <vector>
+#include <array>
+#include <string>
+#include <Eigen/Eigen>
+#include <stdexcept>
+#include "cpprobotics_types.h"
+
+namespace cpprobotics{
+
+Vec_f vec_diff(Vec_f input){
+  Vec_f output;
+  for(unsigned int i=1; i<input.size(); i++){
+    output.push_back(input[i] - input[i-1]);
+  }
+  return output;
+};
+
+Vec_f cum_sum(Vec_f input){
+  Vec_f output;
+  float temp = 0;
+  for(unsigned int i=0; i<input.size(); i++){
+    temp += input[i];
+    output.push_back(temp);
+  }
+  return output;
+};
+
+class Spline{
+public:
+  Vec_f x;
+  Vec_f y;
+  int nx;
+  Vec_f h;
+  Vec_f a;
+  Vec_f b;
+  Vec_f c;
+  //Eigen::VectorXf c;
+  Vec_f d;
+
+  Spline(){};
+  // d_i * (x-x_i)^3 + c_i * (x-x_i)^2 + b_i * (x-x_i) + a_i
+  Spline(Vec_f x_, Vec_f y_):x(x_), y(y_), nx(x_.size()), h(vec_diff(x_)), a(y_){
+    Eigen::MatrixXf A = calc_A();
+    Eigen::VectorXf B = calc_B();
+    Eigen::VectorXf c_eigen = A.colPivHouseholderQr().solve(B);
+    float * c_pointer = c_eigen.data();
+    //Eigen::Map<Eigen::VectorXf>(c, c_eigen.rows(), 1) = c_eigen;
+    c.assign(c_pointer, c_pointer+c_eigen.rows());
+
+    for(int i=0; i<nx-1; i++){
+      d.push_back((c[i+1]-c[i])/(3.0*h[i]));
+      b.push_back((a[i+1] - a[i])/h[i] - h[i] * (c[i+1] + 2*c[i])/3.0);
+    }
+  };
+
+  float calc(float t){
+    if(t<x.front() || t>x.back()){
+      throw std::invalid_argument( "received value out of the pre-defined range" );
+    }
+    int seg_id = bisect(t, 0, nx);
+    float dx = t - x[seg_id];
+    return a[seg_id] + b[seg_id] * dx + c[seg_id] * dx * dx + d[seg_id] * dx * dx * dx;
+  };
+
+  float calc_d(float t){
+    if(t<x.front() || t>x.back()){
+      throw std::invalid_argument( "received value out of the pre-defined range" );
+    }
+    int seg_id = bisect(t, 0, nx-1);
+    float dx = t - x[seg_id];
+    return b[seg_id]  + 2 * c[seg_id] * dx + 3 * d[seg_id] * dx * dx;
+  }
+
+  float calc_dd(float t){
+    if(t<x.front() || t>x.back()){
+      throw std::invalid_argument( "received value out of the pre-defined range" );
+    }
+    int seg_id = bisect(t, 0, nx);
+    float dx = t - x[seg_id];
+    return 2 * c[seg_id] + 6 * d[seg_id] * dx;
+  }
+
+private:
+  Eigen::MatrixXf calc_A(){
+    Eigen::MatrixXf A = Eigen::MatrixXf::Zero(nx, nx);
+    A(0, 0) = 1;
+    for(int i=0; i<nx-1; i++){
+      if (i != nx-2){
+        A(i+1, i+1) = 2 * (h[i] + h[i+1]);
+      }
+      A(i+1, i) = h[i];
+      A(i, i+1) = h[i];
+    }
+    A(0, 1) = 0.0;
+    A(nx-1, nx-2) = 0.0;
+    A(nx-1, nx-1) = 1.0;
+    return A;
+  };
+  Eigen::VectorXf calc_B(){
+    Eigen::VectorXf B = Eigen::VectorXf::Zero(nx);
+    for(int i=0; i<nx-2; i++){
+      B(i+1) = 3.0*(a[i+2]-a[i+1])/h[i+1] - 3.0*(a[i+1]-a[i])/h[i];
+    }
+    return B;
+  };
+
+  int bisect(float t, int start, int end){
+    int mid = (start+end)/2;
+    if (t==x[mid] || end-start<=1){
+      return mid;
+    }else if (t>x[mid]){
+      return bisect(t, mid, end);
+    }else{
+      return bisect(t, start, mid);
+    }
+  }
+};
+
+class Spline2D{
+public:
+  Spline sx;
+  Spline sy;
+  Vec_f s;
+
+  Spline2D(Vec_f x, Vec_f y){
+    s = calc_s(x, y);
+    sx = Spline(s, x);
+    sy = Spline(s, y);
+  };
+
+  Poi_f calc_postion(float s_t){
+    float x = sx.calc(s_t);
+    float y = sy.calc(s_t);
+    return {{x, y}};
+  };
+
+  float calc_curvature(float s_t){
+    float dx = sx.calc_d(s_t);
+    float ddx = sx.calc_dd(s_t);
+    float dy = sy.calc_d(s_t);
+    float ddy = sy.calc_dd(s_t);
+    return (ddy * dx - ddx * dy)/(dx * dx + dy * dy);
+  };
+
+  float calc_yaw(float s_t){
+    float dx = sx.calc_d(s_t);
+    float dy = sy.calc_d(s_t);
+    return std::atan2(dy, dx);
+  };
+
+
+private:
+  Vec_f calc_s(Vec_f x, Vec_f y){
+    Vec_f ds;
+    Vec_f out_s{0};
+    Vec_f dx = vec_diff(x);
+    Vec_f dy = vec_diff(y);
+
+    for(unsigned int i=0; i<dx.size(); i++){
+      ds.push_back(std::sqrt(dx[i]*dx[i] + dy[i]*dy[i]));
+    }
+
+    Vec_f cum_ds = cum_sum(ds);
+    out_s.insert(out_s.end(), cum_ds.begin(), cum_ds.end());
+    return out_s;
+  };
+};
+}
+#endif

src/navigation/teb_local_planner/include/cubic_spline/cubic_spline_ros.h

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *  Copyright (C) 2019 RoboMaster.
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program. If not, see <http://www.gnu.org/licenses/>.
+ ***************************************************************************/
+
+#ifndef ROBOMASTER_CUBIC_SPLINE_ROS_H
+#define ROBOMASTER_CUBIC_SPLINE_ROS_H
+
+#include "cubic_spline.h"
+#include "cpprobotics_types.h"
+
+struct TrajInfo{
+  std::vector<float> smoothed_path_x,
+      smoothed_path_y,
+      smoothed_path_yaw,
+      smoothed_path_curvature,
+      smoothed_path_s;
+};
+
+TrajInfo GenTraj(const nav_msgs::Path& path, nav_msgs::Path& smoothed_path, const float interval = 0.1){
+  using namespace cpprobotics;
+  TrajInfo traj_info;
+  std::vector<float> path_x, path_y;
+
+  for (int j = 0 ; j < path.poses.size(); ++j) {
+    path_x.push_back(path.poses[j].pose.position.x);
+    path_y.push_back(path.poses[j].pose.position.y);
+  }
+
+  // create a cubic spline interpolator
+  Spline2D cublic_spline(path_x, path_y);
+  // calculatae the new smoothed trajectory
+  geometry_msgs::PoseStamped tmp_pose;
+  tmp_pose.header.frame_id = path.header.frame_id;
+  tmp_pose.header.stamp = ros::Time::now();
+  for(float i=0; i<cublic_spline.s.back(); i += interval){
+
+    std::array<float, 2> point_ = cublic_spline.calc_postion(i);
+    traj_info.smoothed_path_x.push_back(point_[0]);
+    traj_info.smoothed_path_y.push_back(point_[1]);
+    float yaw = cublic_spline.calc_yaw(i);
+    traj_info.smoothed_path_yaw.push_back(yaw);
+    traj_info.smoothed_path_curvature.push_back(cublic_spline.calc_curvature(i));
+    traj_info.smoothed_path_s.push_back(i);
+
+    tmp_pose.pose.position.x = point_[0];
+    tmp_pose.pose.position.y = point_[1];
+    tmp_pose.pose.position.z = 0;
+    tmp_pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
+    smoothed_path.poses.push_back(tmp_pose);
+
+  }
+  return traj_info;
+}
+#endif //ROBOMASTER_CUBIC_SPLINE_ROS_H