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
Download Patch File Download Diff File Expand all files Collapse all files

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;