82 lines
4.8 KiB
C++
82 lines
4.8 KiB
C++
#ifndef CURVE_COMMON_H_
|
|
#define CURVE_COMMON_H_
|
|
|
|
#include <Eigen/Eigen>
|
|
#include <robot_nav_msgs/Path.h>
|
|
#include <robot_geometry_msgs/Point.h>
|
|
#include <robot_visualization_msgs/Marker.h>
|
|
#include "color.h"
|
|
#include <robot/robot.h>
|
|
|
|
struct Spline_Inf
|
|
{
|
|
int order;
|
|
std::vector<double> knot_vector;
|
|
std::vector<double> weight;
|
|
//std::vector<double> N;
|
|
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > control_point;
|
|
std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > N;
|
|
std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > dN;
|
|
std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > ddN;
|
|
std::vector<double> N_double_vec;
|
|
std::vector<double> R_double_vec;
|
|
std::vector<double> dR_double_vec;
|
|
std::vector<double> ddR_double_vec;
|
|
// std::vector<double> ddN_double_vec;
|
|
//Eigen::Matrix<Eigen::VectorXd, Eigen::Dynamic, Eigen::Dynamic> N; //bsaic function
|
|
Eigen::MatrixXd curvefit_N;
|
|
|
|
};
|
|
|
|
struct EigenTrajectoryPoint
|
|
{
|
|
typedef std::vector<EigenTrajectoryPoint, Eigen::aligned_allocator<EigenTrajectoryPoint> > Vector;
|
|
double u_data;
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
Eigen::Vector3d position;
|
|
//Eigen::VectorXd u_data;
|
|
};
|
|
|
|
class CurveCommon
|
|
{
|
|
public:
|
|
CurveCommon();
|
|
robot_nav_msgs::Path Generate_Line(robot_geometry_msgs::Point start_point, robot_geometry_msgs::Point end_point, double t_intervel, std::string frame_id);
|
|
robot_nav_msgs::Path Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id);
|
|
robot_nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id);
|
|
robot_nav_msgs::Path Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id);
|
|
robot_nav_msgs::Path Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id);
|
|
robot_nav_msgs::Path Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id);
|
|
|
|
void CalculateDerivativeBasisFunc(Spline_Inf* spline_inf, double u_data, int differential_times);
|
|
robot_geometry_msgs::Point CalculateDerivativeCurvePoint(Spline_Inf* spline_inf, double u_data, int differential_times, bool UsingNURBS);
|
|
robot_geometry_msgs::Point CalculateCurvePoint(Spline_Inf* spline_inf, double u_data, bool UsingNURBS);
|
|
double CalculateCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
|
|
double CalculateSignedCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
|
|
Eigen::Vector3d CalculateCurvatureDirectionVector(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
|
|
double CalculateCurvatureRadius(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
|
|
double CalculateCurveLength(Spline_Inf spline_inf, double start_u, double end_u, int sub_intervals, bool UsingNURBS);
|
|
bool curveIsValid(int degree, const std::vector<double>& knot_vector,
|
|
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>& control_points);
|
|
|
|
void ReadSplineInf(Spline_Inf* bspline_inf, int order, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > control_point, std::vector<double> knot_vector);
|
|
void ReadSplineInf(Spline_Inf* bspline_inf, std::vector<double> weight_vector, bool use_limit_derivative_fitting);
|
|
void ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector* input_point, std::vector<double> file_discreate_point);
|
|
void ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >* input_point, std::vector<double> file_discreate_point);
|
|
void ShowDiscreatePoint(robot_visualization_msgs::Marker* points, EigenTrajectoryPoint::Vector discreate_point);
|
|
void ShowDiscreatePoint(robot_visualization_msgs::Marker* points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point);
|
|
|
|
//TODO: move relate visualize function to new vislization.h
|
|
int print();
|
|
robot_visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale);
|
|
robot_visualization_msgs::Marker ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, robot_std_msgs::ColorRGBA point_color, const std::string& name, double scale);
|
|
robot_visualization_msgs::Marker ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, const std::string& name, double scale);
|
|
robot_visualization_msgs::Marker ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, robot_std_msgs::ColorRGBA point_color, const std::string& name, double scale);
|
|
private:
|
|
|
|
|
|
};
|
|
|
|
#endif //CURVE_COMMON_H_
|