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

Compare commits

base: DuongTD:377ebe3d6f66c6a1c7f2d1a1d76fd1b330a93952
Branches Tags
DuongTD:main
..
compare: DuongTD:60e9c5673f8fd646d628f843ef73e71d1d9b2a17
Branches Tags
DuongTD:main

3 Commits
377ebe3d6f ... 60e9c5673f

Author SHA1 Message Date
HiepLM
60e9c5673f HiepLM update 2025-12-30 10:22:16 +07:00
HiepLM
d6512018ef hiep sua ten file 2025-12-30 09:57:13 +07:00
HiepLM
6d55c6c4be Hiep update 2025-12-30 09:06:20 +07:00
10 changed files with 210 additions and 218 deletions
Expand all files Collapse all files

12
CMakeLists.txt
View File

@@ -44,13 +44,13 @@ add_library(custom_planner
# --- Link các thư viện phụ thuộc ---
target_link_libraries(custom_planner
${Boost_LIBRARIES} # Boost
visualization_msgs
nav_msgs
robot_visualization_msgs
robot_nav_msgs
tf3
tf3_geometry_msgs
robot_tf3_geometry_msgs
robot_time
data_convert
costmap_2d
robot_costmap_2d
nav_core
robot_protocol_msgs
)
@@ -72,8 +72,8 @@ install(TARGETS custom_planner
INCLUDES DESTINATION include # Cài đặt include
)
# --- Xuất export set costmap_2dTargets thành file CMake module ---
# --- Tạo file lib/cmake/custom_planner/costmap_2dTargets.cmake ---
# --- Xuất export set robot_costmap_2dTargets thành file CMake module ---
# --- Tạo file lib/cmake/custom_planner/robot_costmap_2dTargets.cmake ---
# --- File này chứa cấu hình giúp project khác có thể dùng ---
# --- Find_package(custom_planner REQUIRED) ---
# --- Target_link_libraries(my_app PRIVATE custom_planner::custom_planner) ---

34
include/custom_planner/Curve_common.h
View File

@@ -7,9 +7,9 @@
// #include <deque>
// #include <iostream>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>
#include <robot_nav_msgs/Path.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_visualization_msgs/Marker.h>
#include "custom_planner/color.h"
struct Spline_Inf
@@ -47,16 +47,16 @@ class Curve_common
{
public:
Curve_common();
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_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id);
nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id);
nav_msgs::Path Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id);
nav_msgs::Path Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id);
nav_msgs::Path Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id);
robot_nav_msgs::Path Generate_Line(robot_geometry_msgs::Point start_point, robot_geometry_msgs::Point end_point, double t_intervel, std::string frame_id);
robot_nav_msgs::Path Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id);
robot_nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id);
robot_nav_msgs::Path Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id);
robot_nav_msgs::Path Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id);
robot_nav_msgs::Path Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id);
void CalculateDerivativeBasisFunc(Spline_Inf *spline_inf, double u_data, int differential_times);
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);
@@ -67,15 +67,15 @@ class Curve_common
void ReadSplineInf(Spline_Inf *bspline_inf, std::vector<double> weight_vector, bool use_limit_derivative_fitting);
void ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector *input_point, std::vector<double> file_discreate_point);
void ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > *input_point, std::vector<double> file_discreate_point);
void ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point);
void ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point);
void ShowDiscreatePoint(robot_visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point);
void ShowDiscreatePoint(robot_visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point);
//TODO: move relate visualize function to new vislization.h
int print();
visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale);
visualization_msgs::Marker ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale);
visualization_msgs::Marker ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, const std::string& name, double scale);
visualization_msgs::Marker ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale);
robot_visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale);
robot_visualization_msgs::Marker ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, robot_std_msgs::ColorRGBA point_color, const std::string& name, double scale);
robot_visualization_msgs::Marker ShowDiscreatePoint(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, const std::string& name, double scale);
robot_visualization_msgs::Marker ShowDiscreatePoint2(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point, const std::string& frame_id, robot_std_msgs::ColorRGBA point_color, const std::string& name, double scale);
private:

6
include/custom_planner/color.h
View File

