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 ---
target_link_libraries(dock_planner
${Boost_LIBRARIES} # Boost
visualization_msgs
nav_msgs
robot_visualization_msgs
robot_nav_msgs
tf3
tf3_geometry_msgs
robot_time

6
include/dock_planner/dock_calc.h
View File

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

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

@ -1,14 +1,14 @@
#ifndef COLOR_H_
#define COLOR_H_
#include <std_msgs/ColorRGBA.h>
#include <robot_std_msgs/ColorRGBA.h>
namespace agv_visualization
{
class Color : public std_msgs::ColorRGBA
class Color : public robot_std_msgs::ColorRGBA
{
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, double alpha) : Color() {
r = red;

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

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

4
src/dock_planner.cpp
View File

@ -63,7 +63,7 @@ namespace dock_planner
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);
@ -114,7 +114,7 @@ namespace dock_planner
// 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.frame_id = frame_id_;
// 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;
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;
}
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;
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.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.r = 1;
waypoints_marker.color.g = 1;
@ -162,13 +162,13 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(std::vector<Eigen::Ve
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.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.a = 0.75;
waypoints_marker.ns = name;
@ -189,13 +189,13 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(std::vector<Eigen::V
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.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.r = 1;
waypoints_marker.color.g = 1;
@ -218,13 +218,13 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(EigenTrajectoryPoint:
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.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.r = r;
// waypoints_marker.color.g = g;
@ -248,7 +248,7 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(EigenTrajectoryPoint
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;
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;
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;
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;
}
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;
nurbs_curve_result.header.frame_id = frame_id;
@ -873,10 +873,10 @@ robot_geometry_msgs::Point CurveCommon::CalculateDerivativeCurvePoint(Spline_Inf
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;
nav_msgs::Path bspline_derivative_result;
robot_nav_msgs::Path bspline_derivative_result;
robot_geometry_msgs::PoseStamped current_pose;
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;
}
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;
nav_msgs::Path derivative_basis_result;
robot_nav_msgs::Path derivative_basis_result;
robot_geometry_msgs::PoseStamped current_pose;
derivative_basis_result.header.frame_id = frame_id;