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

update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 09:11:02 +07:00
parent c5c55147b7
commit 92264eaffc
8 changed files with 141 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
include/dock_planner/dock_calc.h
View File

@@ -1,8 +1,8 @@
#ifndef DOCK_CALC_H
#define DOCK_CALC_H
#include <nav_2d_utils/footprint.h>
#include <geometry_msgs/PoseStamped.h>
#include <robot_nav_2d_utils/footprint.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
@@ -39,10 +39,10 @@ namespace dock_planner
Spline_Inf* input_spline_inf;
CurveCommon* CurveDesign;
vector<geometry_msgs::Pose> posesOnPathWay;
vector<geometry_msgs::Point> footprint_robot_;
geometry_msgs::PoseStamped save_goal_;
// std::vector<geometry_msgs::Pose> free_zone_;
vector<robot_geometry_msgs::Pose> posesOnPathWay;
vector<robot_geometry_msgs::Point> footprint_robot_;
robot_geometry_msgs::PoseStamped save_goal_;
// std::vector<robot_geometry_msgs::Pose> free_zone_;
bool check_goal_;
int store_start_nearest_idx_;
@@ -50,7 +50,7 @@ namespace dock_planner
double min_distance_to_goal_;
// Tính khoảng cách giữa 2 điểm
inline double calc_distance(const geometry_msgs::Pose& p1, const geometry_msgs::Pose& p2)
inline double calc_distance(const robot_geometry_msgs::Pose& p1, const robot_geometry_msgs::Pose& p2)
{
return std::hypot(p1.position.x - p2.position.x, p1.position.y - p2.position.y);
}
@@ -80,42 +80,42 @@ namespace dock_planner
int degree,
double initial_step = 0.02);
geometry_msgs::Pose offsetPoint(const geometry_msgs::Pose& A,const double& theta, const double& d);
robot_geometry_msgs::Pose offsetPoint(const robot_geometry_msgs::Pose& A,const double& theta, const double& d);
double pointToSegmentDistance(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C);
double pointToSegmentDistance(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C);
double signedAngle(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C);
double signedAngle(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C);
double compute_t(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C);
double compute_t(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C);
geometry_msgs::Pose compute_D(const geometry_msgs::Pose& B, const geometry_msgs::Pose& C,const double& t);
robot_geometry_msgs::Pose compute_D(const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C,const double& t);
void updatePoseOrientation(std::vector<geometry_msgs::Pose>& saved_poses,
std::vector<geometry_msgs::Pose>& nurbs_path,
void updatePoseOrientation(std::vector<robot_geometry_msgs::Pose>& saved_poses,
std::vector<robot_geometry_msgs::Pose>& nurbs_path,
bool reverse = true);
bool findPathTMP(const vector<geometry_msgs::Pose>& control_points,
vector<geometry_msgs::Pose>& saved_poses,
bool findPathTMP(const vector<robot_geometry_msgs::Pose>& control_points,
vector<robot_geometry_msgs::Pose>& saved_poses,
const int& degree, double step = 0.02);
bool findNURBSControlPoints(int& dir, vector<geometry_msgs::Pose>& control_points,
const geometry_msgs::Pose& start_pose,
const std::vector<geometry_msgs::Pose>& posesOnPathWay,
bool findNURBSControlPoints(int& dir, vector<robot_geometry_msgs::Pose>& control_points,
const robot_geometry_msgs::Pose& start_pose,
const std::vector<robot_geometry_msgs::Pose>& posesOnPathWay,
const double& angle_threshol, const int& degree);
int findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Pose>& posesOnPathWay,
int findNearestPointOnPath(int& dir, const robot_geometry_msgs::Pose& pose,
const std::vector<robot_geometry_msgs::Pose>& posesOnPathWay,
const double& angle_threshol);
bool isFreeSpace(const geometry_msgs::Pose& pose, const std::vector<geometry_msgs::Point>& footprint);
bool isFreeSpace(const robot_geometry_msgs::Pose& pose, const std::vector<robot_geometry_msgs::Point>& footprint);
std::vector<geometry_msgs::Point> calcFreeZone(const geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Point>& footprint);
std::vector<robot_geometry_msgs::Point> calcFreeZone(const robot_geometry_msgs::Pose& pose,
const std::vector<robot_geometry_msgs::Point>& footprint);
std::vector<geometry_msgs::Point> interpolateFootprint(const geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Point>& footprint, const double& resolution);
std::vector<robot_geometry_msgs::Point> interpolateFootprint(const robot_geometry_msgs::Pose& pose,
const std::vector<robot_geometry_msgs::Point>& footprint, const double& resolution);
RobotGeometry computeGeometry(const std::vector<geometry_msgs::Point>& fp);
RobotGeometry computeGeometry(const std::vector<robot_geometry_msgs::Point>& fp);
public:
@@ -130,12 +130,12 @@ namespace dock_planner
~DockCalc();
bool makePlanMoveToDock(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
const vector<geometry_msgs::Point>& footprint_robot,
const vector<geometry_msgs::Point>& footprint_dock,
bool makePlanMoveToDock(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
const vector<robot_geometry_msgs::Point>& footprint_robot,
const vector<robot_geometry_msgs::Point>& footprint_dock,
const int& degree, const double& step,
vector<geometry_msgs::Pose>& planMoveToDock,
vector<robot_geometry_msgs::Pose>& planMoveToDock,
bool reverse = false);

10
include/dock_planner/dock_planner.h
View File

@@ -15,9 +15,9 @@ namespace dock_planner {
bool initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot);
bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan);
bool makePlan(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
std::vector<robot_geometry_msgs::PoseStamped>& plan);
private:
// Core components
@@ -26,7 +26,7 @@ namespace dock_planner {
bool initialized_;
std::string frame_id_;
// ros::Publisher plan_pub_;
std::vector<geometry_msgs::Point> footprint_dock_;
std::vector<robot_geometry_msgs::Point> footprint_dock_;
int cost_threshold_;
double calib_safety_distance_;
@@ -35,7 +35,7 @@ namespace dock_planner {
DockCalc calc_plan_to_dock_;
void publishPlan(const std::vector<geometry_msgs::PoseStamped>& plan);
void publishPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan);
};
} // namespace dock_planner

10
include/dock_planner/utils/conversion.h
View File

@@ -3,14 +3,14 @@
#include "curve_common.h"
#include <Eigen/Geometry>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_geometry_msgs/Quaternion.h>
inline Eigen::Vector3d EigenVecter3dFromPointMsg(const geometry_msgs::Point& msg) {
inline Eigen::Vector3d EigenVecter3dFromPointMsg(const robot_geometry_msgs::Point& msg) {
return Eigen::Vector3d(msg.x, msg.y, msg.z);
}
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::PoseStamped> &plan)
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<robot_geometry_msgs::PoseStamped> &plan)
{
EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
EigenTrajectoryPoint eigen_trajectory_point;
@@ -25,7 +25,7 @@ inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::v
return eigen_trajectory_point_vec;
}
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<geometry_msgs::Point> &discreate_point_vec)
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<robot_geometry_msgs::Point> &discreate_point_vec)
{
EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
EigenTrajectoryPoint eigen_trajectory_point;

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

@@ -3,7 +3,7 @@
#include <Eigen/Eigen>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Point.h>
#include <robot_geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>
#include "color.h"
#include <robot/console.h>
@@ -43,7 +43,7 @@ class CurveCommon
{
public:
CurveCommon();
nav_msgs::Path Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, double t_intervel, std::string frame_id);
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);
@@ -51,8 +51,8 @@ public:
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);
geometry_msgs::Point CalculateDerivativeCurvePoint(Spline_Inf* spline_inf, double u_data, int differential_times, bool UsingNURBS);
geometry_msgs::Point CalculateCurvePoint(Spline_Inf* spline_inf, double u_data, bool UsingNURBS);
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);