diff --git a/CMakeLists.txt b/CMakeLists.txt index ffe6962..16b9916 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,7 +53,7 @@ target_link_libraries(dock_planner costmap_2d nav_core robot_cpp - nav_2d_utils + robot_nav_2d_utils ) # --- Include directories cho target --- diff --git a/include/dock_planner/dock_calc.h b/include/dock_planner/dock_calc.h index 42c2a33..da534f9 100644 --- a/include/dock_planner/dock_calc.h +++ b/include/dock_planner/dock_calc.h @@ -1,8 +1,8 @@ #ifndef DOCK_CALC_H #define DOCK_CALC_H -#include -#include +#include +#include #include #include @@ -39,10 +39,10 @@ namespace dock_planner Spline_Inf* input_spline_inf; CurveCommon* CurveDesign; - vector posesOnPathWay; - vector footprint_robot_; - geometry_msgs::PoseStamped save_goal_; - // std::vector free_zone_; + vector posesOnPathWay; + vector footprint_robot_; + robot_geometry_msgs::PoseStamped save_goal_; + // std::vector 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& saved_poses, - std::vector& nurbs_path, + void updatePoseOrientation(std::vector& saved_poses, + std::vector& nurbs_path, bool reverse = true); - bool findPathTMP(const vector& control_points, - vector& saved_poses, + bool findPathTMP(const vector& control_points, + vector& saved_poses, const int& degree, double step = 0.02); - bool findNURBSControlPoints(int& dir, vector& control_points, - const geometry_msgs::Pose& start_pose, - const std::vector& posesOnPathWay, + bool findNURBSControlPoints(int& dir, vector& control_points, + const robot_geometry_msgs::Pose& start_pose, + const std::vector& posesOnPathWay, const double& angle_threshol, const int& degree); - int findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose, - const std::vector& posesOnPathWay, + int findNearestPointOnPath(int& dir, const robot_geometry_msgs::Pose& pose, + const std::vector& posesOnPathWay, const double& angle_threshol); - bool isFreeSpace(const geometry_msgs::Pose& pose, const std::vector& footprint); + bool isFreeSpace(const robot_geometry_msgs::Pose& pose, const std::vector& footprint); - std::vector calcFreeZone(const geometry_msgs::Pose& pose, - const std::vector& footprint); + std::vector calcFreeZone(const robot_geometry_msgs::Pose& pose, + const std::vector& footprint); - std::vector interpolateFootprint(const geometry_msgs::Pose& pose, - const std::vector& footprint, const double& resolution); + std::vector interpolateFootprint(const robot_geometry_msgs::Pose& pose, + const std::vector& footprint, const double& resolution); - RobotGeometry computeGeometry(const std::vector& fp); + RobotGeometry computeGeometry(const std::vector& fp); public: @@ -130,12 +130,12 @@ namespace dock_planner ~DockCalc(); - bool makePlanMoveToDock(const geometry_msgs::PoseStamped& start, - const geometry_msgs::PoseStamped& goal, - const vector& footprint_robot, - const vector& footprint_dock, + bool makePlanMoveToDock(const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, + const vector& footprint_robot, + const vector& footprint_dock, const int& degree, const double& step, - vector& planMoveToDock, + vector& planMoveToDock, bool reverse = false); diff --git a/include/dock_planner/dock_planner.h b/include/dock_planner/dock_planner.h index d69c632..e59fa60 100644 --- a/include/dock_planner/dock_planner.h +++ b/include/dock_planner/dock_planner.h @@ -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& plan); + bool makePlan(const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, + std::vector& plan); private: // Core components @@ -26,7 +26,7 @@ namespace dock_planner { bool initialized_; std::string frame_id_; // ros::Publisher plan_pub_; - std::vector footprint_dock_; + std::vector 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& plan); + void publishPlan(const std::vector& plan); }; } // namespace dock_planner diff --git a/include/dock_planner/utils/conversion.h b/include/dock_planner/utils/conversion.h index b70d885..216b999 100644 --- a/include/dock_planner/utils/conversion.h +++ b/include/dock_planner/utils/conversion.h @@ -3,14 +3,14 @@ #include "curve_common.h" #include -#include -#include +#include +#include -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 &plan) +inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector &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 &discreate_point_vec) +inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector &discreate_point_vec) { EigenTrajectoryPoint::Vector eigen_trajectory_point_vec; EigenTrajectoryPoint eigen_trajectory_point; diff --git a/include/dock_planner/utils/curve_common.h b/include/dock_planner/utils/curve_common.h index 3ff0dee..440010f 100644 --- a/include/dock_planner/utils/curve_common.h +++ b/include/dock_planner/utils/curve_common.h @@ -3,7 +3,7 @@ #include #include -#include +#include #include #include "color.h" #include @@ -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); diff --git a/src/dock_calc.cpp b/src/dock_calc.cpp index b643908..6d597ea 100644 --- a/src/dock_calc.cpp +++ b/src/dock_calc.cpp @@ -18,15 +18,15 @@ namespace dock_planner delete input_spline_inf; } - geometry_msgs::Pose DockCalc::offsetPoint(const geometry_msgs::Pose& A, const double& theta, const double& d) + robot_geometry_msgs::Pose DockCalc::offsetPoint(const robot_geometry_msgs::Pose& A, const double& theta, const double& d) { - geometry_msgs::Pose B; + robot_geometry_msgs::Pose B; B.position.x = A.position.x + d * std::cos(theta); B.position.y = A.position.y + d * std::sin(theta); return B; } - double DockCalc::pointToSegmentDistance(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C) + double DockCalc::pointToSegmentDistance(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C) { double theta = signedAngle(A,B,C); double sin_theta = sin(fabs(theta)); @@ -35,7 +35,7 @@ namespace dock_planner return distance; } - double DockCalc::signedAngle(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C) + double DockCalc::signedAngle(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C) { double ABx = B.position.x - A.position.x; double ABy = B.position.y - A.position.y; @@ -50,7 +50,7 @@ namespace dock_planner return angle; } - double DockCalc::compute_t(const geometry_msgs::Pose& A, const geometry_msgs::Pose& B, const geometry_msgs::Pose& C) + double DockCalc::compute_t(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C) { constexpr double epsilon = 1e-6; double dx = C.position.x - B.position.x; @@ -69,15 +69,15 @@ namespace dock_planner return -numerator / denominator; } - geometry_msgs::Pose DockCalc::compute_D(const geometry_msgs::Pose& B, const geometry_msgs::Pose& C, const double& t) + robot_geometry_msgs::Pose DockCalc::compute_D(const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C, const double& t) { - geometry_msgs::Pose D; + robot_geometry_msgs::Pose D; D.position.x = B.position.x + t * (C.position.x - B.position.x); D.position.y = B.position.y + t * (C.position.y - B.position.y); return D; } - RobotGeometry DockCalc::computeGeometry(const std::vector& fp) + RobotGeometry DockCalc::computeGeometry(const std::vector& fp) { double min_x = 1e9, max_x = -1e9; double min_y = 1e9, max_y = -1e9; @@ -99,12 +99,12 @@ namespace dock_planner return {length, width, radius}; } - bool DockCalc::makePlanMoveToDock(const geometry_msgs::PoseStamped& start, - const geometry_msgs::PoseStamped& goal, - const vector& footprint_robot, - const vector& footprint_dock, + bool DockCalc::makePlanMoveToDock(const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, + const vector& footprint_robot, + const vector& footprint_dock, const int& degree, const double& step, - vector& planMoveToDock, + vector& planMoveToDock, bool reverse) { constexpr double epsilon = 1e-6; @@ -141,10 +141,10 @@ namespace dock_planner } // Lambda: Generate NURBS path - auto generateNURBSPath = [&](const vector& control_points, bool check_isFreeSpace, - bool edge_end_plag, bool path_reverse) -> vector + auto generateNURBSPath = [&](const vector& control_points, bool check_isFreeSpace, + bool edge_end_plag, bool path_reverse) -> vector { - vector result; + vector result; bool obstacle_flag = false; // Convert to Eigen format @@ -180,14 +180,14 @@ namespace dock_planner // robot::log_error("[dock_calc] Adaptive step size: %f", step_new); // Generate path points - vector saved_poses; + vector saved_poses; for (double t = 0.0; t <= 1.0; t += step_new) { - geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true); + robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true); if (!std::isnan(point.x) && !std::isnan(point.y)) { - geometry_msgs::Pose pose; + robot_geometry_msgs::Pose pose; pose.position = point; pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0); @@ -261,9 +261,9 @@ namespace dock_planner min_distance_to_goal_ = (safety_distance_ + distance_dock / 2.0); // Calculate pose C (offset from goal) - geometry_msgs::Pose Goal_Pose = goal.pose; + robot_geometry_msgs::Pose Goal_Pose = goal.pose; double yaw_goal = normalizeAngle(data_convert::getYaw(goal.pose.orientation)); - geometry_msgs::Pose Pose_C = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_C); + robot_geometry_msgs::Pose Pose_C = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_C); // Compute t parameter and determine direction double t = compute_t(start.pose, goal.pose, Pose_C); @@ -288,7 +288,7 @@ namespace dock_planner } // Calculate pose D and distance AD - geometry_msgs::Pose Pose_D = compute_D(goal.pose, Pose_C, t); + robot_geometry_msgs::Pose Pose_D = compute_D(goal.pose, Pose_C, t); double distance_AD = pointToSegmentDistance(start.pose, goal.pose, Pose_C); // Calculate angle threshold with scaling @@ -308,13 +308,13 @@ namespace dock_planner double distance_DF = distance_AD / tan(angle_alpha); double distance_min_for_pose_F = distance_min_for_pose_C + distance_CD + distance_DF; - geometry_msgs::Pose Pose_F = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_F); - geometry_msgs::Pose mid_pose; + robot_geometry_msgs::Pose Pose_F = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_F); + robot_geometry_msgs::Pose mid_pose; mid_pose.position.x = (Pose_C.position.x + Pose_F.position.x) / 2.0; mid_pose.position.y = (Pose_C.position.y + Pose_F.position.y) / 2.0; - vector control_points_tmp = {Pose_C, mid_pose, Pose_F}; - vector saved_poses_tmp; + vector control_points_tmp = {Pose_C, mid_pose, Pose_F}; + vector saved_poses_tmp; if (!findPathTMP(control_points_tmp, saved_poses_tmp, degree, step)) @@ -325,7 +325,7 @@ namespace dock_planner // Find first segment control points int dir; - vector control_points_1; + vector control_points_1; if (!findNURBSControlPoints(dir, control_points_1, start.pose, saved_poses_tmp, angle_threshold, degree)) { robot::log_error("[dock_calc] Failed to find NURBS control points for first segment."); @@ -337,21 +337,21 @@ namespace dock_planner ((dir == 1 || dir == 0) && dir_robot_move_to_goal); // Generate first segment - vector first_segment = generateNURBSPath(control_points_1, true, false, first_segment_reverse); + vector first_segment = generateNURBSPath(control_points_1, true, false, first_segment_reverse); planMoveToDock.insert(planMoveToDock.end(), first_segment.begin(), first_segment.end()); //Generate second segment (to goal) if (!planMoveToDock.empty()) { - geometry_msgs::Pose mid_control_point; + robot_geometry_msgs::Pose mid_control_point; mid_control_point.position.x = (planMoveToDock.back().position.x + goal.pose.position.x) / 2.0; mid_control_point.position.y = (planMoveToDock.back().position.y + goal.pose.position.y) / 2.0; - vector control_points_2 = {planMoveToDock.back(), + vector control_points_2 = {planMoveToDock.back(), mid_control_point, goal.pose}; - vector second_segment = generateNURBSPath(control_points_2, true, true, dir_robot_move_to_goal); + vector second_segment = generateNURBSPath(control_points_2, true, true, dir_robot_move_to_goal); planMoveToDock.insert(planMoveToDock.end(), second_segment.begin(), second_segment.end()); planMoveToDock.insert(planMoveToDock.begin(),start.pose); planMoveToDock.insert(planMoveToDock.end(),goal.pose); @@ -360,8 +360,8 @@ namespace dock_planner return !planMoveToDock.empty(); } - bool DockCalc::findPathTMP(const vector& control_points, - vector& saved_poses, + bool DockCalc::findPathTMP(const vector& control_points, + vector& saved_poses, const int& degree, double step) { // Chuyển đổi control points sang định dạng Eigen::Vector3d @@ -393,15 +393,15 @@ namespace dock_planner CurveDesign->ReadSplineInf(input_spline_inf, degree + 1, eigen_control_points, knot_vector); CurveDesign->ReadSplineInf(input_spline_inf, weights, false); - // vector poses_tmp; + // vector poses_tmp; for(double t = 0.0; t <= 1.0; t += step) { // robot::log_error("[dock_calc][findPathTMP] t(%f) DEBUG 10", t); - geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true); + robot_geometry_msgs::Point point = CurveDesign->CalculateCurvePoint(input_spline_inf, t, true); if(!std::isnan(point.x) && !std::isnan(point.y)) { // robot::log_error("[dock_calc][findPathTMP] point(%f, %f) at t(%f)", point.x, point.y, t); - geometry_msgs::Pose pose; + robot_geometry_msgs::Pose pose; pose.position = point; pose.orientation.x = 0.0; pose.orientation.y = 0.0; @@ -432,8 +432,8 @@ namespace dock_planner return true; } - void DockCalc::updatePoseOrientation(std::vector& saved_poses, - std::vector& nurbs_path, + void DockCalc::updatePoseOrientation(std::vector& saved_poses, + std::vector& nurbs_path, bool reverse) { double yaw; @@ -465,7 +465,7 @@ namespace dock_planner double initial_step) { double length = 0.0; - geometry_msgs::Point prev_point, curr_point; + robot_geometry_msgs::Point prev_point, curr_point; std::vector segment_lengths; static double step; @@ -492,9 +492,9 @@ namespace dock_planner return length; } - bool DockCalc::findNURBSControlPoints(int& dir, vector& control_points, - const geometry_msgs::Pose& start_pose, - const std::vector& saved_poses, + bool DockCalc::findNURBSControlPoints(int& dir, vector& control_points, + const robot_geometry_msgs::Pose& start_pose, + const std::vector& saved_poses, const double& angle_threshold, const int& degree) { if (saved_poses.empty()) @@ -507,9 +507,9 @@ namespace dock_planner int idx = findNearestPointOnPath(dir, start_pose, saved_poses, angle_threshold); // Helper lambda to create pose at position - auto createPose = [](double x, double y, double yaw = 0.0) -> geometry_msgs::Pose + auto createPose = [](double x, double y, double yaw = 0.0) -> robot_geometry_msgs::Pose { - geometry_msgs::Pose pose; + robot_geometry_msgs::Pose pose; pose.position.x = x; pose.position.y = y; pose.orientation = data_convert::createQuaternionMsgFromYaw(0); @@ -584,8 +584,8 @@ namespace dock_planner return true; } - int DockCalc::findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose, - const std::vector& saved_poses, + int DockCalc::findNearestPointOnPath(int& dir, const robot_geometry_msgs::Pose& pose, + const std::vector& saved_poses, const double& angle_threshol) { // Tìm điểm gần nhất @@ -657,14 +657,14 @@ namespace dock_planner return nearest_idx; } - bool DockCalc::isFreeSpace(const geometry_msgs::Pose& pose, - const std::vector& footprint) + bool DockCalc::isFreeSpace(const robot_geometry_msgs::Pose& pose, + const std::vector& footprint) { - // static std::vector free_zone; + // static std::vector free_zone; // auto makePose = [](double x, double y, double yaw) // { - // geometry_msgs::Pose p; + // robot_geometry_msgs::Pose p; // p.position.x = x; // p.position.y = y; // p.position.z = 0.0; @@ -696,17 +696,17 @@ namespace dock_planner // if(check_goal_) // { - // std::vector result_free_zone = calcFreeZone(pose, footprint); + // std::vector result_free_zone = calcFreeZone(pose, footprint); // if((int)result_free_zone.size() < 4) return false; // free_zone.push_back(makePose(result_free_zone[0].x, result_free_zone[0].y, 0.0)); // free_zone.push_back(makePose(result_free_zone[1].x, result_free_zone[1].y, 0.0)); // free_zone.push_back(makePose(result_free_zone[2].x, result_free_zone[2].y, 0.0)); // check_goal_ = false; // } - // std::vector space_footprint = interpolateFootprint(pose, footprint, resolution); + // std::vector space_footprint = interpolateFootprint(pose, footprint, resolution); // for (int i = 0; i < (int)space_footprint.size(); i++) // { - // geometry_msgs::Pose Pose_on_footprint; + // robot_geometry_msgs::Pose Pose_on_footprint; // Pose_on_footprint.position.x = space_footprint[i].x; // Pose_on_footprint.position.y = space_footprint[i].y; // if((int)free_zone.size() < 3) return false; @@ -745,11 +745,11 @@ namespace dock_planner return true; } - std::vector DockCalc::calcFreeZone(const geometry_msgs::Pose& pose, - const std::vector& footprint) + std::vector DockCalc::calcFreeZone(const robot_geometry_msgs::Pose& pose, + const std::vector& footprint) { // Dịch sang tọa độ tuyệt đối - std::vector free_zone; + std::vector free_zone; double cos_th = std::cos(data_convert::getYaw(pose.orientation)); double sin_th = std::sin(data_convert::getYaw(pose.orientation)); for (auto pt : footprint) @@ -757,7 +757,7 @@ namespace dock_planner pt.x *= 1.2; pt.y *= 1.2; - geometry_msgs::Point abs_pt; + robot_geometry_msgs::Point abs_pt; abs_pt.x = pose.position.x + pt.x * cos_th - pt.y * sin_th; abs_pt.y = pose.position.y + pt.x * sin_th + pt.y * cos_th; free_zone.push_back(abs_pt); @@ -765,29 +765,29 @@ namespace dock_planner return free_zone; } - std::vector DockCalc::interpolateFootprint(const geometry_msgs::Pose& pose, - const std::vector& footprint, const double& resolution) + std::vector DockCalc::interpolateFootprint(const robot_geometry_msgs::Pose& pose, + const std::vector& footprint, const double& resolution) { // Dịch sang tọa độ tuyệt đối - std::vector abs_footprint; + std::vector abs_footprint; double cos_th = std::cos(data_convert::getYaw(pose.orientation)); double sin_th = std::sin(data_convert::getYaw(pose.orientation)); for (auto pt : footprint) { pt.x *= 1.1; pt.y *= 1.1; - geometry_msgs::Point abs_pt; + robot_geometry_msgs::Point abs_pt; abs_pt.x = pose.position.x + pt.x * cos_th - pt.y * sin_th; abs_pt.y = pose.position.y + pt.x * sin_th + pt.y * cos_th; abs_footprint.push_back(abs_pt); } - std::vector points; + std::vector points; for (size_t i = 0; i < (int)abs_footprint.size(); ++i) { - const geometry_msgs::Point &start = abs_footprint[i]; - const geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()]; + const robot_geometry_msgs::Point &start = abs_footprint[i]; + const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()]; double dx = end.x - start.x; double dy = end.y - start.y; @@ -799,7 +799,7 @@ namespace dock_planner if (j == steps && i != (int)abs_footprint.size() - 1) continue; // tránh lặp double t = static_cast(j) / steps; - geometry_msgs::Point pt; + robot_geometry_msgs::Point pt; pt.x = start.x + t * dx; pt.y = start.y + t * dy; points.push_back(pt); diff --git a/src/dock_planner.cpp b/src/dock_planner.cpp index 93c25b7..a412f29 100644 --- a/src/dock_planner.cpp +++ b/src/dock_planner.cpp @@ -65,11 +65,11 @@ namespace dock_planner // plan_pub_ = private_nh.advertise("plan", 1); - nav_2d_msgs::Polygon2D footprint_dock ;//= nav_2d_utils::footprintFromParams(private_nh,false); + robot_nav_2d_msgs::Polygon2D footprint_dock ;//= robot_nav_2d_utils::footprintFromParams(private_nh,false); for(auto pt : footprint_dock.points) { - geometry_msgs::Point footprint_point; + robot_geometry_msgs::Point footprint_point; footprint_point.x = pt.x; footprint_point.y = pt.y; footprint_point.z = 0.0; @@ -81,14 +81,14 @@ namespace dock_planner } } - bool DockPlanner::makePlan(const geometry_msgs::PoseStamped& start, - const geometry_msgs::PoseStamped& goal, - std::vector& plan) + bool DockPlanner::makePlan(const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, + std::vector& plan) { if(!initialized_) return false; if(!plan.empty()) plan.clear(); - vector planMoveToDock; - std::vector footprint_robot = costmap_robot_->getRobotFootprint(); + vector planMoveToDock; + std::vector footprint_robot = costmap_robot_->getRobotFootprint(); // footprint_dock_ = costmap_robot_->getRobotFootprint(); int degree = 2; @@ -99,7 +99,7 @@ namespace dock_planner if(!status_makePlanMoveToDock) return false; for(int i = 0; i < (int)planMoveToDock.size(); i++) { - geometry_msgs::PoseStamped pose; + robot_geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.pose.position.x = planMoveToDock[i].position.x; @@ -112,7 +112,7 @@ namespace dock_planner return true; } - // void DockPlanner::publishPlan(const std::vector& plan) + // void DockPlanner::publishPlan(const std::vector& plan) // { // nav_msgs::Path path_msg; // path_msg.header.stamp = robot::Time::now(); diff --git a/src/utils/curve_common.cpp b/src/utils/curve_common.cpp index b5bcfbe..8944a3c 100644 --- a/src/utils/curve_common.cpp +++ b/src/utils/curve_common.cpp @@ -1,16 +1,16 @@ #include "dock_planner/utils/curve_common.h" #include "dock_planner/utils/conversion.h" -#include +#include #include CurveCommon::CurveCommon() { } -nav_msgs::Path CurveCommon::Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, double t_intervel, std::string frame_id) +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; - geometry_msgs::PoseStamped current_pose; + robot_geometry_msgs::PoseStamped current_pose; line_result.header.frame_id = frame_id; line_result.header.stamp = robot::Time::now(); @@ -38,7 +38,7 @@ nav_msgs::Path CurveCommon::Generate_Line(geometry_msgs::Point start_point, geom nav_msgs::Path CurveCommon::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id) { nav_msgs::Path bezier_curve_result; - geometry_msgs::PoseStamped current_pose; + robot_geometry_msgs::PoseStamped current_pose; EigenTrajectoryPoint::Vector temp_control_point_vec; bezier_curve_result.header.frame_id = frame_id; @@ -151,7 +151,7 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(std::vector> discreate_point) { - geometry_msgs::Point view_point; + robot_geometry_msgs::Point view_point; for (int i = 0; i < discreate_point.size(); i++) { view_point.x = discreate_point.at(i)(0); @@ -325,7 +325,7 @@ void CurveCommon::ReadSplineInf(Spline_Inf *spline_inf, std::vector weig nav_msgs::Path CurveCommon::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id) { nav_msgs::Path bspline_curve_result; - geometry_msgs::PoseStamped current_pose; + robot_geometry_msgs::PoseStamped current_pose; bspline_curve_result.header.frame_id = frame_id; bspline_curve_result.header.stamp = robot::Time::now(); @@ -498,7 +498,7 @@ nav_msgs::Path CurveCommon::Generate_BsplineCurve(Spline_Inf bspline_inf, double nav_msgs::Path CurveCommon::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id) { nav_msgs::Path nurbs_curve_result; - geometry_msgs::PoseStamped current_pose; + robot_geometry_msgs::PoseStamped current_pose; nurbs_curve_result.header.frame_id = frame_id; nurbs_curve_result.header.stamp = robot::Time::now(); @@ -749,9 +749,9 @@ void CurveCommon::CalculateDerivativeBasisFunc(Spline_Inf *spline_inf, double u_ } } -geometry_msgs::Point CurveCommon::CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS) +robot_geometry_msgs::Point CurveCommon::CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS) { - geometry_msgs::Point derivative_curve_point; + robot_geometry_msgs::Point derivative_curve_point; int p_degree = spline_inf->order - 1; double sum_x = 0, sum_y = 0; double sum_denom = 0; @@ -875,9 +875,9 @@ geometry_msgs::Point CurveCommon::CalculateDerivativeCurvePoint(Spline_Inf *spli nav_msgs::Path CurveCommon::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id) { - geometry_msgs::Point derivative_point_result; + robot_geometry_msgs::Point derivative_point_result; nav_msgs::Path bspline_derivative_result; - geometry_msgs::PoseStamped current_pose; + robot_geometry_msgs::PoseStamped current_pose; bspline_derivative_result.header.frame_id = frame_id; bspline_derivative_result.header.stamp = robot::Time::now(); @@ -918,9 +918,9 @@ nav_msgs::Path CurveCommon::Generate_DerivativeBsplineCurve(Spline_Inf bspline_i nav_msgs::Path CurveCommon::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id) { - geometry_msgs::Point derivative_point_result; + robot_geometry_msgs::Point derivative_point_result; nav_msgs::Path derivative_basis_result; - geometry_msgs::PoseStamped current_pose; + robot_geometry_msgs::PoseStamped current_pose; derivative_basis_result.header.frame_id = frame_id; derivative_basis_result.header.stamp = robot::Time::now(); @@ -1166,11 +1166,11 @@ bool CurveCommon::curveIsValid(int degree, const std::vector &knot_vecto return true; } -geometry_msgs::Point CurveCommon::CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS) +robot_geometry_msgs::Point CurveCommon::CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS) { // TODO: Check u = 1 bug, why x=nan and y=nan? - geometry_msgs::Point curve_point; + robot_geometry_msgs::Point curve_point; int p_degree = spline_inf->order - 1; int n = spline_inf->control_point.size() - 1; // TODO: Check knot vector size and sequence is correect