@@ -1,14 +1,14 @@
#ifndef CUSTOM_PLANNER_COLOR_H_
#define CUSTOM_PLANNER_COLOR_H_
#include <std_msgs/ColorRGBA.h>
#include <robot_std_msgs/ColorRGBA.h>
namespace agv_visualization
{
class Color : public std_msgs::ColorRGBA
class Color : public robot_std_msgs::ColorRGBA
{
public:
Color() : std_msgs::ColorRGBA() {}
Color() : robot_std_msgs::ColorRGBA() {}
Color(double red, double green, double blue) : Color(red, green, blue, 1.0) {}
Color(double red, double green, double blue, double alpha) : Color() {
r = red;

10
include/custom_planner/conversion.h
View File

@@ -3,14 +3,14 @@
#include "custom_planner/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;

58
include/custom_planner/custom_planner.h
View File

@@ -9,11 +9,11 @@
using namespace std;
// ROS
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_visualization_msgs/Marker.h>
// Costmap used for the map representation
#include <costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
// global representation
#include <nav_core/base_global_planner.h>
@@ -29,7 +29,7 @@ using namespace std;
#include "custom_planner/conversion.h"
#include "custom_planner/merge_path_calc.h"
#include <geometry_msgs/PoseArray.h>
#include <robot_geometry_msgs/PoseArray.h>
#include <thread>
#include <boost/thread.hpp>
@@ -70,7 +70,7 @@ public:
* @param name The name of this planner
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use
*/
CustomPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot);
CustomPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot);
/**
@@ -79,12 +79,12 @@ public:
* @param costmap_ros A pointer to the ROS wrapper of the costmap to use
*/
virtual bool initialize(std::string name,
costmap_2d::Costmap2DROBOT* costmap_robot);
robot_costmap_2d::Costmap2DROBOT* costmap_robot);
virtual bool makePlan(const robot_protocol_msgs::Order& msg,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan);
const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
std::vector<robot_geometry_msgs::PoseStamped>& plan);
/**
* @brief Given a goal pose in the world, compute a plan
* @param start The start pose
@@ -92,9 +92,9 @@ public:
* @param plan The plan... filled by the planner
* @return True if a valid plan was found, false otherwise
*/
virtual bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan)
virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
std::vector<robot_geometry_msgs::PoseStamped>& plan)
{
printf("[%s:%d] This function is not available!",__FILE__,__LINE__);
return false;
@@ -104,12 +104,12 @@ public:
private:
void publishStats(int solution_cost, int solution_size,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal);
const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal);
static void transformFootprintToEdges(const geometry_msgs::Pose& robot_pose,
const std::vector<geometry_msgs::Point>& footprint,
std::vector<geometry_msgs::Point>& out_footprint);
static void transformFootprintToEdges(const robot_geometry_msgs::Pose& robot_pose,
const std::vector<robot_geometry_msgs::Point>& footprint,
std::vector<robot_geometry_msgs::Point>& out_footprint);
Pose pointOnAngleBisector(Pose& A, Pose& B, Pose& C, double length = 1.0);
@@ -130,7 +130,7 @@ private:
}
bool checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose,
const geometry_msgs::PoseStamped &goal);
const robot_geometry_msgs::PoseStamped &goal);
bool loadPathwayData(const string& filename);
@@ -139,17 +139,17 @@ private:
// void order_msg_handle(const vda5050_msgs::Order::ConstPtr& msg);
bool makePlanWithOrder(robot_protocol_msgs::Order msg,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal);
const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal);
bool doIntersect(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1,
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2);
bool doIntersect(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const robot_geometry_msgs::Point& p2, const robot_geometry_msgs::Point& q2);
std::optional<geometry_msgs::Point> computeIntersectionPoint(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1,
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2);
std::optional<robot_geometry_msgs::Point> computeIntersectionPoint(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const robot_geometry_msgs::Point& p2, const robot_geometry_msgs::Point& q2);
bool calcAllYaw(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
bool calcAllYaw(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
vector<Pose>& saveposesOnEdge,
int& total_edge);
@@ -163,7 +163,7 @@ private:
double computeDeltaAngle(Pose& Pose1, Pose& Pose2);
vector<Pose> divideSegment(Pose& A, Pose& B, double d);
vector<geometry_msgs::PoseStamped> divideSegment(geometry_msgs::PoseStamped& A, geometry_msgs::PoseStamped& B, double d);
vector<robot_geometry_msgs::PoseStamped> divideSegment(robot_geometry_msgs::PoseStamped& A, robot_geometry_msgs::PoseStamped& B, double d);
// Vector lưu thông tin các cạnh
std::vector<InfEdge> edges_info_;
@@ -210,8 +210,8 @@ private:
bool allow_unknown_;
std::string name_;
costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
std::vector<geometry_msgs::Point> footprint_;
robot_costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
std::vector<robot_geometry_msgs::Point> footprint_;
unsigned int current_env_width_;
unsigned int current_env_height_;

16
include/custom_planner/merge_path_calc.h
View File

@@ -1,6 +1,6 @@
#ifndef MERGER_PATH_CALC_H_
#define MERGER_PATH_CALC_H_
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <vector>
#include <cmath>
@@ -96,8 +96,8 @@ namespace custom_planner
* Hàm này tìm các điểm điều khiển NURBS dựa trên vị trí bắt đầu, kết thúc và các điểm trên đường đi.
* Nó sử dụng bậc của đường cong NURBS để xác định số lượng điểm điều khiển cần thiết.
*/
bool findNURBSControlPoints(vector<geometry_msgs::PoseStamped>& control_points,
const geometry_msgs::PoseStamped& start,
bool findNURBSControlPoints(vector<robot_geometry_msgs::PoseStamped>& control_points,
const robot_geometry_msgs::PoseStamped& start,
std::vector<Pose>& posesOnPathWay,
point_type type);
@@ -109,7 +109,7 @@ namespace custom_planner
* Hàm này sẽ tính toán đường đi NURBS dựa trên các điểm điều khiển đã tìm được.
* Nó sẽ sử dụng các hàm từ CurveCommon và Spline_Inf để tính toán các điểm trên đường cong.
*/
std::vector<geometry_msgs::PoseStamped> calculateNURBSPath(vector<geometry_msgs::PoseStamped>& control_points,
std::vector<robot_geometry_msgs::PoseStamped> calculateNURBSPath(vector<robot_geometry_msgs::PoseStamped>& control_points,
bool reverse);
bool handleEdgeIntersection(vector<Pose>& savePosesOnEdge,
@@ -174,7 +174,7 @@ namespace custom_planner
private:
// vector<geometry_msgs::PoseStamped> control_points;
// vector<robot_geometry_msgs::PoseStamped> control_points;
bool normal_plag = false;
@@ -191,8 +191,8 @@ namespace custom_planner
* Hàm này sẽ cập nhật hướng (yaw) của các Pose trong saved_poses dựa trên hướng của đường đi NURBS.
* Nó sẽ tính toán góc yaw dựa trên vị trí của các Pose và cập nhật chúng.
*/
void updatePoseOrientation(std::vector<geometry_msgs::PoseStamped>& saved_poses,
std::vector<geometry_msgs::PoseStamped>& nurbs_path,
void updatePoseOrientation(std::vector<robot_geometry_msgs::PoseStamped>& saved_poses,
std::vector<robot_geometry_msgs::PoseStamped>& nurbs_path,
bool reverse);
/**
@@ -204,7 +204,7 @@ namespace custom_planner
* Nếu reverse là true, nó sẽ đảo ngược hướng của các Pose.
* Nếu reverse là false, nó sẽ giữ nguyên hướng của các Pose.
*/
static void setYawAllPosesOnEdge(std::vector<geometry_msgs::PoseStamped>& Poses,
static void setYawAllPosesOnEdge(std::vector<robot_geometry_msgs::PoseStamped>& Poses,
bool reverse = false);

90
src/Curve_common.cpp
View File

@@ -1,7 +1,7 @@
#include "custom_planner/Curve_common.h"
#include "custom_planner/conversion.h"
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <iostream>
@@ -10,10 +10,10 @@ Curve_common::Curve_common()
}
nav_msgs::Path Curve_common::Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, double t_intervel, std::string frame_id)
robot_nav_msgs::Path Curve_common::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_nav_msgs::Path line_result;
robot_geometry_msgs::PoseStamped current_pose;
line_result.header.frame_id = frame_id;
line_result.header.stamp = robot::Time::now();
@@ -38,10 +38,10 @@ nav_msgs::Path Curve_common::Generate_Line(geometry_msgs::Point start_point, geo
return line_result;
}
nav_msgs::Path Curve_common::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id)
robot_nav_msgs::Path Curve_common::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_nav_msgs::Path bezier_curve_result;
robot_geometry_msgs::PoseStamped current_pose;
EigenTrajectoryPoint::Vector temp_control_point_vec;
bezier_curve_result.header.frame_id = frame_id;
@@ -136,13 +136,13 @@ void Curve_common::ReadDiscreate2DPointFromLaunch(std::vector<Eigen::Vector3d, E
// }
}
visualization_msgs::Marker Curve_common::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 Curve_common::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;
@@ -154,7 +154,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(std::vector<Eigen::V
waypoints_marker.points.reserve(static_cast<int>(discreate_point.size()));
geometry_msgs::Point view_point;
robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{
view_point.x = discreate_point.at(i)(0);
@@ -165,13 +165,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(std::vector<Eigen::V
return waypoints_marker;
}
visualization_msgs::Marker Curve_common::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 Curve_common::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;
@@ -181,7 +181,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(std::vector<Eigen::
waypoints_marker.points.reserve(static_cast<int>(discreate_point.size()));
geometry_msgs::Point view_point;
robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{
view_point.x = discreate_point.at(i)(0);
@@ -192,13 +192,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(std::vector<Eigen::
return waypoints_marker;
}
visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale)
robot_visualization_msgs::Marker Curve_common::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;
@@ -210,7 +210,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint
waypoints_marker.points.reserve(static_cast<int>(discreate_point.size()));
geometry_msgs::Point view_point;
robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{
view_point.x = discreate_point.at(i).position(0);
@@ -221,13 +221,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(EigenTrajectoryPoint
return waypoints_marker;
}
visualization_msgs::Marker Curve_common::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 Curve_common::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;
@@ -240,7 +240,7 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoin
waypoints_marker.points.reserve(static_cast<int>(discreate_point.size()));
geometry_msgs::Point view_point;
robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{
view_point.x = discreate_point.at(i).position(0);
@@ -251,9 +251,9 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(EigenTrajectoryPoin
return waypoints_marker;
}
void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point)
void Curve_common::ShowDiscreatePoint(robot_visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point)
{
geometry_msgs::Point view_point;
robot_geometry_msgs::Point view_point;
for(int i = 0; i < static_cast<int>(discreate_point.size()); i++)
{
view_point.x = discreate_point.at(i).position(0);
@@ -262,9 +262,9 @@ void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenT
}
}
void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > discreate_point)
void Curve_common::ShowDiscreatePoint(robot_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 < static_cast<int>(discreate_point.size()); i++)
{
view_point.x = discreate_point.at(i)(0);
@@ -326,10 +326,10 @@ void Curve_common::ReadSplineInf(Spline_Inf *spline_inf, std::vector<double> wei
// }
}
nav_msgs::Path Curve_common::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id)
robot_nav_msgs::Path Curve_common::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_nav_msgs::Path bspline_curve_result;
robot_geometry_msgs::PoseStamped current_pose;
bspline_curve_result.header.frame_id = frame_id;
bspline_curve_result.header.stamp = robot::Time::now();
@@ -500,10 +500,10 @@ nav_msgs::Path Curve_common::Generate_BsplineCurve(Spline_Inf bspline_inf, doubl
return bspline_curve_result;
}
nav_msgs::Path Curve_common::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id)
robot_nav_msgs::Path Curve_common::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_nav_msgs::Path nurbs_curve_result;
robot_geometry_msgs::PoseStamped current_pose;
nurbs_curve_result.header.frame_id = frame_id;
nurbs_curve_result.header.stamp = robot::Time::now();
@@ -754,9 +754,9 @@ void Curve_common::CalculateDerivativeBasisFunc(Spline_Inf *spline_inf, double u
}
}
geometry_msgs::Point Curve_common::CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS)
robot_geometry_msgs::Point Curve_common::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;
@@ -878,11 +878,11 @@ geometry_msgs::Point Curve_common::CalculateDerivativeCurvePoint(Spline_Inf *spl
return derivative_curve_point;
}
nav_msgs::Path Curve_common::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id)
robot_nav_msgs::Path Curve_common::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id)
{
geometry_msgs::Point derivative_point_result;
nav_msgs::Path bspline_derivative_result;
geometry_msgs::PoseStamped current_pose;
robot_geometry_msgs::Point derivative_point_result;
robot_nav_msgs::Path bspline_derivative_result;
robot_geometry_msgs::PoseStamped current_pose;
bspline_derivative_result.header.frame_id = frame_id;
bspline_derivative_result.header.stamp = robot::Time::now();
@@ -921,11 +921,11 @@ nav_msgs::Path Curve_common::Generate_DerivativeBsplineCurve(Spline_Inf bspline_
return bspline_derivative_result;
}
nav_msgs::Path Curve_common::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id)
robot_nav_msgs::Path Curve_common::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id)
{
geometry_msgs::Point derivative_point_result;
nav_msgs::Path derivative_basis_result;
geometry_msgs::PoseStamped current_pose;
robot_geometry_msgs::Point derivative_point_result;
robot_nav_msgs::Path derivative_basis_result;
robot_geometry_msgs::PoseStamped current_pose;
derivative_basis_result.header.frame_id = frame_id;
derivative_basis_result.header.stamp = robot::Time::now();
@@ -1156,11 +1156,11 @@ double Curve_common::CalculateCurveLength(Spline_Inf spline_inf, double start_u,
return sum_length;
}
geometry_msgs::Point Curve_common::CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS)
robot_geometry_msgs::Point Curve_common::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 = static_cast<int>(spline_inf->control_point.size()) - 1;
//TODO: Check knot vector size and sequence is correect

