DuongTD/custom_planner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

hiep sua ten file

Browse Source
This commit is contained in:
HiepLM 2025-12-30 09:57:13 +07:00
parent 6d55c6c4be
commit d6512018ef
6 changed files with 48 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -44,8 +44,8 @@ add_library(custom_planner
# --- Link các thư viện phụ thuộc --- # --- Link các thư viện phụ thuộc ---
target_link_libraries(custom_planner target_link_libraries(custom_planner
${Boost_LIBRARIES} # Boost ${Boost_LIBRARIES} # Boost
visualization_msgs robot_visualization_msgs
nav_msgs robot_nav_msgs
tf3 tf3
tf3_geometry_msgs tf3_geometry_msgs
robot_time robot_time

28
include/custom_planner/Curve_common.h
View File

@ -7,9 +7,9 @@
// #include <deque> // #include <deque>
// #include <iostream> // #include <iostream>
#include <nav_msgs/Path.h> #include <robot_nav_msgs/Path.h>
#include <robot_geometry_msgs/Point.h> #include <robot_geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h> #include <robot_visualization_msgs/Marker.h>
#include "custom_planner/color.h" #include "custom_planner/color.h"
struct Spline_Inf struct Spline_Inf
@ -47,12 +47,12 @@ class Curve_common
{ {
public: public:
Curve_common(); Curve_common();
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_Line(robot_geometry_msgs::Point start_point, robot_geometry_msgs::Point end_point, double t_intervel, std::string frame_id);
nav_msgs::Path Generate_BezierCurve(EigenTrajectoryPoint::Vector control_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);
nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id); robot_nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id);
nav_msgs::Path Generate_NURBSCurve(Spline_Inf spline_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);
nav_msgs::Path Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, 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);
nav_msgs::Path Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, 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); 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 CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS);
@ -67,15 +67,15 @@ class Curve_common
void ReadSplineInf(Spline_Inf *bspline_inf, std::vector<double> weight_vector, bool use_limit_derivative_fitting); 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(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 ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > *input_point, std::vector<double> file_discreate_point);
void ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point); void ShowDiscreatePoint(robot_visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point);
void ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > 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 //TODO: move relate visualize function to new vislization.h
int print(); int print();
visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale); robot_visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale);
visualization_msgs::Marker ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, 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);
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 ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, const std::string& name, double scale);
visualization_msgs::Marker ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, 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: private:

6
include/custom_planner/color.h
View File

