diff --git a/CMakeLists.txt b/CMakeLists.txt index c01b697..0044c04 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,8 +44,8 @@ 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_time diff --git a/include/custom_planner/Curve_common.h b/include/custom_planner/Curve_common.h index d4e9a23..014ad0d 100755 --- a/include/custom_planner/Curve_common.h +++ b/include/custom_planner/Curve_common.h @@ -7,9 +7,9 @@ // #include // #include -#include +#include #include -#include +#include #include "custom_planner/color.h" struct Spline_Inf @@ -47,12 +47,12 @@ class Curve_common { public: Curve_common(); - nav_msgs::Path Generate_Line(robot_geometry_msgs::Point start_point, robot_geometry_msgs::Point end_point, double t_intervel, std::string frame_id); - nav_msgs::Path Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id); - nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id); - nav_msgs::Path Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id); - 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); robot_geometry_msgs::Point CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS); @@ -67,15 +67,15 @@ class Curve_common void ReadSplineInf(Spline_Inf *bspline_inf, std::vector weight_vector, bool use_limit_derivative_fitting); void ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector *input_point, std::vector file_discreate_point); void ReadDiscreate2DPointFromLaunch(std::vector > *input_point, std::vector file_discreate_point); - void ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point); - void ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector > discreate_point); + void ShowDiscreatePoint(robot_visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point); + void ShowDiscreatePoint(robot_visualization_msgs::Marker *points, std::vector > 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 > discreate_point, const std::string& frame_id, const std::string& name, double scale); - visualization_msgs::Marker ShowDiscreatePoint2(std::vector > 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 > discreate_point, const std::string& frame_id, const std::string& name, double scale); + robot_visualization_msgs::Marker ShowDiscreatePoint2(std::vector > discreate_point, const std::string& frame_id, robot_std_msgs::ColorRGBA point_color, const std::string& name, double scale); private: diff --git a/include/custom_planner/color.h b/include/custom_planner/color.h index 0f7640d..ce80d18 100755 --- a/include/custom_planner/color.h +++ b/include/custom_planner/color.h @@ -1,14 +1,14 @@ #ifndef CUSTOM_PLANNER_COLOR_H_ #define CUSTOM_PLANNER_COLOR_H_ -#include +#include 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; diff --git a/include/custom_planner/custom_planner.h b/include/custom_planner/custom_planner.h index a5b09a2..3ab1395 100755 --- a/include/custom_planner/custom_planner.h +++ b/include/custom_planner/custom_planner.h @@ -10,7 +10,7 @@ using namespace std; // ROS #include -#include +#include // Costmap used for the map representation #include diff --git a/src/Curve_common.cpp b/src/Curve_common.cpp index ae8de29..bcf294c 100755 --- a/src/Curve_common.cpp +++ b/src/Curve_common.cpp @@ -10,9 +10,9 @@ Curve_common::Curve_common() } -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) +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; + robot_nav_msgs::Path line_result; robot_geometry_msgs::PoseStamped current_pose; line_result.header.frame_id = frame_id; @@ -38,9 +38,9 @@ nav_msgs::Path Curve_common::Generate_Line(robot_geometry_msgs::Point start_poin 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; + robot_nav_msgs::Path bezier_curve_result; robot_geometry_msgs::PoseStamped current_pose; EigenTrajectoryPoint::Vector temp_control_point_vec; @@ -136,13 +136,13 @@ void Curve_common::ReadDiscreate2DPointFromLaunch(std::vector > discreate_point, const std::string& frame_id, const std::string& name, double scale) +robot_visualization_msgs::Marker Curve_common::ShowDiscreatePoint(std::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; @@ -165,13 +165,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint(std::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(std::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.a = 0.75; waypoints_marker.ns = name; @@ -192,13 +192,13 @@ visualization_msgs::Marker Curve_common::ShowDiscreatePoint2(std::vector(discreate_point.size()); i++) @@ -262,7 +262,7 @@ void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenT } } -void Curve_common::ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector > discreate_point) +void Curve_common::ShowDiscreatePoint(robot_visualization_msgs::Marker *points, std::vector > discreate_point) { robot_geometry_msgs::Point view_point; for(int i = 0; i < static_cast(discreate_point.size()); i++) @@ -326,9 +326,9 @@ void Curve_common::ReadSplineInf(Spline_Inf *spline_inf, std::vector 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; + robot_nav_msgs::Path bspline_curve_result; robot_geometry_msgs::PoseStamped current_pose; bspline_curve_result.header.frame_id = frame_id; @@ -500,9 +500,9 @@ 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; + robot_nav_msgs::Path nurbs_curve_result; robot_geometry_msgs::PoseStamped current_pose; nurbs_curve_result.header.frame_id = frame_id; @@ -878,10 +878,10 @@ robot_geometry_msgs::Point Curve_common::CalculateDerivativeCurvePoint(Spline_In 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) { robot_geometry_msgs::Point derivative_point_result; - nav_msgs::Path bspline_derivative_result; + robot_nav_msgs::Path bspline_derivative_result; robot_geometry_msgs::PoseStamped current_pose; bspline_derivative_result.header.frame_id = frame_id; @@ -921,10 +921,10 @@ 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) { robot_geometry_msgs::Point derivative_point_result; - nav_msgs::Path derivative_basis_result; + robot_nav_msgs::Path derivative_basis_result; robot_geometry_msgs::PoseStamped current_pose; derivative_basis_result.header.frame_id = frame_id; diff --git a/src/custom_planner.cpp b/src/custom_planner.cpp index b9d0873..3f6c36b 100755 --- a/src/custom_planner.cpp +++ b/src/custom_planner.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include #include @@ -336,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;