diff --git a/CMakeLists.txt b/CMakeLists.txt index 16b9916..44b9a16 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,8 +44,8 @@ add_library(dock_planner # --- Link các thư viện phụ thuộc --- target_link_libraries(dock_planner ${Boost_LIBRARIES} # Boost - visualization_msgs - nav_msgs + robot_visualization_msgs + robot_nav_msgs tf3 tf3_geometry_msgs robot_time diff --git a/include/dock_planner/dock_calc.h b/include/dock_planner/dock_calc.h index da534f9..077d913 100644 --- a/include/dock_planner/dock_calc.h +++ b/include/dock_planner/dock_calc.h @@ -4,9 +4,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include diff --git a/include/dock_planner/utils/color.h b/include/dock_planner/utils/color.h index 263d901..d55aaed 100644 --- a/include/dock_planner/utils/color.h +++ b/include/dock_planner/utils/color.h @@ -1,14 +1,14 @@ #ifndef COLOR_H_ #define 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/dock_planner/utils/curve_common.h b/include/dock_planner/utils/curve_common.h index 440010f..59d74df 100644 --- a/include/dock_planner/utils/curve_common.h +++ b/include/dock_planner/utils/curve_common.h @@ -2,9 +2,9 @@ #define CURVE_COMMON_H_ #include -#include +#include #include -#include +#include #include "color.h" #include @@ -43,12 +43,12 @@ class CurveCommon { public: CurveCommon(); - 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); @@ -65,15 +65,15 @@ public: 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/src/dock_planner.cpp b/src/dock_planner.cpp index a412f29..ae599d5 100644 --- a/src/dock_planner.cpp +++ b/src/dock_planner.cpp @@ -63,7 +63,7 @@ namespace dock_planner calc_plan_to_dock_.calib_safety_distance_ = calib_safety_distance_; - // plan_pub_ = private_nh.advertise("plan", 1); + // plan_pub_ = private_nh.advertise("plan", 1); robot_nav_2d_msgs::Polygon2D footprint_dock ;//= robot_nav_2d_utils::footprintFromParams(private_nh,false); @@ -114,7 +114,7 @@ namespace dock_planner // void DockPlanner::publishPlan(const std::vector& plan) // { - // nav_msgs::Path path_msg; + // robot_nav_msgs::Path path_msg; // path_msg.header.stamp = robot::Time::now(); // path_msg.header.frame_id = frame_id_; // path_msg.poses = plan; diff --git a/src/utils/curve_common.cpp b/src/utils/curve_common.cpp index 8944a3c..cfd439a 100644 --- a/src/utils/curve_common.cpp +++ b/src/utils/curve_common.cpp @@ -7,9 +7,9 @@ CurveCommon::CurveCommon() { } -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) +robot_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; + robot_nav_msgs::Path line_result; robot_geometry_msgs::PoseStamped current_pose; line_result.header.frame_id = frame_id; @@ -35,9 +35,9 @@ nav_msgs::Path CurveCommon::Generate_Line(robot_geometry_msgs::Point start_point return line_result; } -nav_msgs::Path CurveCommon::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id) +robot_nav_msgs::Path CurveCommon::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; @@ -133,13 +133,13 @@ void CurveCommon::ReadDiscreate2DPointFromLaunch(std::vector> discreate_point, const std::string &frame_id, const std::string &name, double scale) +robot_visualization_msgs::Marker CurveCommon::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; @@ -162,13 +162,13 @@ visualization_msgs::Marker CurveCommon::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 CurveCommon::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; @@ -189,13 +189,13 @@ visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(std::vector> discreate_point) +void CurveCommon::ShowDiscreatePoint(robot_visualization_msgs::Marker *points, std::vector> discreate_point) { robot_geometry_msgs::Point view_point; for (int i = 0; i < discreate_point.size(); i++) @@ -322,9 +322,9 @@ void CurveCommon::ReadSplineInf(Spline_Inf *spline_inf, std::vector weig // } } -nav_msgs::Path CurveCommon::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id) +robot_nav_msgs::Path CurveCommon::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; @@ -495,9 +495,9 @@ nav_msgs::Path CurveCommon::Generate_BsplineCurve(Spline_Inf bspline_inf, double return bspline_curve_result; } -nav_msgs::Path CurveCommon::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id) +robot_nav_msgs::Path CurveCommon::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; @@ -873,10 +873,10 @@ robot_geometry_msgs::Point CurveCommon::CalculateDerivativeCurvePoint(Spline_Inf return derivative_curve_point; } -nav_msgs::Path CurveCommon::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id) +robot_nav_msgs::Path CurveCommon::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; @@ -916,10 +916,10 @@ nav_msgs::Path CurveCommon::Generate_DerivativeBsplineCurve(Spline_Inf bspline_i return bspline_derivative_result; } -nav_msgs::Path CurveCommon::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id) +robot_nav_msgs::Path CurveCommon::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;