@ -1,14 +1,14 @@
#ifndef CUSTOM_PLANNER_COLOR_H_ #ifndef CUSTOM_PLANNER_COLOR_H_
#define CUSTOM_PLANNER_COLOR_H_ #define CUSTOM_PLANNER_COLOR_H_
#include <std_msgs/ColorRGBA.h> #include <robot_std_msgs/ColorRGBA.h>
namespace agv_visualization namespace agv_visualization
{ {
class Color : public std_msgs::ColorRGBA class Color : public robot_std_msgs::ColorRGBA
{ {
public: public:
Color() : std_msgs::ColorRGBA() {} Color() : robot_std_msgs::ColorRGBA() {}
Color(double red, double green, double blue) : Color(red, green, blue, 1.0) {} Color(double red, double green, double blue) : Color(red, green, blue, 1.0) {}
Color(double red, double green, double blue, double alpha) : Color() { Color(double red, double green, double blue, double alpha) : Color() {
r = red; r = red;

2
include/custom_planner/custom_planner.h
View File

@ -10,7 +10,7 @@ using namespace std;
// ROS // ROS
#include <robot_geometry_msgs/PoseStamped.h> #include <robot_geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h> #include <robot_visualization_msgs/Marker.h>
// Costmap used for the map representation // Costmap used for the map representation
#include <costmap_2d/costmap_2d_robot.h> #include <costmap_2d/costmap_2d_robot.h>

52
src/Curve_common.cpp
View File

@ -10,9 +10,9 @@ Curve_common::Curve_common()
} }
nav_msgs::Path Curve_common::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 Curve_common::Generate_Line(robot_geometry_msgs::Point start_point, robot_geometry_msgs::Point end_point, double t_intervel, std::string frame_id)
{ {
nav_msgs::Path line_result; robot_nav_msgs::Path line_result;
robot_geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
line_result.header.frame_id = frame_id; line_result.header.frame_id = frame_id;
@ -38,9 +38,9 @@ nav_msgs::Path Curve_common::Generate_Line(robot_geometry_msgs::Point start_poin
return line_result; return line_result;
} }
nav_msgs::Path Curve_common::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id) robot_nav_msgs::Path Curve_common::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id)
{ {
nav_msgs::Path bezier_curve_result; robot_nav_msgs::Path bezier_curve_result;
robot_geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
EigenTrajectoryPoint::Vector temp_control_point_vec; EigenTrajectoryPoint::Vector temp_control_point_vec;
@ -136,13 +136,13 @@ void Curve_common::ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, E
// } // }
} }
visualization_msgs::Marker Curve_common::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 Curve_common::ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, const std::string& name, double scale)
{ {
visualization_msgs::Marker waypoints_marker; robot_visualization_msgs::Marker waypoints_marker;
waypoints_marker.header.frame_id = frame_id; waypoints_marker.header.frame_id = frame_id;
waypoints_marker.header.stamp = robot::Time::now(); waypoints_marker.header.stamp = robot::Time::now();
waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST; waypoints_marker.type = robot_visualization_msgs::Marker::SPHERE_LIST;
//waypoints_marker.color = color; //waypoints_marker.color = color;
waypoints_marker.color.r = 1; waypoints_marker.color.r = 1;
waypoints_marker.color.g = 1; waypoints_marker.color.g = 1;
@ -165,13 +165,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(std::vector<Eigen::V
return waypoints_marker; return waypoints_marker;
} }
visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale) robot_visualization_msgs::Marker Curve_common::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)
{ {
visualization_msgs::Marker waypoints_marker; robot_visualization_msgs::Marker waypoints_marker;
waypoints_marker.header.frame_id = frame_id; waypoints_marker.header.frame_id = frame_id;
waypoints_marker.header.stamp = robot::Time::now(); waypoints_marker.header.stamp = robot::Time::now();
waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST; waypoints_marker.type = robot_visualization_msgs::Marker::SPHERE_LIST;
waypoints_marker.color = point_color; waypoints_marker.color = point_color;
waypoints_marker.color.a = 0.75; waypoints_marker.color.a = 0.75;
waypoints_marker.ns = name; waypoints_marker.ns = name;
@ -192,13 +192,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(std::vector<Eigen::
return waypoints_marker; return waypoints_marker;
} }
visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale) robot_visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale)
{ {
visualization_msgs::Marker waypoints_marker; robot_visualization_msgs::Marker waypoints_marker;
waypoints_marker.header.frame_id = frame_id; waypoints_marker.header.frame_id = frame_id;
waypoints_marker.header.stamp = robot::Time::now(); waypoints_marker.header.stamp = robot::Time::now();
waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST; waypoints_marker.type = robot_visualization_msgs::Marker::SPHERE_LIST;
//waypoints_marker.color = color; //waypoints_marker.color = color;
waypoints_marker.color.r = 1; waypoints_marker.color.r = 1;
waypoints_marker.color.g = 1; waypoints_marker.color.g = 1;
@ -221,13 +221,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint
return waypoints_marker; return waypoints_marker;
} }
visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale) robot_visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, robot_std_msgs::ColorRGBA point_color, const std::string& name, double scale)
{ {
visualization_msgs::Marker waypoints_marker; robot_visualization_msgs::Marker waypoints_marker;
waypoints_marker.header.frame_id = frame_id; waypoints_marker.header.frame_id = frame_id;
waypoints_marker.header.stamp = robot::Time::now(); waypoints_marker.header.stamp = robot::Time::now();
waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST; waypoints_marker.type = robot_visualization_msgs::Marker::SPHERE_LIST;
waypoints_marker.color = point_color; waypoints_marker.color = point_color;
// waypoints_marker.color.r = r; // waypoints_marker.color.r = r;
// waypoints_marker.color.g = g; // waypoints_marker.color.g = g;
@ -251,7 +251,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoin
return waypoints_marker; return waypoints_marker;
} }
void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point) void Curve_common::ShowDiscreatePoint(robot_visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point)
{ {
robot_geometry_msgs::Point view_point; robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++) for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
@ -262,7 +262,7 @@ void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenT
} }
} }
void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point) void Curve_common::ShowDiscreatePoint(robot_visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point)
{ {
robot_geometry_msgs::Point view_point; robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++) for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
@ -326,9 +326,9 @@ void Curve_common::ReadSplineInf(Spline_Inf *spline_inf, std::vector<double> wei
// } // }
} }
nav_msgs::Path Curve_common::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id) robot_nav_msgs::Path Curve_common::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id)
{ {
nav_msgs::Path bspline_curve_result; robot_nav_msgs::Path bspline_curve_result;
robot_geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
bspline_curve_result.header.frame_id = frame_id; bspline_curve_result.header.frame_id = frame_id;
@ -500,9 +500,9 @@ nav_msgs::Path Curve_common::Generate_BsplineCurve(Spline_Inf bspline_inf, doubl
return bspline_curve_result; return bspline_curve_result;
} }
nav_msgs::Path Curve_common::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id) robot_nav_msgs::Path Curve_common::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id)
{ {
nav_msgs::Path nurbs_curve_result; robot_nav_msgs::Path nurbs_curve_result;
robot_geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
nurbs_curve_result.header.frame_id = frame_id; nurbs_curve_result.header.frame_id = frame_id;
@ -878,10 +878,10 @@ robot_geometry_msgs::Point Curve_common::CalculateDerivativeCurvePoint(Spline_In
return derivative_curve_point; return derivative_curve_point;
} }
nav_msgs::Path Curve_common::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id) robot_nav_msgs::Path Curve_common::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id)
{ {
robot_geometry_msgs::Point derivative_point_result; robot_geometry_msgs::Point derivative_point_result;
nav_msgs::Path bspline_derivative_result; robot_nav_msgs::Path bspline_derivative_result;
robot_geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
bspline_derivative_result.header.frame_id = frame_id; bspline_derivative_result.header.frame_id = frame_id;
@ -921,10 +921,10 @@ nav_msgs::Path Curve_common::Generate_DerivativeBsplineCurve(Spline_Inf bspline_
return bspline_derivative_result; return bspline_derivative_result;
} }
nav_msgs::Path Curve_common::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id) robot_nav_msgs::Path Curve_common::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id)
{ {
robot_geometry_msgs::Point derivative_point_result; robot_geometry_msgs::Point derivative_point_result;
nav_msgs::Path derivative_basis_result; robot_nav_msgs::Path derivative_basis_result;
robot_geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
derivative_basis_result.header.frame_id = frame_id; derivative_basis_result.header.frame_id = frame_id;

4
src/custom_planner.cpp
View File

@ -1,5 +1,5 @@
#include <custom_planner/custom_planner.h> #include <custom_planner/custom_planner.h>
#include <nav_msgs/Path.h> #include <robot_nav_msgs/Path.h>
#include <robot_geometry_msgs/Point.h> #include <robot_geometry_msgs/Point.h>
#include <costmap_2d/inflation_layer.h> #include <costmap_2d/inflation_layer.h>
#include <tf3/LinearMath/Quaternion.h> #include <tf3/LinearMath/Quaternion.h>
@ -336,7 +336,7 @@ namespace custom_planner
} }
// Publish plan for visualization // Publish plan for visualization
nav_msgs::Path gui_path; robot_nav_msgs::Path gui_path;
gui_path.header.frame_id = costmap_robot_->getGlobalFrameID(); gui_path.header.frame_id = costmap_robot_->getGlobalFrameID();
gui_path.header.stamp = plan_time; gui_path.header.stamp = plan_time;
gui_path.poses = plan; gui_path.poses = plan;