DuongTD/dock_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:59:48 +07:00
parent 92264eaffc
commit 9c84e64253
6 changed files with 50 additions and 50 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(dock_planner
# --- Link các thư viện phụ thuộc --- # --- Link các thư viện phụ thuộc ---
target_link_libraries(dock_planner target_link_libraries(dock_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

6
include/dock_planner/dock_calc.h
View File

@ -4,9 +4,9 @@
#include <robot_nav_2d_utils/footprint.h> #include <robot_nav_2d_utils/footprint.h>
#include <robot_geometry_msgs/PoseStamped.h> #include <robot_geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h> #include <robot_nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h> #include <robot_nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GetPlan.h> #include <robot_nav_msgs/GetPlan.h>
#include <costmap_2d/costmap_2d_robot.h> #include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/costmap_2d.h> #include <costmap_2d/costmap_2d.h>

6
include/dock_planner/utils/color.h
View File

@ -1,14 +1,14 @@
#ifndef COLOR_H_ #ifndef COLOR_H_
#define COLOR_H_ #define 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;

28
include/dock_planner/utils/curve_common.h
View File

@ -2,9 +2,9 @@
#define CURVE_COMMON_H_ #define CURVE_COMMON_H_
#include <Eigen/Eigen> #include <Eigen/Eigen>
#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 "color.h" #include "color.h"
#include <robot/console.h> #include <robot/console.h>
@ -43,12 +43,12 @@ class CurveCommon
{ {
public: public:
CurveCommon(); CurveCommon();
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);
@ -65,15 +65,15 @@ public:
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:

4
src/dock_planner.cpp
View File

@ -63,7 +63,7 @@ namespace dock_planner
calc_plan_to_dock_.calib_safety_distance_ = calib_safety_distance_; calc_plan_to_dock_.calib_safety_distance_ = calib_safety_distance_;
// plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1); // plan_pub_ = private_nh.advertise<robot_nav_msgs::Path>("plan", 1);
robot_nav_2d_msgs::Polygon2D footprint_dock ;//= robot_nav_2d_utils::footprintFromParams(private_nh,false); robot_nav_2d_msgs::Polygon2D footprint_dock ;//= robot_nav_2d_utils::footprintFromParams(private_nh,false);
@ -114,7 +114,7 @@ namespace dock_planner
// void DockPlanner::publishPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) // void DockPlanner::publishPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan)
// { // {
// nav_msgs::Path path_msg; // robot_nav_msgs::Path path_msg;
// path_msg.header.stamp = robot::Time::now(); // path_msg.header.stamp = robot::Time::now();
// path_msg.header.frame_id = frame_id_; // path_msg.header.frame_id = frame_id_;
// path_msg.poses = plan; // path_msg.poses = plan;

52
src/utils/curve_common.cpp
View File

@ -7,9 +7,9 @@ CurveCommon::CurveCommon()
{ {
} }
nav_msgs::Path CurveCommon::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 CurveCommon::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;
@ -35,9 +35,9 @@ nav_msgs::Path CurveCommon::Generate_Line(robot_geometry_msgs::Point start_point
return line_result; return line_result;
} }
nav_msgs::Path CurveCommon::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id) robot_nav_msgs::Path CurveCommon::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;
@ -133,13 +133,13 @@ void CurveCommon::ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, Ei
// } // }
} }
visualization_msgs::Marker CurveCommon::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 CurveCommon::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;
@ -162,13 +162,13 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(std::vector<Eigen::Ve
return waypoints_marker; return waypoints_marker;
} }
visualization_msgs::Marker CurveCommon::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 CurveCommon::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;
@ -189,13 +189,13 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(std::vector<Eigen::V
return waypoints_marker; return waypoints_marker;
} }
visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(EigenTrajectoryPoint::Vector &discreate_point, const std::string &frame_id, const std::string &name, double scale) robot_visualization_msgs::Marker CurveCommon::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;
@ -218,13 +218,13 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(EigenTrajectoryPoint:
return waypoints_marker; return waypoints_marker;
} }
visualization_msgs::Marker CurveCommon::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 CurveCommon::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;
@ -248,7 +248,7 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(EigenTrajectoryPoint
return waypoints_marker; return waypoints_marker;
} }
void CurveCommon::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point) void CurveCommon::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 < discreate_point.size(); i++) for (int i = 0; i < discreate_point.size(); i++)
@ -259,7 +259,7 @@ void CurveCommon::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTr
} }
} }
void CurveCommon::ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> discreate_point) void CurveCommon::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 < discreate_point.size(); i++) for (int i = 0; i < discreate_point.size(); i++)
@ -322,9 +322,9 @@ void CurveCommon::ReadSplineInf(Spline_Inf *spline_inf, std::vector<double> weig
// } // }
} }
nav_msgs::Path CurveCommon::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id) robot_nav_msgs::Path CurveCommon::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;
@ -495,9 +495,9 @@ nav_msgs::Path CurveCommon::Generate_BsplineCurve(Spline_Inf bspline_inf, double
return bspline_curve_result; return bspline_curve_result;
} }
nav_msgs::Path CurveCommon::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id) robot_nav_msgs::Path CurveCommon::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;
@ -873,10 +873,10 @@ robot_geometry_msgs::Point CurveCommon::CalculateDerivativeCurvePoint(Spline_Inf
return derivative_curve_point; return derivative_curve_point;
} }
nav_msgs::Path CurveCommon::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id) robot_nav_msgs::Path CurveCommon::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;
@ -916,10 +916,10 @@ nav_msgs::Path CurveCommon::Generate_DerivativeBsplineCurve(Spline_Inf bspline_i
return bspline_derivative_result; return bspline_derivative_result;
} }
nav_msgs::Path CurveCommon::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id) robot_nav_msgs::Path CurveCommon::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;