108
src/custom_planner.cpp
View File

@@ -1,20 +1,12 @@
#include <custom_planner/custom_planner.h>
#include <nav_msgs/Path.h>
#include <costmap_2d/inflation_layer.h>
#include <robot_nav_msgs/Path.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_costmap_2d/inflation_layer.h>
#include <tf3/LinearMath/Quaternion.h>
#include <boost/dll/alias.hpp>
using namespace std;
namespace geometry_msgs
{
bool operator==(const Point &p1, const Point &p2)
{
return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z;
}
}
namespace custom_planner
{
CustomPlanner::CustomPlanner()
@@ -22,13 +14,13 @@ namespace custom_planner
{
}
CustomPlanner::CustomPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot)
CustomPlanner::CustomPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
: initialized_(false), costmap_robot_(NULL)
{
initialize(name, costmap_robot);
}
bool CustomPlanner::initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot)
bool CustomPlanner::initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
{
if (!initialized_)
{
@@ -73,9 +65,9 @@ namespace custom_planner
}
bool CustomPlanner::makePlan(const robot_protocol_msgs::Order& msg,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan)
const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
std::vector<robot_geometry_msgs::PoseStamped>& plan)
{
plan.clear();
@@ -146,7 +138,7 @@ namespace custom_planner
printf("[custom_planner] Using direct pathway");
for (size_t i = nearest_idx; i < posesOnPathWay.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 = posesOnPathWay[i].getX();
@@ -160,17 +152,17 @@ namespace custom_planner
// CASE 2: Start is far, goal is close - NURBS from start to pathway
printf("[custom_planner] Using start NURBS path reverse_(%x)", reverse_);
std::vector<geometry_msgs::PoseStamped> start_control_points;
std::vector<robot_geometry_msgs::PoseStamped> start_control_points;
bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints(
start_control_points, start, posesOnPathWay, START);
if (valid_start_nurbs) {
std::vector<geometry_msgs::PoseStamped> nurbs_path =
std::vector<robot_geometry_msgs::PoseStamped> nurbs_path =
merge_path_calc_.calculateNURBSPath(start_control_points, reverse_);
// Add NURBS path
for (const auto& pose : nurbs_path) {
geometry_msgs::PoseStamped new_pose = pose;
robot_geometry_msgs::PoseStamped new_pose = pose;
new_pose.header.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0;
@@ -179,7 +171,7 @@ namespace custom_planner
// Add remaining pathway
for (size_t i = merge_path_calc_.start_target_idx_; i < posesOnPathWay.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 = posesOnPathWay[i].getX();
@@ -191,7 +183,7 @@ namespace custom_planner
} else {
// Fallback to direct path
for (size_t i = nearest_idx; i < posesOnPathWay.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 = posesOnPathWay[i].getX();
@@ -206,14 +198,14 @@ namespace custom_planner
// CASE 3: Start is close, goal is far - NURBS from pathway to goal
printf("[custom_planner] Using goal NURBS path");
std::vector<geometry_msgs::PoseStamped> goal_control_points;
std::vector<robot_geometry_msgs::PoseStamped> goal_control_points;
bool valid_goal_nurbs = merge_path_calc_.findNURBSControlPoints(
goal_control_points, goal, posesOnPathWay, GOAL);
if (valid_goal_nurbs) {
// Add pathway up to goal connection point
for (size_t i = nearest_idx; i < merge_path_calc_.goal_target_idx_; 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 = posesOnPathWay[i].getX();
@@ -224,11 +216,11 @@ namespace custom_planner
}
// Add NURBS path to goal
std::vector<geometry_msgs::PoseStamped> nurbs_path =
std::vector<robot_geometry_msgs::PoseStamped> nurbs_path =
merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_);
for (const auto& pose : nurbs_path) {
geometry_msgs::PoseStamped new_pose = pose;
robot_geometry_msgs::PoseStamped new_pose = pose;
new_pose.header.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0;
@@ -237,7 +229,7 @@ namespace custom_planner
} else {
// Fallback: use entire pathway
for (size_t i = 0; i < posesOnPathWay.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 = posesOnPathWay[i].getX();
@@ -252,7 +244,7 @@ namespace custom_planner
// CASE 4: Both start and goal are far - Dual NURBS or special handling
printf("[custom_planner] Using dual NURBS path");
std::vector<geometry_msgs::PoseStamped> start_control_points, goal_control_points;
std::vector<robot_geometry_msgs::PoseStamped> start_control_points, goal_control_points;
bool valid_start_nurbs = merge_path_calc_.findNURBSControlPoints(
start_control_points, start, posesOnPathWay, START);
bool valid_goal_nurbs = merge_path_calc_.findNURBSControlPoints(
@@ -265,12 +257,12 @@ namespace custom_planner
costmap_robot_->getRobotPose(goal_control_points[0]);
goal_control_points[goal_control_points.size() - 1] = goal;
std::vector<geometry_msgs::PoseStamped> nurbs_path =
std::vector<robot_geometry_msgs::PoseStamped> nurbs_path =
merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_);
// Skip first point to avoid duplication
for (size_t i = 1; i < nurbs_path.size(); i++) {
geometry_msgs::PoseStamped new_pose = nurbs_path[i];
robot_geometry_msgs::PoseStamped new_pose = nurbs_path[i];
new_pose.header.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0;
@@ -281,10 +273,10 @@ namespace custom_planner
// Dual NURBS: Start NURBS + Pathway + Goal NURBS
// Add start NURBS path
std::vector<geometry_msgs::PoseStamped> start_nurbs_path =
std::vector<robot_geometry_msgs::PoseStamped> start_nurbs_path =
merge_path_calc_.calculateNURBSPath(start_control_points, false);
for (const auto& pose : start_nurbs_path) {
geometry_msgs::PoseStamped new_pose = pose;
robot_geometry_msgs::PoseStamped new_pose = pose;
new_pose.header.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0;
@@ -293,7 +285,7 @@ namespace custom_planner
// Add middle pathway segment
for (size_t i = (merge_path_calc_.start_target_idx_ - 1); i < merge_path_calc_.goal_target_idx_; 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 = posesOnPathWay[i].getX();
@@ -304,10 +296,10 @@ namespace custom_planner
}
// Add goal NURBS path
std::vector<geometry_msgs::PoseStamped> goal_nurbs_path =
std::vector<robot_geometry_msgs::PoseStamped> goal_nurbs_path =
merge_path_calc_.calculateNURBSPath(goal_control_points, reverse_);
for (const auto& pose : goal_nurbs_path) {
geometry_msgs::PoseStamped new_pose = pose;
robot_geometry_msgs::PoseStamped new_pose = pose;
new_pose.header.stamp = plan_time;
new_pose.header.frame_id = costmap_robot_->getGlobalFrameID();
new_pose.pose.position.z = 0;
@@ -318,7 +310,7 @@ namespace custom_planner
// Fallback: Direct pathway from nearest point
printf("[custom_planner] NURBS control points not found, using fallback path");
for (size_t i = nearest_idx; i < posesOnPathWay.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 = posesOnPathWay[i].getX();
@@ -331,7 +323,7 @@ namespace custom_planner
}
// ===== STEP 7: ADD FINAL GOAL POSE =====
geometry_msgs::PoseStamped pose_goal;
robot_geometry_msgs::PoseStamped pose_goal;
pose_goal.header.stamp = plan_time;
pose_goal.header.frame_id = costmap_robot_->getGlobalFrameID();
pose_goal.pose = goal.pose;
@@ -344,7 +336,7 @@ namespace custom_planner
}
// Publish plan for visualization
nav_msgs::Path gui_path;
robot_nav_msgs::Path gui_path;
gui_path.header.frame_id = costmap_robot_->getGlobalFrameID();
gui_path.header.stamp = plan_time;
gui_path.poses = plan;
@@ -354,9 +346,9 @@ namespace custom_planner
return true;
}
void CustomPlanner::transformFootprintToEdges(const geometry_msgs::Pose &robot_pose,
const std::vector<geometry_msgs::Point> &footprint,
std::vector<geometry_msgs::Point> &out_footprint)
void CustomPlanner::transformFootprintToEdges(const robot_geometry_msgs::Pose &robot_pose,
const std::vector<robot_geometry_msgs::Point> &footprint,
std::vector<robot_geometry_msgs::Point> &out_footprint)
{
out_footprint.resize(2 * footprint.size());
double yaw = data_convert::getYaw(robot_pose.orientation);
@@ -378,8 +370,8 @@ namespace custom_planner
}
bool CustomPlanner::makePlanWithOrder(robot_protocol_msgs::Order msg,
const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal)
const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal)
{
vector<Pose> savePosesOnEdge;
int count_two_curve_idx = 0;
@@ -479,7 +471,7 @@ namespace custom_planner
//Tính các điểm theo step mới
for(double u_test = 0; u_test <= 1; u_test += t_intervel_new)
{
geometry_msgs::Point curve_point;
robot_geometry_msgs::Point curve_point;
curve_point = CurveDesign->CalculateCurvePoint(input_spline_inf, u_test, true);
if(!std::isnan(curve_point.x)&&!std::isnan(curve_point.y))
posesOnEdge.push_back(Pose(curve_point.x, curve_point.y, 0));
@@ -504,19 +496,19 @@ namespace custom_planner
if(edges_info_.back().isCurve || delta_angle > EPSILON)
for (int idx_segment_A = 0; idx_segment_A + 1 < (int)savePosesOnEdge.size(); ++idx_segment_A)
{
geometry_msgs::Point gp1 ;
robot_geometry_msgs::Point gp1 ;
gp1.x = savePosesOnEdge[idx_segment_A].getX();
gp1.y = savePosesOnEdge[idx_segment_A].getY();
geometry_msgs::Point gp2;
robot_geometry_msgs::Point gp2;
gp2.x = savePosesOnEdge[idx_segment_A + 1].getX();
gp2.y = savePosesOnEdge[idx_segment_A + 1].getY();
for (int idx_segment_B = (int)posesOnEdge.size() - 1; idx_segment_B > 0; --idx_segment_B)
{
geometry_msgs::Point lp1;
robot_geometry_msgs::Point lp1;
lp1.x = posesOnEdge[idx_segment_B].getX();
lp1.y = posesOnEdge[idx_segment_B].getY();
geometry_msgs::Point lp2;
robot_geometry_msgs::Point lp2;
lp2.x = posesOnEdge[idx_segment_B - 1].getX();
lp2.y = posesOnEdge[idx_segment_B - 1].getY();
@@ -528,7 +520,7 @@ namespace custom_planner
auto intersection_point = computeIntersectionPoint(gp1, gp2, lp1, lp2);
if (intersection_point)
{
geometry_msgs::Point p = intersection_point.value();
robot_geometry_msgs::Point p = intersection_point.value();
// savePosesOnEdge.push_back(Pose(p.x, p.y, 0));
posesOnEdge.insert(posesOnEdge.begin(), Pose(p.x, p.y, 0));
}
@@ -613,10 +605,10 @@ namespace custom_planner
return status_calcAllYaw;
}
bool CustomPlanner::doIntersect(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1,
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2)
bool CustomPlanner::doIntersect(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const robot_geometry_msgs::Point& p2, const robot_geometry_msgs::Point& q2)
{
auto orientation = [](const geometry_msgs::Point& a, const geometry_msgs::Point& b, const geometry_msgs::Point& c)
auto orientation = [](const robot_geometry_msgs::Point& a, const robot_geometry_msgs::Point& b, const robot_geometry_msgs::Point& c)
{
double val = (b.y - a.y) * (c.x - b.x) -
(b.x - a.x) * (c.y - b.y);
@@ -632,8 +624,8 @@ namespace custom_planner
return (o1 != o2 && o3 != o4);
}
std::optional<geometry_msgs::Point> CustomPlanner::computeIntersectionPoint(const geometry_msgs::Point& p1, const geometry_msgs::Point& q1,
const geometry_msgs::Point& p2, const geometry_msgs::Point& q2)
std::optional<robot_geometry_msgs::Point> CustomPlanner::computeIntersectionPoint(const robot_geometry_msgs::Point& p1, const robot_geometry_msgs::Point& q1,
const robot_geometry_msgs::Point& p2, const robot_geometry_msgs::Point& q2)
{
double x1 = p1.x, y1 = p1.y;
double x2 = q1.x, y2 = q1.y;
@@ -659,7 +651,7 @@ namespace custom_planner
if (between(x1, x2, x) && between(y1, y2, y) &&
between(x3, x4, x) && between(y3, y4, y))
{
geometry_msgs::Point pt;
robot_geometry_msgs::Point pt;
pt.x = x;
pt.y = y;
pt.z = 0.0;
@@ -799,8 +791,8 @@ namespace custom_planner
return true;
}
bool CustomPlanner::calcAllYaw(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
bool CustomPlanner::calcAllYaw(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
vector<Pose>& savePosesOnEdge,
int& total_edge)
{
@@ -1174,7 +1166,7 @@ namespace custom_planner
}
}
bool CustomPlanner::checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose, const geometry_msgs::PoseStamped& goal)
bool CustomPlanner::checkYawEdgeEnd(Pose& start_edge_pose, Pose& end_edge_pose, const robot_geometry_msgs::PoseStamped& goal)
{
double angle = calculateAngle(start_edge_pose.getX(), start_edge_pose.getY(), end_edge_pose.getX(), end_edge_pose.getY());

44
src/merge_path_calc.cpp
View File

@@ -12,10 +12,10 @@ namespace custom_planner {
delete spline_inf;
}
std::vector<geometry_msgs::PoseStamped> MergePathCalc::calculateNURBSPath(vector<geometry_msgs::PoseStamped>& control_points, bool reverse)
std::vector<robot_geometry_msgs::PoseStamped> MergePathCalc::calculateNURBSPath(vector<robot_geometry_msgs::PoseStamped>& control_points, bool reverse)
{
std::vector<geometry_msgs::PoseStamped> nurbs_path;
std::vector<geometry_msgs::PoseStamped> saved_poses; // Lưu các pose đã tính toán
std::vector<robot_geometry_msgs::PoseStamped> nurbs_path;
std::vector<robot_geometry_msgs::PoseStamped> saved_poses; // Lưu các pose đã tính toán
if((int)nurbs_path.size() > 0)
{
@@ -61,10 +61,10 @@ namespace custom_planner {
for(double t = 0.0; t <= 1.0; t += step)
{
geometry_msgs::Point point = curve_design->CalculateCurvePoint(spline_inf, t, true);
robot_geometry_msgs::Point point = curve_design->CalculateCurvePoint(spline_inf, t, true);
if(!std::isnan(point.x) && !std::isnan(point.y))
{
geometry_msgs::PoseStamped pose;
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = robot::Time::now();
pose.pose.position = point;
pose.pose.orientation.x = 0.0;
@@ -112,7 +112,7 @@ namespace custom_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<double> segment_lengths;
static double step;
@@ -137,8 +137,8 @@ namespace custom_planner {
return length;
}
void MergePathCalc::updatePoseOrientation(std::vector<geometry_msgs::PoseStamped>& saved_poses,
std::vector<geometry_msgs::PoseStamped>& nurbs_path,
void MergePathCalc::updatePoseOrientation(std::vector<robot_geometry_msgs::PoseStamped>& saved_poses,
std::vector<robot_geometry_msgs::PoseStamped>& nurbs_path,
bool reverse)
{
double yaw;
@@ -312,8 +312,8 @@ namespace custom_planner {
return nearest_idx;
}
bool MergePathCalc::findNURBSControlPoints(vector<geometry_msgs::PoseStamped>& control_points,
const geometry_msgs::PoseStamped& pose,
bool MergePathCalc::findNURBSControlPoints(vector<robot_geometry_msgs::PoseStamped>& control_points,
const robot_geometry_msgs::PoseStamped& pose,
std::vector<Pose>& posesOnPathWay,
point_type type)
{
@@ -349,7 +349,7 @@ namespace custom_planner {
control_points.push_back(pose);
// Với bậc 2, thêm 1 điểm trung gian nằm trên đường thẳng
geometry_msgs::PoseStamped mid_pose;
robot_geometry_msgs::PoseStamped mid_pose;
mid_pose.pose.position.x = start_pose.getX() + dx/2.0;
mid_pose.pose.position.y = start_pose.getY() + dy/2.0;
mid_pose.pose.orientation = pose.pose.orientation;
@@ -358,7 +358,7 @@ namespace custom_planner {
control_points.push_back(mid_pose);
// Thêm điểm cuối
geometry_msgs::PoseStamped end_pose;
robot_geometry_msgs::PoseStamped end_pose;
end_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
end_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[start_target_idx_].getYaw());
@@ -376,7 +376,7 @@ namespace custom_planner {
// Tạo các control point cho NURBS
control_points.push_back(pose);
geometry_msgs::PoseStamped mid_pose;
robot_geometry_msgs::PoseStamped mid_pose;
mid_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
mid_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
mid_pose.pose.orientation = pose.pose.orientation;
@@ -434,14 +434,14 @@ namespace custom_planner {
control_points.push_back(pose);
geometry_msgs::PoseStamped mid_pose_straight;
robot_geometry_msgs::PoseStamped mid_pose_straight;
mid_pose_straight.pose.position.x = start_pose.getX() + dx/2.0;
mid_pose_straight.pose.position.y = start_pose.getY() + dy/2.0;
mid_pose_straight.pose.orientation = pose.pose.orientation;
mid_pose_straight.header = pose.header;
control_points.push_back(mid_pose_straight);
geometry_msgs::PoseStamped end_pose_straight;
robot_geometry_msgs::PoseStamped end_pose_straight;
end_pose_straight.pose.position.x = posesOnPathWay[start_target_idx_].getX();
end_pose_straight.pose.position.y = posesOnPathWay[start_target_idx_].getY();
end_pose_straight.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[start_target_idx_].getYaw());
@@ -451,7 +451,7 @@ namespace custom_planner {
return true;
}
geometry_msgs::PoseStamped end_pose;
robot_geometry_msgs::PoseStamped end_pose;
end_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
end_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0);
@@ -482,7 +482,7 @@ namespace custom_planner {
goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
// Thêm điểm đầu từ đường đã có
geometry_msgs::PoseStamped start_pose;
robot_geometry_msgs::PoseStamped start_pose;
start_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
@@ -495,7 +495,7 @@ namespace custom_planner {
double dy = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
// Thêm 1 điểm trung gian cho bậc 2
geometry_msgs::PoseStamped mid_pose;
robot_geometry_msgs::PoseStamped mid_pose;
mid_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX() + dx/2.0;
mid_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY() + dy/2.0;
mid_pose.pose.orientation = start_pose.pose.orientation;
@@ -553,7 +553,7 @@ namespace custom_planner {
goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
geometry_msgs::PoseStamped start_pose_straight;
robot_geometry_msgs::PoseStamped start_pose_straight;
start_pose_straight.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
start_pose_straight.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
start_pose_straight.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
@@ -563,7 +563,7 @@ namespace custom_planner {
double dx_straight = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX();
double dy_straight = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
geometry_msgs::PoseStamped mid_pose_straight;
robot_geometry_msgs::PoseStamped mid_pose_straight;
mid_pose_straight.pose.position.x = posesOnPathWay[goal_target_idx_].getX() + dx_straight/2.0;
mid_pose_straight.pose.position.y = posesOnPathWay[goal_target_idx_].getY() + dy_straight/2.0;
mid_pose_straight.pose.orientation = start_pose_straight.pose.orientation;
@@ -575,7 +575,7 @@ namespace custom_planner {
}
// Thêm điểm đầu từ đường đã có
geometry_msgs::PoseStamped start_pose;
robot_geometry_msgs::PoseStamped start_pose;
start_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
@@ -583,7 +583,7 @@ namespace custom_planner {
start_pose.header = pose.header;
control_points.push_back(start_pose);
geometry_msgs::PoseStamped mid_pose;
robot_geometry_msgs::PoseStamped mid_pose;
mid_pose.pose.position.x = posesOnPathWay[idx].getX();
mid_pose.pose.position.y = posesOnPathWay[idx].getY();
mid_pose.pose.orientation = pose.pose.orientation;

50
src/plan_for_retry.cpp
View File

@@ -9,7 +9,7 @@
#include <tf2/convert.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
using namespace std;
@@ -41,7 +41,7 @@ bool isThetaValid(double theta)
return result;
}
double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose, geometry_msgs::Pose& next_Pose)
double computeDeltaAngleStartOfPlan(double theta, robot_geometry_msgs::Pose& startPose, robot_geometry_msgs::Pose& next_Pose)
{
double delta_angle = 0;
if(isThetaValid(theta))
@@ -66,7 +66,7 @@ double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose
return delta_angle;
}
double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose& endPose, geometry_msgs::Pose& prev_Pose)
double computeDeltaAngleEndOfPlan(double theta, robot_geometry_msgs::Pose& endPose, robot_geometry_msgs::Pose& prev_Pose)
{
double delta_angle = 0;
if(isThetaValid(theta))
@@ -92,8 +92,8 @@ double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose& endPose, ge
}
// Hàm chia đoạn thẳng AB thành các đoạn có độ dài d
std::vector<geometry_msgs::PoseStamped> divideSegment(geometry_msgs::PoseStamped& A, geometry_msgs::PoseStamped& B, double d) {
std::vector<geometry_msgs::PoseStamped> Poses;
std::vector<robot_geometry_msgs::PoseStamped> divideSegment(robot_geometry_msgs::PoseStamped& A, robot_geometry_msgs::PoseStamped& B, double d) {
std::vector<robot_geometry_msgs::PoseStamped> Poses;
double xAB = B.pose.position.x - A.pose.position.x;
double yAB = B.pose.position.y - A.pose.position.y;
double length = sqrt(xAB*xAB + yAB*yAB);
@@ -106,7 +106,7 @@ std::vector<geometry_msgs::PoseStamped> divideSegment(geometry_msgs::PoseStamped
// Tính toán tọa độ của các điểm trên đoạn AB
double ratio = d / length;
for (int i = 1; i <= segments; ++i) {
geometry_msgs::PoseStamped p;
robot_geometry_msgs::PoseStamped p;
double p_x = A.pose.position.x + (B.pose.position.x - A.pose.position.x) * ratio * i;
double p_y = A.pose.position.y + (B.pose.position.y - A.pose.position.y) * ratio * i;
p.pose.position.x = p_x;
@@ -181,14 +181,14 @@ std::vector<geometry_msgs::PoseStamped> divideSegment(geometry_msgs::PoseStamped
// pose_C: tâm của cung tròn AB
// result_plan: vector chứa plan kết quả
bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
int indexOfPoseA, geometry_msgs::PoseStamped& pose_B, geometry_msgs::PoseStamped& pose_B_behind,
geometry_msgs::PoseStamped& pose_C, std::vector<geometry_msgs::PoseStamped>& result_plan)
bool makePlanForRetry(std::vector<robot_geometry_msgs::PoseStamped>& current_plan,
int indexOfPoseA, robot_geometry_msgs::PoseStamped& pose_B, robot_geometry_msgs::PoseStamped& pose_B_behind,
robot_geometry_msgs::PoseStamped& pose_C, std::vector<robot_geometry_msgs::PoseStamped>& result_plan)
{
bool result = false;
std::vector<geometry_msgs::PoseStamped> PlanRetry_1;
std::vector<geometry_msgs::PoseStamped> PlanRetry_2;
std::vector<geometry_msgs::PoseStamped> PlanRetry_3;
std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_1;
std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_2;
std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_3;
if(current_plan.empty()||current_plan.size()<2)
{
@@ -196,7 +196,7 @@ bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
return false;
}
geometry_msgs::PoseStamped pose_A;
robot_geometry_msgs::PoseStamped pose_A;
pose_A = current_plan[indexOfPoseA];
// Tính ra PlanRetry_1 điểm retry tại Pose_A
@@ -228,7 +228,7 @@ bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
pose_B.pose, pose_A.pose) >= 0))
{
std::vector<geometry_msgs::PoseStamped> planSegment_AB;
std::vector<robot_geometry_msgs::PoseStamped> planSegment_AB;
planSegment_AB = divideSegment(pose_A, pose_B, 0.1);
PlanRetry_2.assign(planSegment_AB.begin(), planSegment_AB.end());
}
@@ -285,7 +285,7 @@ bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
double angle_tmp = angleCA + angleACB*i;
double xP = pose_C.pose.position.x + rCA*cos(angle_tmp);
double yP = pose_C.pose.position.y + rCA*sin(angle_tmp);
geometry_msgs::PoseStamped p;
robot_geometry_msgs::PoseStamped p;
p.pose.position.x = xP;
p.pose.position.y = yP;
p.pose.position.z = 0;
@@ -299,7 +299,7 @@ bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
double angle_tmp = angleCA - angleACB*i;
double xP = pose_C.pose.position.x + rCA*cos(angle_tmp);
double yP = pose_C.pose.position.y + rCA*sin(angle_tmp);
geometry_msgs::PoseStamped p;
robot_geometry_msgs::PoseStamped p;
p.pose.position.x = xP;
p.pose.position.y = yP;
p.pose.position.z = 0;
@@ -420,13 +420,13 @@ bool makePlanForRetry(std::vector<geometry_msgs::PoseStamped>& current_plan,
// pose_C: tâm của cung tròn AB
// result_plan: vector chứa plan kết quả
bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseStamped& pose_B, geometry_msgs::PoseStamped& pose_B_behind,
geometry_msgs::PoseStamped& pose_C, std::vector<geometry_msgs::PoseStamped>& result_plan)
bool makePlanForRetry(robot_geometry_msgs::PoseStamped& pose_A, robot_geometry_msgs::PoseStamped& pose_B, robot_geometry_msgs::PoseStamped& pose_B_behind,
robot_geometry_msgs::PoseStamped& pose_C, std::vector<robot_geometry_msgs::PoseStamped>& result_plan)
{
bool result = false;
std::vector<geometry_msgs::PoseStamped> PlanRetry_1;
std::vector<geometry_msgs::PoseStamped> PlanRetry_2;
std::vector<geometry_msgs::PoseStamped> PlanRetry_3;
std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_1;
std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_2;
std::vector<robot_geometry_msgs::PoseStamped> PlanRetry_3;
ROS_INFO("[makePlanForRetry] pose_A: %f, %f, %f pose_B: %f, %f, %f pose_C: %f, %f",
pose_A.pose.position.x, pose_A.pose.position.y,
@@ -459,7 +459,7 @@ bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseSta
computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),
pose_B.pose, pose_A.pose) >= 0))
{
std::vector<geometry_msgs::PoseStamped> planSegment_AB;
std::vector<robot_geometry_msgs::PoseStamped> planSegment_AB;
planSegment_AB = divideSegment(pose_A, pose_B, 0.1);
PlanRetry_2.assign(planSegment_AB.begin(), planSegment_AB.end());
}
@@ -516,7 +516,7 @@ bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseSta
double angle_tmp = angleCA + angleACB*i;
double xP = pose_C.pose.position.x + rCA*cos(angle_tmp);
double yP = pose_C.pose.position.y + rCA*sin(angle_tmp);
geometry_msgs::PoseStamped p;
robot_geometry_msgs::PoseStamped p;
p.pose.position.x = xP;
p.pose.position.y = yP;
p.pose.position.z = 0;
@@ -530,7 +530,7 @@ bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseSta
double angle_tmp = angleCA - angleACB*i;
double xP = pose_C.pose.position.x + rCA*cos(angle_tmp);
double yP = pose_C.pose.position.y + rCA*sin(angle_tmp);
geometry_msgs::PoseStamped p;
robot_geometry_msgs::PoseStamped p;
p.pose.position.x = xP;
p.pose.position.y = yP;
p.pose.position.z = 0;
@@ -648,7 +648,7 @@ bool makePlanForRetry(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseSta
// pose_B: điểm đích trên cung tròn
// pose_C: tâm của cung tròn AB (kết quả)
bool findCenterOfCurve(geometry_msgs::PoseStamped& pose_A, geometry_msgs::PoseStamped& pose_B, geometry_msgs::PoseStamped& pose_C)
bool findCenterOfCurve(robot_geometry_msgs::PoseStamped& pose_A, robot_geometry_msgs::PoseStamped& pose_B, robot_geometry_msgs::PoseStamped& pose_C)
{
// nếu hướng của vector AB và hướng của pose_B tạo với nhau một góc ~0 độ hoặc ~180 độ -> điểm C sẽ gần xấp xỉ với trung điểm của đoạn thẳng AB.
if((computeDeltaAngleEndOfPlan(getYaw(pose_B.pose.orientation.x, pose_B.pose.orientation.y, pose_B.pose.orientation.z, pose_B.pose.orientation.w),