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

2
CMakeLists.txt
View File

@ -53,7 +53,7 @@ target_link_libraries(dock_planner
costmap_2d costmap_2d
nav_core nav_core
robot_cpp robot_cpp
nav_2d_utils robot_nav_2d_utils
) )
# --- Include directories cho target --- # --- Include directories cho target ---

64
include/dock_planner/dock_calc.h
View File

@ -1,8 +1,8 @@
#ifndef DOCK_CALC_H #ifndef DOCK_CALC_H
#define DOCK_CALC_H #define DOCK_CALC_H
#include <nav_2d_utils/footprint.h> #include <robot_nav_2d_utils/footprint.h>
#include <geometry_msgs/PoseStamped.h> #include <robot_geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h> #include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h> #include <nav_msgs/OccupancyGrid.h>
@ -39,10 +39,10 @@ namespace dock_planner
Spline_Inf* input_spline_inf; Spline_Inf* input_spline_inf;
CurveCommon* CurveDesign; CurveCommon* CurveDesign;
vector<geometry_msgs::Pose> posesOnPathWay; vector<robot_geometry_msgs::Pose> posesOnPathWay;
vector<geometry_msgs::Point> footprint_robot_; vector<robot_geometry_msgs::Point> footprint_robot_;
geometry_msgs::PoseStamped save_goal_; robot_geometry_msgs::PoseStamped save_goal_;
// std::vector<geometry_msgs::Pose> free_zone_; // std::vector<robot_geometry_msgs::Pose> free_zone_;
bool check_goal_; bool check_goal_;
int store_start_nearest_idx_; int store_start_nearest_idx_;
@ -50,7 +50,7 @@ namespace dock_planner
double min_distance_to_goal_; double min_distance_to_goal_;
// Tính khoảng cách giữa 2 điểm // 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); return std::hypot(p1.position.x - p2.position.x, p1.position.y - p2.position.y);
} }
@ -80,42 +80,42 @@ namespace dock_planner
int degree, int degree,
double initial_step = 0.02); 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, void updatePoseOrientation(std::vector<robot_geometry_msgs::Pose>& saved_poses,
std::vector<geometry_msgs::Pose>& nurbs_path, std::vector<robot_geometry_msgs::Pose>& nurbs_path,
bool reverse = true); bool reverse = true);
bool findPathTMP(const vector<geometry_msgs::Pose>& control_points, bool findPathTMP(const vector<robot_geometry_msgs::Pose>& control_points,
vector<geometry_msgs::Pose>& saved_poses, vector<robot_geometry_msgs::Pose>& saved_poses,
const int& degree, double step = 0.02); const int& degree, double step = 0.02);
bool findNURBSControlPoints(int& dir, vector<geometry_msgs::Pose>& control_points, bool findNURBSControlPoints(int& dir, vector<robot_geometry_msgs::Pose>& control_points,
const geometry_msgs::Pose& start_pose, const robot_geometry_msgs::Pose& start_pose,
const std::vector<geometry_msgs::Pose>& posesOnPathWay, const std::vector<robot_geometry_msgs::Pose>& posesOnPathWay,
const double& angle_threshol, const int& degree); const double& angle_threshol, const int& degree);
int findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose, int findNearestPointOnPath(int& dir, const robot_geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Pose>& posesOnPathWay, const std::vector<robot_geometry_msgs::Pose>& posesOnPathWay,
const double& angle_threshol); 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, std::vector<robot_geometry_msgs::Point> calcFreeZone(const robot_geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Point>& footprint); const std::vector<robot_geometry_msgs::Point>& footprint);
std::vector<geometry_msgs::Point> interpolateFootprint(const geometry_msgs::Pose& pose, std::vector<robot_geometry_msgs::Point> interpolateFootprint(const robot_geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Point>& footprint, const double& resolution); 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: public:
@ -130,12 +130,12 @@ namespace dock_planner
~DockCalc(); ~DockCalc();
bool makePlanMoveToDock(const geometry_msgs::PoseStamped& start, bool makePlanMoveToDock(const robot_geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, const robot_geometry_msgs::PoseStamped& goal,
const vector<geometry_msgs::Point>& footprint_robot, const vector<robot_geometry_msgs::Point>& footprint_robot,
const vector<geometry_msgs::Point>& footprint_dock, const vector<robot_geometry_msgs::Point>& footprint_dock,
const int& degree, const double& step, const int& degree, const double& step,
vector<geometry_msgs::Pose>& planMoveToDock, vector<robot_geometry_msgs::Pose>& planMoveToDock,
bool reverse = false); 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 initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot);
bool makePlan(const geometry_msgs::PoseStamped& start, bool makePlan(const robot_geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, const robot_geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan); std::vector<robot_geometry_msgs::PoseStamped>& plan);
private: private:
// Core components // Core components
@ -26,7 +26,7 @@ namespace dock_planner {
bool initialized_; bool initialized_;
std::string frame_id_; std::string frame_id_;
// ros::Publisher plan_pub_; // ros::Publisher plan_pub_;
std::vector<geometry_msgs::Point> footprint_dock_; std::vector<robot_geometry_msgs::Point> footprint_dock_;
int cost_threshold_; int cost_threshold_;
double calib_safety_distance_; double calib_safety_distance_;
@ -35,7 +35,7 @@ namespace dock_planner {
DockCalc calc_plan_to_dock_; 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 } // namespace dock_planner

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

@ -3,14 +3,14 @@
#include "curve_common.h" #include "curve_common.h"
#include <Eigen/Geometry> #include <Eigen/Geometry>
#include <geometry_msgs/Point.h> #include <robot_geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.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); 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::Vector eigen_trajectory_point_vec;
EigenTrajectoryPoint eigen_trajectory_point; EigenTrajectoryPoint eigen_trajectory_point;
@ -25,7 +25,7 @@ inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::v
return eigen_trajectory_point_vec; 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::Vector eigen_trajectory_point_vec;
EigenTrajectoryPoint eigen_trajectory_point; EigenTrajectoryPoint eigen_trajectory_point;

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

@ -3,7 +3,7 @@
#include <Eigen/Eigen> #include <Eigen/Eigen>
#include <nav_msgs/Path.h> #include <nav_msgs/Path.h>
#include <geometry_msgs/Point.h> #include <robot_geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h> #include <visualization_msgs/Marker.h>
#include "color.h" #include "color.h"
#include <robot/console.h> #include <robot/console.h>
@ -43,7 +43,7 @@ class CurveCommon
{ {
public: public:
CurveCommon(); 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_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_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_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); 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);
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);
geometry_msgs::Point CalculateCurvePoint(Spline_Inf* spline_inf, double u_data, 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 CalculateCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS);
double CalculateSignedCurvature(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); Eigen::Vector3d CalculateCurvatureDirectionVector(Spline_Inf spline_inf, double u_data, bool UsingNURBS);

130
src/dock_calc.cpp
View File

@ -18,15 +18,15 @@ namespace dock_planner
delete input_spline_inf; 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.x = A.position.x + d * std::cos(theta);
B.position.y = A.position.y + d * std::sin(theta); B.position.y = A.position.y + d * std::sin(theta);
return B; 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 theta = signedAngle(A,B,C);
double sin_theta = sin(fabs(theta)); double sin_theta = sin(fabs(theta));
@ -35,7 +35,7 @@ namespace dock_planner
return distance; 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 ABx = B.position.x - A.position.x;
double ABy = B.position.y - A.position.y; double ABy = B.position.y - A.position.y;
@ -50,7 +50,7 @@ namespace dock_planner
return angle; 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; constexpr double epsilon = 1e-6;
double dx = C.position.x - B.position.x; double dx = C.position.x - B.position.x;
@ -69,15 +69,15 @@ namespace dock_planner
return -numerator / denominator; 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.x = B.position.x + t * (C.position.x - B.position.x);
D.position.y = B.position.y + t * (C.position.y - B.position.y); D.position.y = B.position.y + t * (C.position.y - B.position.y);
return D; return D;
} }
RobotGeometry DockCalc::computeGeometry(const std::vector<geometry_msgs::Point>& fp) RobotGeometry DockCalc::computeGeometry(const std::vector<robot_geometry_msgs::Point>& fp)
{ {
double min_x = 1e9, max_x = -1e9; double min_x = 1e9, max_x = -1e9;
double min_y = 1e9, max_y = -1e9; double min_y = 1e9, max_y = -1e9;
@ -99,12 +99,12 @@ namespace dock_planner
return {length, width, radius}; return {length, width, radius};
} }
bool DockCalc::makePlanMoveToDock(const geometry_msgs::PoseStamped& start, bool DockCalc::makePlanMoveToDock(const robot_geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, const robot_geometry_msgs::PoseStamped& goal,
const vector<geometry_msgs::Point>& footprint_robot, const vector<robot_geometry_msgs::Point>& footprint_robot,
const vector<geometry_msgs::Point>& footprint_dock, const vector<robot_geometry_msgs::Point>& footprint_dock,
const int& degree, const double& step, const int& degree, const double& step,
vector<geometry_msgs::Pose>& planMoveToDock, vector<robot_geometry_msgs::Pose>& planMoveToDock,
bool reverse) bool reverse)
{ {
constexpr double epsilon = 1e-6; constexpr double epsilon = 1e-6;
@ -141,10 +141,10 @@ namespace dock_planner
} }
// Lambda: Generate NURBS path // Lambda: Generate NURBS path
auto generateNURBSPath = [&](const vector<geometry_msgs::Pose>& control_points, bool check_isFreeSpace, auto generateNURBSPath = [&](const vector<robot_geometry_msgs::Pose>& control_points, bool check_isFreeSpace,
bool edge_end_plag, bool path_reverse) -> vector<geometry_msgs::Pose> bool edge_end_plag, bool path_reverse) -> vector<robot_geometry_msgs::Pose>
{ {
vector<geometry_msgs::Pose> result; vector<robot_geometry_msgs::Pose> result;
bool obstacle_flag = false; bool obstacle_flag = false;
// Convert to Eigen format // Convert to Eigen format
@ -180,14 +180,14 @@ namespace dock_planner
// robot::log_error("[dock_calc] Adaptive step size: %f", step_new); // robot::log_error("[dock_calc] Adaptive step size: %f", step_new);
// Generate path points // Generate path points
vector<geometry_msgs::Pose> saved_poses; vector<robot_geometry_msgs::Pose> saved_poses;
for (double t = 0.0; t <= 1.0; t += step_new) 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)) if (!std::isnan(point.x) && !std::isnan(point.y))
{ {
geometry_msgs::Pose pose; robot_geometry_msgs::Pose pose;
pose.position = point; pose.position = point;
pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0); pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0);
@ -261,9 +261,9 @@ namespace dock_planner
min_distance_to_goal_ = (safety_distance_ + distance_dock / 2.0); min_distance_to_goal_ = (safety_distance_ + distance_dock / 2.0);
// Calculate pose C (offset from goal) // 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)); 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 // Compute t parameter and determine direction
double t = compute_t(start.pose, goal.pose, Pose_C); double t = compute_t(start.pose, goal.pose, Pose_C);
@ -288,7 +288,7 @@ namespace dock_planner
} }
// Calculate pose D and distance AD // 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); double distance_AD = pointToSegmentDistance(start.pose, goal.pose, Pose_C);
// Calculate angle threshold with scaling // Calculate angle threshold with scaling
@ -308,13 +308,13 @@ namespace dock_planner
double distance_DF = distance_AD / tan(angle_alpha); double distance_DF = distance_AD / tan(angle_alpha);
double distance_min_for_pose_F = distance_min_for_pose_C + distance_CD + distance_DF; 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); robot_geometry_msgs::Pose Pose_F = offsetPoint(Goal_Pose, yaw_goal, distance_min_for_pose_F);
geometry_msgs::Pose mid_pose; robot_geometry_msgs::Pose mid_pose;
mid_pose.position.x = (Pose_C.position.x + Pose_F.position.x) / 2.0; 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; mid_pose.position.y = (Pose_C.position.y + Pose_F.position.y) / 2.0;
vector<geometry_msgs::Pose> control_points_tmp = {Pose_C, mid_pose, Pose_F}; vector<robot_geometry_msgs::Pose> control_points_tmp = {Pose_C, mid_pose, Pose_F};
vector<geometry_msgs::Pose> saved_poses_tmp; vector<robot_geometry_msgs::Pose> saved_poses_tmp;
if (!findPathTMP(control_points_tmp, saved_poses_tmp, degree, step)) if (!findPathTMP(control_points_tmp, saved_poses_tmp, degree, step))
@ -325,7 +325,7 @@ namespace dock_planner
// Find first segment control points // Find first segment control points
int dir; int dir;
vector<geometry_msgs::Pose> control_points_1; vector<robot_geometry_msgs::Pose> control_points_1;
if (!findNURBSControlPoints(dir, control_points_1, start.pose, saved_poses_tmp, angle_threshold, degree)) 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."); 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); ((dir == 1 || dir == 0) && dir_robot_move_to_goal);
// Generate first segment // Generate first segment
vector<geometry_msgs::Pose> first_segment = generateNURBSPath(control_points_1, true, false, first_segment_reverse); vector<robot_geometry_msgs::Pose> first_segment = generateNURBSPath(control_points_1, true, false, first_segment_reverse);
planMoveToDock.insert(planMoveToDock.end(), first_segment.begin(), first_segment.end()); planMoveToDock.insert(planMoveToDock.end(), first_segment.begin(), first_segment.end());
//Generate second segment (to goal) //Generate second segment (to goal)
if (!planMoveToDock.empty()) 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.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; mid_control_point.position.y = (planMoveToDock.back().position.y + goal.pose.position.y) / 2.0;
vector<geometry_msgs::Pose> control_points_2 = {planMoveToDock.back(), vector<robot_geometry_msgs::Pose> control_points_2 = {planMoveToDock.back(),
mid_control_point, mid_control_point,
goal.pose}; goal.pose};
vector<geometry_msgs::Pose> second_segment = generateNURBSPath(control_points_2, true, true, dir_robot_move_to_goal); vector<robot_geometry_msgs::Pose> 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.end(), second_segment.begin(), second_segment.end());
planMoveToDock.insert(planMoveToDock.begin(),start.pose); planMoveToDock.insert(planMoveToDock.begin(),start.pose);
planMoveToDock.insert(planMoveToDock.end(),goal.pose); planMoveToDock.insert(planMoveToDock.end(),goal.pose);
@ -360,8 +360,8 @@ namespace dock_planner
return !planMoveToDock.empty(); return !planMoveToDock.empty();
} }
bool DockCalc::findPathTMP(const vector<geometry_msgs::Pose>& control_points, bool DockCalc::findPathTMP(const vector<robot_geometry_msgs::Pose>& control_points,
vector<geometry_msgs::Pose>& saved_poses, vector<robot_geometry_msgs::Pose>& saved_poses,
const int& degree, double step) const int& degree, double step)
{ {
// Chuyển đổi control points sang định dạng Eigen::Vector3d // 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, degree + 1, eigen_control_points, knot_vector);
CurveDesign->ReadSplineInf(input_spline_inf, weights, false); CurveDesign->ReadSplineInf(input_spline_inf, weights, false);
// vector<geometry_msgs::Pose> poses_tmp; // vector<robot_geometry_msgs::Pose> poses_tmp;
for(double t = 0.0; t <= 1.0; t += step) for(double t = 0.0; t <= 1.0; t += step)
{ {
// robot::log_error("[dock_calc][findPathTMP] t(%f) DEBUG 10", t); // 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)) 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); // 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.position = point;
pose.orientation.x = 0.0; pose.orientation.x = 0.0;
pose.orientation.y = 0.0; pose.orientation.y = 0.0;
@ -432,8 +432,8 @@ namespace dock_planner
return true; return true;
} }
void DockCalc::updatePoseOrientation(std::vector<geometry_msgs::Pose>& saved_poses, void DockCalc::updatePoseOrientation(std::vector<robot_geometry_msgs::Pose>& saved_poses,
std::vector<geometry_msgs::Pose>& nurbs_path, std::vector<robot_geometry_msgs::Pose>& nurbs_path,
bool reverse) bool reverse)
{ {
double yaw; double yaw;
@ -465,7 +465,7 @@ namespace dock_planner
double initial_step) double initial_step)
{ {
double length = 0.0; double length = 0.0;
geometry_msgs::Point prev_point, curr_point; robot_geometry_msgs::Point prev_point, curr_point;
std::vector<double> segment_lengths; std::vector<double> segment_lengths;
static double step; static double step;
@ -492,9 +492,9 @@ namespace dock_planner
return length; return length;
} }
bool DockCalc::findNURBSControlPoints(int& dir, vector<geometry_msgs::Pose>& control_points, bool DockCalc::findNURBSControlPoints(int& dir, vector<robot_geometry_msgs::Pose>& control_points,
const geometry_msgs::Pose& start_pose, const robot_geometry_msgs::Pose& start_pose,
const std::vector<geometry_msgs::Pose>& saved_poses, const std::vector<robot_geometry_msgs::Pose>& saved_poses,
const double& angle_threshold, const int& degree) const double& angle_threshold, const int& degree)
{ {
if (saved_poses.empty()) if (saved_poses.empty())
@ -507,9 +507,9 @@ namespace dock_planner
int idx = findNearestPointOnPath(dir, start_pose, saved_poses, angle_threshold); int idx = findNearestPointOnPath(dir, start_pose, saved_poses, angle_threshold);
// Helper lambda to create pose at position // 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.x = x;
pose.position.y = y; pose.position.y = y;
pose.orientation = data_convert::createQuaternionMsgFromYaw(0); pose.orientation = data_convert::createQuaternionMsgFromYaw(0);
@ -584,8 +584,8 @@ namespace dock_planner
return true; return true;
} }
int DockCalc::findNearestPointOnPath(int& dir, const geometry_msgs::Pose& pose, int DockCalc::findNearestPointOnPath(int& dir, const robot_geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Pose>& saved_poses, const std::vector<robot_geometry_msgs::Pose>& saved_poses,
const double& angle_threshol) const double& angle_threshol)
{ {
// Tìm điểm gần nhất // Tìm điểm gần nhất
@ -657,14 +657,14 @@ namespace dock_planner
return nearest_idx; return nearest_idx;
} }
bool DockCalc::isFreeSpace(const geometry_msgs::Pose& pose, bool DockCalc::isFreeSpace(const robot_geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Point>& footprint) const std::vector<robot_geometry_msgs::Point>& footprint)
{ {
// static std::vector<geometry_msgs::Pose> free_zone; // static std::vector<robot_geometry_msgs::Pose> free_zone;
// auto makePose = [](double x, double y, double yaw) // auto makePose = [](double x, double y, double yaw)
// { // {
// geometry_msgs::Pose p; // robot_geometry_msgs::Pose p;
// p.position.x = x; // p.position.x = x;
// p.position.y = y; // p.position.y = y;
// p.position.z = 0.0; // p.position.z = 0.0;
@ -696,17 +696,17 @@ namespace dock_planner
// if(check_goal_) // if(check_goal_)
// { // {
// std::vector<geometry_msgs::Point> result_free_zone = calcFreeZone(pose, footprint); // std::vector<robot_geometry_msgs::Point> result_free_zone = calcFreeZone(pose, footprint);
// if((int)result_free_zone.size() < 4) return false; // 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[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[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)); // free_zone.push_back(makePose(result_free_zone[2].x, result_free_zone[2].y, 0.0));
// check_goal_ = false; // check_goal_ = false;
// } // }
// std::vector<geometry_msgs::Point> space_footprint = interpolateFootprint(pose, footprint, resolution); // std::vector<robot_geometry_msgs::Point> space_footprint = interpolateFootprint(pose, footprint, resolution);
// for (int i = 0; i < (int)space_footprint.size(); i++) // 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.x = space_footprint[i].x;
// Pose_on_footprint.position.y = space_footprint[i].y; // Pose_on_footprint.position.y = space_footprint[i].y;
// if((int)free_zone.size() < 3) return false; // if((int)free_zone.size() < 3) return false;
@ -745,11 +745,11 @@ namespace dock_planner
return true; return true;
} }
std::vector<geometry_msgs::Point> DockCalc::calcFreeZone(const geometry_msgs::Pose& pose, std::vector<robot_geometry_msgs::Point> DockCalc::calcFreeZone(const robot_geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Point>& footprint) const std::vector<robot_geometry_msgs::Point>& footprint)
{ {
// Dịch sang tọa độ tuyệt đối // Dịch sang tọa độ tuyệt đối
std::vector<geometry_msgs::Point> free_zone; std::vector<robot_geometry_msgs::Point> free_zone;
double cos_th = std::cos(data_convert::getYaw(pose.orientation)); double cos_th = std::cos(data_convert::getYaw(pose.orientation));
double sin_th = std::sin(data_convert::getYaw(pose.orientation)); double sin_th = std::sin(data_convert::getYaw(pose.orientation));
for (auto pt : footprint) for (auto pt : footprint)
@ -757,7 +757,7 @@ namespace dock_planner
pt.x *= 1.2; pt.x *= 1.2;
pt.y *= 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.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_pt.y = pose.position.y + pt.x * sin_th + pt.y * cos_th;
free_zone.push_back(abs_pt); free_zone.push_back(abs_pt);
@ -765,29 +765,29 @@ namespace dock_planner
return free_zone; return free_zone;
} }
std::vector<geometry_msgs::Point> DockCalc::interpolateFootprint(const geometry_msgs::Pose& pose, std::vector<robot_geometry_msgs::Point> DockCalc::interpolateFootprint(const robot_geometry_msgs::Pose& pose,
const std::vector<geometry_msgs::Point>& footprint, const double& resolution) const std::vector<robot_geometry_msgs::Point>& footprint, const double& resolution)
{ {
// Dịch sang tọa độ tuyệt đối // Dịch sang tọa độ tuyệt đối
std::vector<geometry_msgs::Point> abs_footprint; std::vector<robot_geometry_msgs::Point> abs_footprint;
double cos_th = std::cos(data_convert::getYaw(pose.orientation)); double cos_th = std::cos(data_convert::getYaw(pose.orientation));
double sin_th = std::sin(data_convert::getYaw(pose.orientation)); double sin_th = std::sin(data_convert::getYaw(pose.orientation));
for (auto pt : footprint) for (auto pt : footprint)
{ {
pt.x *= 1.1; pt.x *= 1.1;
pt.y *= 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.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_pt.y = pose.position.y + pt.x * sin_th + pt.y * cos_th;
abs_footprint.push_back(abs_pt); abs_footprint.push_back(abs_pt);
} }
std::vector<geometry_msgs::Point> points; std::vector<robot_geometry_msgs::Point> points;
for (size_t i = 0; i < (int)abs_footprint.size(); ++i) for (size_t i = 0; i < (int)abs_footprint.size(); ++i)
{ {
const geometry_msgs::Point &start = abs_footprint[i]; const robot_geometry_msgs::Point &start = abs_footprint[i];
const geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()]; const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
double dx = end.x - start.x; double dx = end.x - start.x;
double dy = end.y - start.y; double dy = end.y - start.y;
@ -799,7 +799,7 @@ namespace dock_planner
if (j == steps && i != (int)abs_footprint.size() - 1) if (j == steps && i != (int)abs_footprint.size() - 1)
continue; // tránh lặp continue; // tránh lặp
double t = static_cast<double>(j) / steps; double t = static_cast<double>(j) / steps;
geometry_msgs::Point pt; robot_geometry_msgs::Point pt;
pt.x = start.x + t * dx; pt.x = start.x + t * dx;
pt.y = start.y + t * dy; pt.y = start.y + t * dy;
points.push_back(pt); points.push_back(pt);

18
src/dock_planner.cpp
View File

@ -65,11 +65,11 @@ namespace dock_planner
// plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1); // plan_pub_ = private_nh.advertise<nav_msgs::Path>("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) for(auto pt : footprint_dock.points)
{ {
geometry_msgs::Point footprint_point; robot_geometry_msgs::Point footprint_point;
footprint_point.x = pt.x; footprint_point.x = pt.x;
footprint_point.y = pt.y; footprint_point.y = pt.y;
footprint_point.z = 0.0; footprint_point.z = 0.0;
@ -81,14 +81,14 @@ namespace dock_planner
} }
} }
bool DockPlanner::makePlan(const geometry_msgs::PoseStamped& start, bool DockPlanner::makePlan(const robot_geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, const robot_geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan) std::vector<robot_geometry_msgs::PoseStamped>& plan)
{ {
if(!initialized_) return false; if(!initialized_) return false;
if(!plan.empty()) plan.clear(); if(!plan.empty()) plan.clear();
vector<geometry_msgs::Pose> planMoveToDock; vector<robot_geometry_msgs::Pose> planMoveToDock;
std::vector<geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint(); std::vector<robot_geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint();
// footprint_dock_ = costmap_robot_->getRobotFootprint(); // footprint_dock_ = costmap_robot_->getRobotFootprint();
int degree = 2; int degree = 2;
@ -99,7 +99,7 @@ namespace dock_planner
if(!status_makePlanMoveToDock) return false; if(!status_makePlanMoveToDock) return false;
for(int i = 0; i < (int)planMoveToDock.size(); i++) 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.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID(); pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = planMoveToDock[i].position.x; pose.pose.position.x = planMoveToDock[i].position.x;
@ -112,7 +112,7 @@ namespace dock_planner
return true; return true;
} }
// void DockPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& plan) // void DockPlanner::publishPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan)
// { // {
// nav_msgs::Path path_msg; // nav_msgs::Path path_msg;
// path_msg.header.stamp = robot::Time::now(); // path_msg.header.stamp = robot::Time::now();

40
src/utils/curve_common.cpp
View File

@ -1,16 +1,16 @@
#include "dock_planner/utils/curve_common.h" #include "dock_planner/utils/curve_common.h"
#include "dock_planner/utils/conversion.h" #include "dock_planner/utils/conversion.h"
#include <geometry_msgs/PoseStamped.h> #include <robot_geometry_msgs/PoseStamped.h>
#include <iostream> #include <iostream>
CurveCommon::CurveCommon() 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; 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.frame_id = frame_id;
line_result.header.stamp = robot::Time::now(); 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 CurveCommon::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id)
{ {
nav_msgs::Path bezier_curve_result; nav_msgs::Path bezier_curve_result;
geometry_msgs::PoseStamped current_pose; robot_geometry_msgs::PoseStamped current_pose;
EigenTrajectoryPoint::Vector temp_control_point_vec; EigenTrajectoryPoint::Vector temp_control_point_vec;
bezier_curve_result.header.frame_id = frame_id; bezier_curve_result.header.frame_id = frame_id;
@ -151,7 +151,7 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(std::vector<Eigen::Ve
waypoints_marker.points.reserve(discreate_point.size()); waypoints_marker.points.reserve(discreate_point.size());
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++)
{ {
view_point.x = discreate_point.at(i)(0); view_point.x = discreate_point.at(i)(0);
@ -178,7 +178,7 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(std::vector<Eigen::V
waypoints_marker.points.reserve(discreate_point.size()); waypoints_marker.points.reserve(discreate_point.size());
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++)
{ {
view_point.x = discreate_point.at(i)(0); view_point.x = discreate_point.at(i)(0);
@ -207,7 +207,7 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(EigenTrajectoryPoint:
waypoints_marker.points.reserve(discreate_point.size()); waypoints_marker.points.reserve(discreate_point.size());
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++)
{ {
view_point.x = discreate_point.at(i).position(0); view_point.x = discreate_point.at(i).position(0);
@ -237,7 +237,7 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(EigenTrajectoryPoint
waypoints_marker.points.reserve(discreate_point.size()); waypoints_marker.points.reserve(discreate_point.size());
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++)
{ {
view_point.x = discreate_point.at(i).position(0); view_point.x = discreate_point.at(i).position(0);
@ -250,7 +250,7 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(EigenTrajectoryPoint
void CurveCommon::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point) void CurveCommon::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point)
{ {
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++)
{ {
view_point.x = discreate_point.at(i).position(0); view_point.x = discreate_point.at(i).position(0);
@ -261,7 +261,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(visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> discreate_point)
{ {
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++)
{ {
view_point.x = discreate_point.at(i)(0); view_point.x = discreate_point.at(i)(0);
@ -325,7 +325,7 @@ 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) nav_msgs::Path CurveCommon::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id)
{ {
nav_msgs::Path bspline_curve_result; 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.frame_id = frame_id;
bspline_curve_result.header.stamp = robot::Time::now(); 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 CurveCommon::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id)
{ {
nav_msgs::Path nurbs_curve_result; 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.frame_id = frame_id;
nurbs_curve_result.header.stamp = robot::Time::now(); 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; int p_degree = spline_inf->order - 1;
double sum_x = 0, sum_y = 0; double sum_x = 0, sum_y = 0;
double sum_denom = 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) 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; 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.frame_id = frame_id;
bspline_derivative_result.header.stamp = robot::Time::now(); 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) 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; 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.frame_id = frame_id;
derivative_basis_result.header.stamp = robot::Time::now(); derivative_basis_result.header.stamp = robot::Time::now();
@ -1166,11 +1166,11 @@ bool CurveCommon::curveIsValid(int degree, const std::vector<double> &knot_vecto
return true; 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? // 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 p_degree = spline_inf->order - 1;
int n = spline_inf->control_point.size() - 1; int n = spline_inf->control_point.size() - 1;
// TODO: Check knot vector size and sequence is correect // TODO: Check knot vector size and sequence is correect