From 89f435633c2783920062740adddbcb3f523e4348 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Tue, 16 Dec 2025 16:18:35 +0700 Subject: [PATCH] update --- CMakeLists.txt | 19 +- config/{mprim => }/costmap_common_params.yaml | 0 config/{mprim => }/costmap_global_params.yaml | 0 ...lobal_params_plugins_no_virtual_walls.yaml | 0 config/{mprim => }/costmap_local_params.yaml | 0 .../{mprim => }/move_base_common_params.yaml | 0 .../pnkx_local_planner_params.yaml | 2 +- .../{mprim => }/two_points_global_params.yaml | 0 src/APIs/c_api/CMakeLists.txt | 4 +- .../Cores/score_algorithm/CMakeLists.txt | 6 +- .../score_algorithm/include/angles/angles.h | 26 - .../include/score_algorithm/score_algorithm.h | 6 +- .../score_algorithm/trajectory_generator.h | 7 +- .../Libraries/angles/include/angles/angles.h | 22 + .../Libraries/kalman/CMakeLists.txt | 4 +- .../Libraries/mkt_algorithm/CMakeLists.txt | 19 +- .../mkt_algorithm/diff/diff_go_straight.h | 8 +- .../diff/diff_predictive_trajectory.h | 41 +- .../mkt_algorithm/diff/diff_rotate_to_goal.h | 8 +- .../Libraries/mkt_algorithm/package.xml | 3 - .../src/diff/diff_go_straight.cpp | 52 +- .../src/diff/diff_predictive_trajectory.cpp | 560 +++++------------- .../src/diff/diff_rotate_to_goal.cpp | 25 +- .../two_points_planner/CMakeLists.txt | 4 +- .../src/two_points_planner.cpp | 204 +++---- src/Libraries/common_msgs | 2 +- src/Libraries/costmap_2d | 2 +- src/Libraries/nav_2d_utils/CMakeLists.txt | 12 + .../include/nav_2d_utils/footprint.h | 4 +- .../include/nav_2d_utils/odom_subscriber.h | 4 +- .../include/nav_2d_utils/parameters.h | 22 +- .../include/nav_2d_utils/plugin_mux.h | 2 +- .../include/nav_2d_utils/polygons.h | 4 +- src/Libraries/robot_cpp | 2 +- .../Cores/nav_core2/CMakeLists.txt | 2 + .../nav_core2/include/nav_core2/costmap.h | 4 +- .../include/nav_core2/global_planner.h | 2 +- .../include/nav_core2/local_planner.h | 2 +- .../Cores/nav_core_adapter/CMakeLists.txt | 4 +- .../nav_core_adapter/costmap_adapter.h | 2 +- .../nav_core_adapter/global_planner_adapter.h | 2 +- .../nav_core_adapter/local_planner_adapter.h | 5 +- .../nav_core_adapter/src/costmap_adapter.cpp | 4 +- .../src/global_planner_adapter.cpp | 2 +- .../src/local_planner_adapter.cpp | 37 +- .../Packages/move_base/CMakeLists.txt | 4 +- .../Packages/move_base/src/move_base.cpp | 337 ++++++----- 47 files changed, 600 insertions(+), 881 deletions(-) rename config/{mprim => }/costmap_common_params.yaml (100%) rename config/{mprim => }/costmap_global_params.yaml (100%) rename config/{mprim => }/costmap_global_params_plugins_no_virtual_walls.yaml (100%) rename config/{mprim => }/costmap_local_params.yaml (100%) rename config/{mprim => }/move_base_common_params.yaml (100%) rename config/{mprim => }/pnkx_local_planner_params.yaml (99%) rename config/{mprim => }/two_points_global_params.yaml (100%) delete mode 100644 src/Algorithms/Cores/score_algorithm/include/angles/angles.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 1d6da81..6c1e008 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,9 @@ if(NOT CMAKE_BUILD_TYPE) endif() # Flags chung -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +# Warning flags - disabled to suppress warnings during build +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") @@ -64,10 +66,6 @@ if (NOT TARGET voxel_grid) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/voxel_grid) endif() -if (NOT TARGET nav_grid) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/nav_grid) -endif() - if (NOT TARGET nav_2d_msgs) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/nav_2d_msgs) endif() @@ -84,11 +82,14 @@ if (NOT TARGET nav_core) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core) endif() +if (NOT TARGET nav_grid) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/nav_grid) +endif() + if (NOT TARGET nav_core2) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core2) endif() - if (NOT TARGET nav_core_adapter) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core_adapter) endif() @@ -113,9 +114,9 @@ if (NOT TARGET score_algorithm) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm) endif() -#if (NOT TARGET mkt_algorithm) -# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm) -#endif() +if (NOT TARGET mkt_algorithm) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm) +endif() if (NOT TARGET two_points_planner) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner) diff --git a/config/mprim/costmap_common_params.yaml b/config/costmap_common_params.yaml similarity index 100% rename from config/mprim/costmap_common_params.yaml rename to config/costmap_common_params.yaml diff --git a/config/mprim/costmap_global_params.yaml b/config/costmap_global_params.yaml similarity index 100% rename from config/mprim/costmap_global_params.yaml rename to config/costmap_global_params.yaml diff --git a/config/mprim/costmap_global_params_plugins_no_virtual_walls.yaml b/config/costmap_global_params_plugins_no_virtual_walls.yaml similarity index 100% rename from config/mprim/costmap_global_params_plugins_no_virtual_walls.yaml rename to config/costmap_global_params_plugins_no_virtual_walls.yaml diff --git a/config/mprim/costmap_local_params.yaml b/config/costmap_local_params.yaml similarity index 100% rename from config/mprim/costmap_local_params.yaml rename to config/costmap_local_params.yaml diff --git a/config/mprim/move_base_common_params.yaml b/config/move_base_common_params.yaml similarity index 100% rename from config/mprim/move_base_common_params.yaml rename to config/move_base_common_params.yaml diff --git a/config/mprim/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml similarity index 99% rename from config/mprim/pnkx_local_planner_params.yaml rename to config/pnkx_local_planner_params.yaml index 5b76e45..f7e6e6d 100644 --- a/config/mprim/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -4,7 +4,7 @@ docking_planner_name: PNKXDockingLocalPlanner go_straight_planner_name: PNKXGoStraightLocalPlanner rotate_planner_name: PNKXRotateLocalPlanner -base_local_planner: nav_core_adapter::LocalPlannerAdapter +base_local_planner: LocalPlannerAdapter yaw_goal_tolerance: 0.017 xy_goal_tolerance: 0.03 min_approach_linear_velocity: 0.06 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) diff --git a/config/mprim/two_points_global_params.yaml b/config/two_points_global_params.yaml similarity index 100% rename from config/mprim/two_points_global_params.yaml rename to config/two_points_global_params.yaml diff --git a/src/APIs/c_api/CMakeLists.txt b/src/APIs/c_api/CMakeLists.txt index ce964f8..5ad0c41 100644 --- a/src/APIs/c_api/CMakeLists.txt +++ b/src/APIs/c_api/CMakeLists.txt @@ -69,7 +69,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON) option(BUILD_TESTS "Build test programs" OFF) # Flags biên dịch -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +# Warning flags - disabled to suppress warnings during build +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") diff --git a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt index a87d9d5..6de2889 100644 --- a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt +++ b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt @@ -25,6 +25,8 @@ add_library(score_algorithm src/score_algorithm.cpp) target_link_libraries(score_algorithm PUBLIC ${PACKAGES_DIR} + robot::node_handle + robot::console PRIVATE Boost::filesystem Boost::system @@ -55,7 +57,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON) option(BUILD_TESTS "Build test programs" OFF) # Flags biên dịch -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +# Warning flags - disabled to suppress warnings during build +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") diff --git a/src/Algorithms/Cores/score_algorithm/include/angles/angles.h b/src/Algorithms/Cores/score_algorithm/include/angles/angles.h deleted file mode 100644 index 2b8c866..0000000 --- a/src/Algorithms/Cores/score_algorithm/include/angles/angles.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef ANGLES_ANGLES_H -#define ANGLES_ANGLES_H - -#include - -namespace angles -{ - -/** - * @brief Normalize an angle to the range [-π, π] - * @param angle The angle in radians to normalize - * @return The normalized angle in the range [-π, π] - */ -inline double normalize_angle(double angle) -{ - while (angle > M_PI) - angle -= 2.0 * M_PI; - while (angle < -M_PI) - angle += 2.0 * M_PI; - return angle; -} - -} // namespace angles - -#endif // ANGLES_ANGLES_H - diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h index 76ed97f..d57e0d3 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h @@ -1,7 +1,7 @@ #ifndef _SCORE_ALGORITHM_H_INCLUDED__ #define _SCORE_ALGORITHM_H_INCLUDED__ -#include +#include #include #include #include @@ -24,7 +24,7 @@ namespace score_algorithm class ScoreAlgorithm { public: - using Ptr = boost::shared_ptr; + using Ptr = std::shared_ptr; inline double smoothstep(double x) { @@ -38,7 +38,7 @@ namespace score_algorithm * @param tf TFListener pointer * @param costmap_ros Costmap pointer */ - virtual void initialize(YAML::Node &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_ros, + virtual void initialize(robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) = 0; /** diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h index a88a962..209ce1a 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h @@ -1,7 +1,7 @@ #ifndef _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H #define _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H -#include +#include #include #include #include @@ -39,7 +39,7 @@ namespace score_algorithm * @brief Initialize parameters as needed * @param nh NodeHandle to read parameters from */ - virtual void initialize(const YAML::Node &nh) = 0; + virtual void initialize(const robot::NodeHandle &nh) = 0; /** * @brief Reset the state (if any) when the planner gets a new goal @@ -128,7 +128,8 @@ namespace score_algorithm const nav_2d_msgs::Twist2D &start_vel, const nav_2d_msgs::Twist2D &cmd_vel) = 0; - virtual YAML::Node getNodeHandle() = 0; + virtual robot::NodeHandle getNodeHandle() = 0; + }; } // namespace score_algorithm diff --git a/src/Algorithms/Libraries/angles/include/angles/angles.h b/src/Algorithms/Libraries/angles/include/angles/angles.h index 2b8c866..4efd80e 100644 --- a/src/Algorithms/Libraries/angles/include/angles/angles.h +++ b/src/Algorithms/Libraries/angles/include/angles/angles.h @@ -20,6 +20,28 @@ inline double normalize_angle(double angle) return angle; } +/** + * @brief Calculate the shortest angular distance between two angles + * + * Computes the shortest angular distance from angle 'from' to angle 'to'. + * The result is in the range [-π, π], where positive values indicate + * counter-clockwise rotation and negative values indicate clockwise rotation. + * + * @param from The starting angle in radians + * @param to The target angle in radians + * @return The shortest angular distance in radians, in the range [-π, π] + * + * @code + * double diff = angles::shortest_angular_distance(0.0, M_PI/2); // Returns π/2 + * double diff2 = angles::shortest_angular_distance(M_PI, 0.0); // Returns -π (or π, normalized) + * @endcode + */ +inline double shortest_angular_distance(double from, double to) +{ + double diff = to - from; + return normalize_angle(diff); +} + } // namespace angles #endif // ANGLES_ANGLES_H diff --git a/src/Algorithms/Libraries/kalman/CMakeLists.txt b/src/Algorithms/Libraries/kalman/CMakeLists.txt index 6c3ea7d..b6bb455 100755 --- a/src/Algorithms/Libraries/kalman/CMakeLists.txt +++ b/src/Algorithms/Libraries/kalman/CMakeLists.txt @@ -78,7 +78,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON) option(BUILD_TESTS "Build test programs" OFF) # Flags biên dịch -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +# Warning flags - disabled to suppress warnings during build +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") diff --git a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt index 46560bf..3532a2e 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt @@ -22,16 +22,15 @@ include_directories( # Dependencies packages set(PACKAGES_DIR - geometry_msgs - score_algorithm - nav_2d_msgs - nav_2d_utils - kalman + geometry_msgs + score_algorithm + nav_2d_msgs + nav_2d_utils + kalman angles nav_grid - costmap_2d + costmap_2d sensor_msgs - visualization_msgs std_msgs ) @@ -53,6 +52,8 @@ add_library(mkt_algorithm_diff SHARED ${DIFF_SOURCES} ${DIFF_HEADERS}) target_link_libraries(mkt_algorithm_diff PUBLIC ${PACKAGES_DIR} + robot::node_handle + robot::console Boost::system Eigen3::Eigen ) @@ -97,7 +98,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON) option(BUILD_TESTS "Build test programs" OFF) # Flags biên dịch -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +# Warning flags - disabled to suppress warnings during build +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h index 4b2cbf1..cd5237a 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h @@ -24,7 +24,7 @@ namespace mkt_algorithm * @param nh NodeHandle to read parameters from */ virtual void initialize( - ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; + robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; /** * @brief Calculating algorithm @@ -35,6 +35,12 @@ namespace mkt_algorithm virtual mkt_msgs::Trajectory2D calculator( const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + /** + * @brief Create a new GoStraight instance + * @return A pointer to the new GoStraight instance + */ + static score_algorithm::ScoreAlgorithm::Ptr create(); + }; // class GoStraight } // namespace diff diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index 833db5e..b0e1aa5 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -1,26 +1,18 @@ #ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ #define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ +#include #include -#include +#include #include #include #include -// #include -// #include - -// #include -// #include -// #include -// #include -// #include -// #include - #include #include #include #include +#include #include #include #include @@ -52,7 +44,7 @@ namespace mkt_algorithm * @param nh NodeHandle to read parameters from */ virtual void initialize( - ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; + robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; /** * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories @@ -92,6 +84,12 @@ namespace mkt_algorithm */ virtual void resume() override; + /** + * @brief Create a new PredictiveTrajectory instance + * @return A pointer to the new PredictiveTrajectory instance + */ + static score_algorithm::ScoreAlgorithm::Ptr create(); + protected: inline double sign(double x) { @@ -107,7 +105,7 @@ namespace mkt_algorithm * @brief Initialize dynamic reconfigure * @param nh NodeHandle to read parameters from */ - virtual void initDynamicReconfigure(const ros::NodeHandle &nh); + virtual void initDynamicReconfigure(const robot::NodeHandle &nh); /** * @brief Dynamically adjust look ahead distance based on the speed @@ -160,7 +158,7 @@ namespace mkt_algorithm */ bool transformGlobalPlan( TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose, - const costmap_2d::Costmap2DROS *costmap, const std::string &robot_base_frame, double max_plan_length, + const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length, nav_2d_msgs::Path2D &transformed_plan); /** @@ -241,17 +239,15 @@ namespace mkt_algorithm bool initialized_; bool nav_stop_, avoid_obstacles_; - ros::NodeHandle nh_, nh_priv_; + robot::NodeHandle nh_, nh_priv_; std::string frame_id_path_; std::string robot_base_frame_; - ros::Publisher carrot_pub_; nav_2d_msgs::Pose2DStamped goal_; nav_2d_msgs::Path2D global_plan_; nav_2d_msgs::Path2D compute_plan_; nav_2d_msgs::Path2D transform_plan_; nav_2d_msgs::Twist2D prevous_drive_cmd_; - ros::Publisher drive_pub_; double x_direction_, y_direction_, theta_direction_; double max_robot_pose_search_dist_; @@ -296,16 +292,9 @@ namespace mkt_algorithm // Control frequency double control_duration_; - - std::shared_ptr ddr_; - base_local_planner::BaseLocalPlannerConfig config_; - - base_local_planner::WorldModel *world_model_; ///< @brief The world model that the controller will use - base_local_planner::TrajectoryPlanner *tc_; ///< @brief The trajectory controller - base_local_planner::MapGridVisualizer map_viz_; std::vector footprint_spec_; - ros::Time last_actuator_update_; + robot::Time last_actuator_update_; boost::shared_ptr kf_; int m_, n_; Eigen::MatrixXd A; @@ -314,8 +303,6 @@ namespace mkt_algorithm Eigen::MatrixXd R; Eigen::MatrixXd P; - ros::Publisher cost_right_goals_pub_, cost_left_goals_pub_; - // visualization_msgs::Marker L_, R_; }; // class PredictiveTrajectory } // namespace diff diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h index 5d21ef7..8ccb9ff 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h @@ -23,7 +23,7 @@ namespace mkt_algorithm * @param nh NodeHandle to read parameters from */ virtual void initialize( - ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; + robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; /** * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories @@ -53,6 +53,12 @@ namespace mkt_algorithm virtual mkt_msgs::Trajectory2D calculator( const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + /** + * @brief Create a new RotateToGoal instance + * @return A pointer to the new RotateToGoal instance + */ + static score_algorithm::ScoreAlgorithm::Ptr create(); + protected: /** * @brief Initialize parameters diff --git a/src/Algorithms/Libraries/mkt_algorithm/package.xml b/src/Algorithms/Libraries/mkt_algorithm/package.xml index 3cb27e7..7e9bcac 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/package.xml +++ b/src/Algorithms/Libraries/mkt_algorithm/package.xml @@ -59,7 +59,6 @@ ddynamic_reconfigure costmap_2d base_local_planner - visualization_msgs geometry_msgs score_algorithm @@ -71,7 +70,6 @@ ddynamic_reconfigure costmap_2d base_local_planner - visualization_msgs geometry_msgs score_algorithm @@ -83,7 +81,6 @@ ddynamic_reconfigure costmap_2d base_local_planner - visualization_msgs pluginlib diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp index d17c2b9..09475e3 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp @@ -1,35 +1,24 @@ #include - -// other -// #include #include +#include void mkt_algorithm::diff::GoStraight::initialize( - ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) + robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) { if (!initialized_) { - nh_ = ros::NodeHandle("~/"); - nh_priv_ = ros::NodeHandle("~/" + name); + nh_ = robot::NodeHandle("~"); + nh_priv_ = robot::NodeHandle(nh, name); name_ = name; tf_ = tf; traj_ = traj; - costmap_ros_ = costmap_ros; + costmap_robot_ = costmap_robot; this->getParams(); - // this->initDynamicReconfigure(nh_priv_); nh_.param("publish_topic", enable_publish_, false); nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); - - carrot_pub_ = nh_.advertise("lookahead_point", 1); - reached_intermediate_goals_pub_ = nh_.advertise("reached_intermediate_goals", 1); - current_goal_pub_ = nh_.advertise("sub_goal", 1); - closet_robot_goal_pub_ = nh_.advertise("closet_robot_goal", 1); - transformed_plan_pub_ = nh_.advertise("transformed_plan", 1); - compute_plan_pub_ = nh_.advertise("compute_plan", 1); - - std::vector footprint = costmap_ros_? costmap_ros_->getRobotFootprint() : std::vector(); + std::vector footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector(); if (footprint.size() > 1) { double min_length = 1e10; @@ -53,7 +42,7 @@ void mkt_algorithm::diff::GoStraight::initialize( } // kalman - last_actuator_update_ = ros::Time::now(); + last_actuator_update_ = robot::Time::now(); n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta] m_ = 2; // measurements: x, y, theta double dt = control_duration_; @@ -87,11 +76,11 @@ void mkt_algorithm::diff::GoStraight::initialize( kf_ = boost::make_shared(dt, A, C, Q, R, P); Eigen::VectorXd x0(n_); x0 << 0, 0, 0, 0, 0, 0; - kf_->init(ros::Time::now().toSec(), x0); + kf_->init(robot::Time::now().toSec(), x0); x_direction_ = y_direction_ = theta_direction_ = 0; initialized_ = true; - ROS_INFO("GoStraight Initialized successfully"); + robot::log_info("GoStraight Initialized successfully"); } } @@ -104,11 +93,11 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( return result; if (compute_plan_.poses.size() < 2) { - ROS_WARN_THROTTLE(2.0, "Local compute plan is available"); + robot::log_warning("Local compute plan is available"); return result; } - ros::Time current_time = ros::Time::now(); + robot::Time current_time = robot::Time::now(); double dt = (current_time - last_actuator_update_).toSec(); last_actuator_update_ = current_time; @@ -117,8 +106,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( nav_2d_msgs::Twist2D twist; traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom - ros::Rate r(50); - while (ros::ok() && traj_->hasMoreTwists()) + robot::Rate r(50); + while (traj_->hasMoreTwists()) { twist = traj_->nextTwist(); // ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta); @@ -129,13 +118,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( nav_2d_msgs::Path2D transformed_plan = transform_plan_; if (transformed_plan.poses.empty()) { - ROS_WARN("Transformed plan is empty. Cannot determine a local plan."); + robot::log_warning("Transformed plan is empty. Cannot determine a local plan."); return result; } - if (enable_publish_) - transformed_plan_pub_.publish(nav_2d_utils::pathToPath(transformed_plan)); - // Dynamically adjust look ahead distance based on the speed double lookahead_dist = this->getLookAheadDistance(twist); // Return false if the transformed global plan is empty @@ -145,9 +131,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( // Get lookahead point and publish for visualization auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); - if (enable_publish_) - carrot_pub_.publish(this->createCarrotMsg(carrot_pose)); - // Carrot distance squared const double carrot_dist2 = (carrot_pose.pose.x * carrot_pose.pose.x) + @@ -240,4 +223,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( return result; } -PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::GoStraight, score_algorithm::ScoreAlgorithm) \ No newline at end of file +score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::GoStraight::create() +{ + return std::make_shared(); +} + +BOOST_DLL_ALIAS(mkt_algorithm::diff::GoStraight::create, MKTAlgorithmDiffGoStraight) \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 9cad56e..d5cc6aa 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -1,93 +1,27 @@ #include -// other -#include +#include +#include + #define LIMIT_VEL_THETA 0.33 void mkt_algorithm::diff::PredictiveTrajectory::initialize( - ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) + robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) { if (!initialized_) { - nh_ = ros::NodeHandle("~/"); - nh_priv_ = ros::NodeHandle("~/" + name); + nh_ = robot::NodeHandle("~"); + nh_priv_ = robot::NodeHandle(nh, name); name_ = name; tf_ = tf; traj_ = traj; - costmap_ros_ = costmap_ros; + costmap_robot_ = costmap_robot; this->getParams(); // this->initDynamicReconfigure(nh_priv_); nh_.param("publish_topic", enable_publish_, false); nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); - std::vector y_vels = {}; - footprint_spec_ = costmap_ros_->getRobotFootprint(); - world_model_ = new base_local_planner::CostmapModel(*costmap_ros->getCostmap()); - - this->config_.acc_lim_x = fabs(acc_lim_x_); - this->config_.acc_lim_y = fabs(acc_lim_y_); - this->config_.acc_lim_theta = fabs(acc_lim_theta_); - - this->config_.max_vel_x = max_vel_x_; - this->config_.min_vel_x = fabs(min_vel_x_); - - this->config_.max_vel_theta = max_vel_theta_; - this->config_.min_vel_theta = (-1.0) * max_vel_theta_; - this->config_.min_in_place_vel_theta = 0.4; - - this->config_.sim_time = 1.5; - this->config_.sim_granularity = 0.025; - this->config_.vx_samples = 10; - this->config_.vtheta_samples = 40; - this->config_.path_distance_bias = 0.6; - this->config_.goal_distance_bias = 0.4; - this->config_.occdist_scale = 0.2; - this->config_.heading_lookahead = 1.0; - this->config_.oscillation_reset_dist = 0.05; - this->config_.escape_reset_dist = 0.1; - this->config_.escape_reset_theta = 1.570796; - this->config_.escape_vel = -0.1; - this->config_.holonomic_robot = false; - this->config_.heading_scoring_timestep = 0.1; - this->config_.dwa = true; - this->config_.simple_attractor = true; - this->config_.heading_scoring = false; - this->config_.angular_sim_granularity = 0.025; - bool meter_scoring = false; - double stop_time_buffer = 0.2; - - tc_ = new base_local_planner::TrajectoryPlanner( - *world_model_, *costmap_ros->getCostmap(), footprint_spec_, - this->config_.acc_lim_x, this->config_.acc_lim_y, this->config_.acc_lim_theta, - this->config_.sim_time, this->config_.sim_granularity, - this->config_.vx_samples, this->config_.vtheta_samples, - this->config_.path_distance_bias, this->config_.goal_distance_bias, this->config_.occdist_scale, - this->config_.heading_lookahead, this->config_.oscillation_reset_dist, - this->config_.escape_reset_dist, this->config_.escape_reset_theta, - this->config_.holonomic_robot, - this->config_.max_vel_x, this->config_.min_vel_x, - this->config_.max_vel_theta, this->config_.min_vel_theta, this->config_.min_in_place_vel_theta, - this->config_.escape_vel, - this->config_.dwa, this->config_.heading_scoring, this->config_.heading_scoring_timestep, meter_scoring, this->config_.simple_attractor, - y_vels, stop_time_buffer, control_duration_, this->config_.angular_sim_granularity); - - map_viz_.initialize(name, - costmap_ros_->getGlobalFrameID(), - [this](int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost) - { - return tc_->getCellCosts(cx, cy, path_cost, goal_cost, occ_cost, total_cost); - }); - - carrot_pub_ = nh_.advertise("lookahead_point", 1); - reached_intermediate_goals_pub_ = nh_.advertise("reached_intermediate_goals", 1); - current_goal_pub_ = nh_.advertise("sub_goal", 1); - closet_robot_goal_pub_ = nh_.advertise("closet_robot_goal", 1); - transformed_plan_pub_ = nh_.advertise("transformed_plan", 1); - compute_plan_pub_ = nh_.advertise("compute_plan", 1); - cost_right_goals_pub_ = nh_.advertise("cost_right_goals", 1); - cost_left_goals_pub_ = nh_.advertise("cost_left_goals", 1); - drive_pub_ = nh_.advertise("cmd_vel_raw", 1); - - std::vector footprint = costmap_ros_ ? costmap_ros_->getRobotFootprint() : std::vector(); + footprint_spec_ = costmap_robot_->getRobotFootprint(); + std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); if (footprint.size() > 1) { double min_length = 1e10; @@ -111,7 +45,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize( } // kalman - last_actuator_update_ = ros::Time::now(); + last_actuator_update_ = robot::Time::now(); n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta] m_ = 2; // measurements: x, y, theta double dt = control_duration_; @@ -145,47 +79,20 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize( kf_ = boost::make_shared(dt, A, C, Q, R, P); Eigen::VectorXd x0(n_); x0 << 0, 0, 0, 0, 0, 0; - kf_->init(ros::Time::now().toSec(), x0); - - L_.header.frame_id = R_.header.frame_id = costmap_ros_->getGlobalFrameID(); // hoặc frame phù hợp - L_.ns = "left_cost_points"; - R_.ns = "right_cost_points"; - L_.id = 0; - R_.id = 1; - L_.type = R_.type = visualization_msgs::Marker::POINTS; - L_.action = R_.action = visualization_msgs::Marker::ADD; - L_.scale.x = L_.scale.y = R_.scale.x = R_.scale.y = 0.05; // kích thước điểm - // màu: L = xanh lá, R = đỏ - L_.color.r = 0.0; - L_.color.g = 1.0; - L_.color.b = 0.0; - L_.color.a = 1.0; - R_.color.r = 1.0; - R_.color.g = 0.0; - R_.color.b = 0.0; - R_.color.a = 1.0; - + kf_->init(robot::Time::now().toSec(), x0); x_direction_ = y_direction_ = theta_direction_ = 0; this->initialized_ = true; - ROS_INFO("PredictiveTrajectory Initialized successfully"); + robot::log_info("[%s:%d] PredictiveTrajectory Initialized successfully", __FILE__, __LINE__); } } mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() { - if (ddr_) - ddr_.reset(); - - if (tc_ != NULL) - delete tc_; - - if (world_model_ != NULL) - delete world_model_; } void mkt_algorithm::diff::PredictiveTrajectory::getParams() { - robot_base_frame_ = nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link")); + robot_base_frame_ = nh_priv_.param("robot_base_frame", std::string("base_link")); nh_priv_.param("avoid_obstacles", avoid_obstacles_, false); nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); nh_priv_.param("angle_threshold", angle_threshold_, M_PI / 8); @@ -220,7 +127,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams() nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); if (inflation_cost_scaling_factor_ <= 0.0) { - ROS_WARN("The value inflation_cost_scaling_factor is incorrectly set, " + robot::log_warning("[%s:%d] The value inflation_cost_scaling_factor is incorrectly set, " "it should be >0. Disabling cost regulated linear velocity scaling."); use_cost_regulated_linear_velocity_scaling_ = false; } @@ -247,59 +154,25 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams() } } -void mkt_algorithm::diff::PredictiveTrajectory::initDynamicReconfigure(const ros::NodeHandle &nh) -{ - // Ddynamic Reconfigure - ddr_ = std::make_shared(nh); - ddr_->registerVariable("avoid_obstacles", &this->avoid_obstacles_, "", true); - ddr_->registerVariable("lookahead_time", &this->lookahead_time_, "", 0.0, 20.0); - ddr_->registerVariable("lookahead_dist", &this->lookahead_dist_, "", 0.0, 20.0); - - ddr_->registerVariable("use_velocity_scaled_lookahead_dist", &this->use_velocity_scaled_lookahead_dist_); - ddr_->registerVariable("min_lookahead_dist", &this->min_lookahead_dist_, "", 0.0, 5.0); - ddr_->registerVariable("max_lookahead_dist", &this->max_lookahead_dist_, "", 0.0, 10.0); - ddr_->registerVariable("min_journey_squared", &this->min_journey_squared_, "", 0.0, 1.0); - ddr_->registerVariable("max_journey_squared", &this->max_journey_squared_, "", 0.0, 1.0); - // Rotate to heading param - ddr_->registerVariable("use_rotate_to_heading", &this->use_rotate_to_heading_); - ddr_->registerVariable("rotate_to_heading_min_angle", &this->rotate_to_heading_min_angle_, "", 0.0, 15.0); - - // Speed - ddr_->registerVariable("min_approach_linear_velocity", &this->min_approach_linear_velocity_, "", 0.0, 10.0); - - // Regulated linear velocity scaling - ddr_->registerVariable("use_regulated_linear_velocity_scaling", &this->use_regulated_linear_velocity_scaling_); - ddr_->registerVariable("regulated_linear_scaling_min_radius", &this->regulated_linear_scaling_min_radius_, "", 0.0, 5.0); - ddr_->registerVariable("regulated_linear_scaling_min_speed", &this->regulated_linear_scaling_min_speed_, "", 0.0, 5.0); - - // Inflation cost scaling (Limit velocity by proximity to obstacles) - ddr_->registerVariable("use_cost_regulated_linear_velocity_scaling", &this->use_cost_regulated_linear_velocity_scaling_); - ddr_->registerVariable("inflation_cost_scaling_factor", &this->inflation_cost_scaling_factor_, "", 0.0, 10.0); - ddr_->registerVariable("cost_scaling_dist", &this->cost_scaling_dist_, "", 0.0, 10.0); - ddr_->registerVariable("cost_scaling_gain", &this->cost_scaling_gain_, "", 0.0, 10.0); - - ddr_->publishServicesTopics(); -} - void mkt_algorithm::diff::PredictiveTrajectory::reset() { if (this->initialized_) { - ROS_INFO("PredictiveTrajectory is reset"); + robot::log_info("[%s:%d] PredictiveTrajectory is reset", __FILE__, __LINE__); reached_intermediate_goals_.clear(); start_index_saved_vt_.clear(); this->clear(); x_direction_ = y_direction_ = theta_direction_ = 0; this->follow_step_path_ = false; this->nav_stop_ = false; - last_actuator_update_ = ros::Time::now(); + last_actuator_update_ = robot::Time::now(); prevous_drive_cmd_ = nav_2d_msgs::Twist2D(); if (kf_) { Eigen::VectorXd x0(n_); x0 << 0, 0, 0, 0, 0, 0; - kf_->init(ros::Time::now().toSec(), x0); + kf_->init(robot::Time::now().toSec(), x0); } } } @@ -316,13 +189,13 @@ void mkt_algorithm::diff::PredictiveTrajectory::resume() if(!this->nav_stop_) return; prevous_drive_cmd_ = nav_2d_msgs::Twist2D(); - last_actuator_update_ = ros::Time::now(); + last_actuator_update_ = robot::Time::now(); if (kf_) { Eigen::VectorXd x0(n_); x0 << 0, 0, 0, 0, 0, 0; - kf_->init(ros::Time::now().toSec(), x0); + kf_->init(robot::Time::now().toSec(), x0); } this->nav_stop_ = false; } @@ -334,11 +207,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 { if (!initialized_) { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + robot::log_error("[%s:%d] This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__); return false; } - std::vector footprint = costmap_ros_ ? costmap_ros_->getRobotFootprint() : std::vector(); + std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); if (footprint.size() > 1) { double min_length = 1e10; @@ -363,12 +236,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2) { - ROS_ERROR("The Local plan is empty or less than 1 points %d", (unsigned int)global_plan.poses.size()); + robot::log_error("[%s:%d] The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size()); return false; } this->getParams(); - if (avoid_obstacles_) - map_viz_.publishCostCloud(costmap_ros_->getCostmap()); frame_id_path_ = global_plan.header.frame_id; goal_ = goal; @@ -377,13 +248,13 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 // prune global plan to cut off parts of the past (spatially before the robot) if (!this->pruneGlobalPlan(tf_, pose, global_plan_, 1.0)) { - ROS_ERROR("pruneGlobalPlan Failed"); + robot::log_error("[%s:%d] pruneGlobalPlan Failed", __FILE__, __LINE__); return false; } double S = std::numeric_limits::infinity(); - S = std::max(costmap_ros_->getCostmap()->getSizeInCellsX() * costmap_ros_->getCostmap()->getResolution() / 2.0, - costmap_ros_->getCostmap()->getSizeInCellsY() * costmap_ros_->getCostmap()->getResolution() / 2.0); + S = std::max(costmap_robot_->getCostmap()->getSizeInCellsX() * costmap_robot_->getCostmap()->getResolution() / 2.0, + costmap_robot_->getCostmap()->getSizeInCellsY() * costmap_robot_->getCostmap()->getResolution() / 2.0); const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_; S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S); compute_plan_.poses.clear(); @@ -392,13 +263,13 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 { double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x; double dy = global_plan_.poses.back().pose.y - global_plan_.poses.front().pose.y; - if (hypot(dx, dy) <= 2.0 * costmap_ros_->getCostmap()->getResolution()) + if (hypot(dx, dy) <= 2.0 * costmap_robot_->getCostmap()->getResolution()) { compute_plan_ = global_plan_; } else { - ROS_ERROR("The Local Plan has 2 points and hypot between more than is %.3f m", costmap_ros_->getCostmap()->getResolution()); + robot::log_error("[%s:%d] The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution()); return false; } } @@ -407,24 +278,21 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 unsigned int start_index, goal_index; if (!this->computePlanCommand(pose, velocity, S, global_plan_, start_index, goal_index, compute_plan_)) { - ROS_ERROR("computePlanCommand is Failed"); + robot::log_error("[%s:%d] computePlanCommand is Failed", __FILE__, __LINE__); return false; } } - if (enable_publish_) - compute_plan_pub_.publish(nav_2d_utils::pathToPath(compute_plan_)); - double lookahead_dist = this->getLookAheadDistance(velocity); transform_plan_.poses.clear(); - if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_ros_, robot_base_frame_, lookahead_dist, transform_plan_)) + if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_)) { - ROS_WARN("Could not transform the global plan to the frame of the controller"); + robot::log_warning("[%s:%d] Could not transform the global plan to the frame of the controller", __FILE__, __LINE__); return false; } // else // { - // ROS_INFO_THROTTLE(0.5, "Transform plan journey: %f %f %f", journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_); + // robot::log_info("[%s:%d] Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_); // } x_direction = x_direction_; @@ -462,29 +330,30 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 double dx = it->pose.x - carrot_pose_it->pose.x; double dy = it->pose.x - carrot_pose_it->pose.y; distance_it += std::hypot(dx, dy); - if (distance_it > costmap_ros_->getCostmap()->getResolution()) + if (distance_it > costmap_robot_->getCostmap()->getResolution()) { prev_carrot_pose_it = it; break; } } - geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_ros_->getCostmap()->getResolution() + geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution() ? nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose) : nav_2d_utils::pose2DToPose(geometry_msgs::Pose2D()); geometry_msgs::Pose back = nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose); - teb_local_planner::PoseSE2 start_pose(front); - teb_local_planner::PoseSE2 goal_pose(back); - const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec()); + // teb_local_planner::PoseSE2 start_pose(front); + // teb_local_planner::PoseSE2 goal_pose(back); + // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec()); + const double dir_path = 0.0; if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) x_direction = dir_path > 0 ? FORWARD : BACKWARD; } catch (std::exception &e) { - ROS_ERROR("getLookAheadPoint throw an exception: %s", e.what()); + robot::log_error("[%s:%d] getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what()); return false; } } @@ -504,11 +373,11 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( return result; if (compute_plan_.poses.size() < 2) { - ROS_WARN("Local compute plan is available"); + robot::log_warning("[%s:%d] Local compute plan is available", __FILE__, __LINE__); return result; } - ros::Time current_time = ros::Time::now(); + robot::Time current_time = robot::Time::now(); double dt = (current_time - last_actuator_update_).toSec(); last_actuator_update_ = current_time; @@ -516,77 +385,29 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( double sign_x = sign(x_direction_); nav_2d_msgs::Twist2D twist; traj_->startNewIteration(velocity); - while (ros::ok() && traj_->hasMoreTwists()) + while (traj_->hasMoreTwists()) { twist = traj_->nextTwist(); } drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5); double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; - if (avoid_obstacles_) - { - if ((cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE) || - (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE)) - { - nav_2d_msgs::Twist2D cmd; - cmd.x = 0.2; - moveWithAccLimits(velocity, cmd, drive_cmd); - config_.max_vel_x = drive_cmd.x; - config_.sim_time = 1.9; - config_.path_distance_bias = 0.7; - config_.goal_distance_bias = 0.6; - config_.occdist_scale = 0.05; - config_.dwa = true; - } - else if (cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE || cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE || - cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE || cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE) - { - nav_2d_msgs::Twist2D cmd; - cmd.x = 0.3; - moveWithAccLimits(velocity, cmd, drive_cmd); - config_.max_vel_x = drive_cmd.x; - config_.sim_time = 1.9; - config_.path_distance_bias = 0.7; - config_.goal_distance_bias = 0.6; - config_.occdist_scale = 0.12; - config_.dwa = false; - } - else - { - config_.max_vel_x = std::max(drive_cmd.x, 0.2); - config_.sim_time = 1.5; - config_.path_distance_bias = 0.8; - config_.goal_distance_bias = 0.6; - config_.occdist_scale = 0.01; - config_.dwa = false; - } - tc_->reconfigure(config_); - this->publishMarkers(pose); - } - nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; if (transformed_plan.poses.empty()) { - ROS_WARN("Transformed plan is empty. Cannot determine a localglobal_plan."); + robot::log_warning("[%s:%d] Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__); return result; } - if (enable_publish_) - transformed_plan_pub_.publish(nav_2d_utils::pathToPath(transformed_plan)); double lookahead_dist = getLookAheadDistance(velocity); double tolerance = hypot(pose.pose.x - compute_plan_.poses.front().pose.x, pose.pose.y - compute_plan_.poses.front().pose.y); - // lookahead_dist = std::clamp(lookahead_dist - tolerance * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_); if (transformed_plan.poses.empty()) { - ROS_WARN("Transformed plan is empty after compute lookahead point"); + robot::log_warning("[%s:%d] Transformed plan is empty after compute lookahead point", __FILE__, __LINE__); return result; } auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); - - if (enable_publish_) - carrot_pub_.publish(nav_2d_utils::pose2DToPoseStamped(carrot_pose.pose, carrot_pose.header.frame_id, ros::Time::now())); - bool allow_rotate = false; nh_priv_.param("allow_rotate", allow_rotate, false); @@ -598,7 +419,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( if (avoid_obstacles_) allow_rotate = journey_plan >= distance_allow_rotate && fabs(front.y) <= 0.2 && - (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_ros_->getCostmap()->getResolution()); + (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution()); else allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; @@ -607,7 +428,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( { if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_)) { - tc_->updatePlan(nav_2d_utils::pathToPath(compute_plan_).poses, true); if (!stopWithAccLimits(pose, velocity, drive_cmd)) return result; } @@ -618,78 +438,75 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( } else { - bool narrow_road = (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE); - if (!avoid_obstacles_ || sign_x < 0 || journey_plan < max_journey_squared_ || narrow_road) + const double vel_x_reduce = std::min(fabs(v_max), 0.3); + double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; + carrot_dist2 = std::max(carrot_dist2, 0.05); + double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + + const auto &plan_back_pose = compute_plan_.poses.back(); + double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y); + post_cost = std::max(post_cost, center_cost_); + this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x); + + const double scale = fabs(velocity.x) * lookahead_time_ * 0.9; + const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale; + double d_reduce = std::clamp(journey_plan, min_S, max_S); + double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0); + double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce))); + double v_min = + journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_; + v_min *= sign_x; + + double effective_journey = getEffectiveDistance(carrot_pose, journey_plan); + double decel_factor = computeDecelerationFactor(effective_journey, d_reduce); + double vel_reduce = sign_x > 0 + ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min) + : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min); + drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce; + + double v_theta = drive_cmd.x * curvature; + double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); + if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_)) { - const double vel_x_reduce = std::min(fabs(v_max), 0.3); - double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; - carrot_dist2 = std::max(carrot_dist2, 0.05); - double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + carrot_dist2 *= 0.6; + curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + v_theta = drive_cmd.x * curvature; + } + if (fabs(v_theta) > LIMIT_VEL_THETA) + { + nav_2d_msgs::Twist2D cmd_vel, cmd_result; + cmd_vel.x = sign_x > 0 + ? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1)) + : std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1)); + cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5); + this->moveWithAccLimits(velocity, cmd_vel, cmd_result); + drive_cmd.x = std::copysign(cmd_result.x, sign_x); + v_theta = drive_cmd.x * curvature; + } - const auto &plan_back_pose = compute_plan_.poses.back(); - double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y); - post_cost = std::max(post_cost, center_cost_); - this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x); - - const double scale = fabs(velocity.x) * lookahead_time_ * 0.9; - const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale; - double d_reduce = std::clamp(journey_plan, min_S, max_S); - double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0); - double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce))); - double v_min = - journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_; - v_min *= sign_x; - - double effective_journey = getEffectiveDistance(carrot_pose, journey_plan); - double decel_factor = computeDecelerationFactor(effective_journey, d_reduce); - double vel_reduce = sign_x > 0 - ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min) - : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min); - drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce; - - double v_theta = drive_cmd.x * curvature; - double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); - if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_)) + if (journey_plan < min_journey_squared_) + { + if (transform_plan_.poses.size() > 2) { - carrot_dist2 *= 0.6; - curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; - v_theta = drive_cmd.x * curvature; + nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back(); + nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2]; + double dx = end.pose.x - start.pose.x; + double dy = end.pose.y - start.pose.y; + v_theta = atan2(dy, dx); + if (v_theta > M_PI_2) + v_theta -= M_PI; + else if (v_theta < -M_PI_2) + v_theta += M_PI; + // v_theta *= 0.5; + v_theta = std::clamp(v_theta, -0.02, 0.02); } - if (fabs(v_theta) > LIMIT_VEL_THETA) - { - nav_2d_msgs::Twist2D cmd_vel, cmd_result; - cmd_vel.x = sign_x > 0 - ? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1)) - : std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1)); - cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5); - this->moveWithAccLimits(velocity, cmd_vel, cmd_result); - drive_cmd.x = std::copysign(cmd_result.x, sign_x); - v_theta = drive_cmd.x * curvature; - } - - if (journey_plan < min_journey_squared_) - { - if (transform_plan_.poses.size() > 2) - { - nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back(); - nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2]; - double dx = end.pose.x - start.pose.x; - double dy = end.pose.y - start.pose.y; - v_theta = atan2(dy, dx); - if (v_theta > M_PI_2) - v_theta -= M_PI; - else if (v_theta < -M_PI_2) - v_theta += M_PI; - // v_theta *= 0.5; - v_theta = std::clamp(v_theta, -0.02, 0.02); - } - else - v_theta = 0.0; - } - double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8; - double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt; - double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt; - drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth); + else + v_theta = 0.0; + } + double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8; + double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt; + double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt; + drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth); if (this->nav_stop_) { @@ -703,78 +520,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( result.velocity = drive_cmd; return result; } - } - else - { - geometry_msgs::PoseStamped traj_cmd, robot_vel; - robot_vel.pose.position.x = velocity.x; - robot_vel.pose.position.y = velocity.y; - tf2::Quaternion q; - - q.setRPY(0, 0, velocity.theta); - tf2::convert(q, robot_vel.pose.orientation); - tc_->updatePlan(nav_2d_utils::pathToPath(compute_plan_).poses, true); - auto path = tc_->findBestPath(nav_2d_utils::pose2DToPoseStamped(pose), robot_vel, traj_cmd); - - if (path.cost_ < 0) - { - ROS_DEBUG_NAMED("trajectory_planner_ros", - "The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories."); - - throw nav_core2::PlannerTFException("The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories."); - } - - if (path.cost_ < 0 || nav_stop_) - { - if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_)) - { - if (!stopWithAccLimits(pose, velocity, drive_cmd)) - return result; - } - else - drive_cmd = {}; - result.velocity = drive_cmd; - return result; - } - else - { - drive_cmd.x = std::max(drive_cmd.x, min_approach_linear_velocity_); - drive_cmd.x = std::clamp(traj_cmd.pose.position.x, -fabs(drive_cmd.x), fabs(drive_cmd.x)); - drive_cmd.y = traj_cmd.pose.position.y; - drive_cmd.theta = tf2::getYaw(traj_cmd.pose.orientation); - - if ((cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE || cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE || - cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE || cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE) && - fabs(traj_cmd.pose.position.x) < 0.02 && fabs(drive_cmd.theta) > 0.1) - { - nav_2d_msgs::Twist2D cmd, cmd_result; - cmd.x = sign_x * min_approach_linear_velocity_; - moveWithAccLimits(velocity, cmd, cmd_result); - - if (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE) - drive_cmd.theta = 0.0; - - else if (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE) - drive_cmd.theta = -min_vel_theta_ * sign_x; - - else - drive_cmd.theta = min_vel_theta_ * sign_x; - drive_cmd.x = sign_x * cmd_result.x; - } - - for (unsigned int i = 0; i < path.getPointsSize(); ++i) - { - double p_x, p_y, p_th; - path.getPoint(i, p_x, p_y, p_th); - geometry_msgs::Pose2D pose; - pose.x = p_x; - pose.y = p_y; - pose.theta = p_th; - result.poses.push_back(pose); - } - } - } - + Eigen::VectorXd y(2); y << drive_cmd.x, drive_cmd.theta; @@ -1063,9 +809,9 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf if (erase_end != global_plan.poses.begin()) global_plan.poses.erase(global_plan.poses.begin(), erase_end); } - catch (const tf::TransformException &ex) + catch (const tf3::TransformException &ex) { - ROS_DEBUG("Cannot prune path since no transform is available: %s\n", ex.what()); + robot::log_debug("[%s:%d] Cannot prune path since no transform is available: %s", __FILE__, __LINE__, ex.what()); return false; } return true; @@ -1073,7 +819,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose, - const costmap_2d::Costmap2DROS *costmap, const std::string &robot_base_frame, double max_plan_length, + const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length, nav_2d_msgs::Path2D &transformed_plan) { // this method is a slightly modified version of base_local_planner/goal_functions.h @@ -1094,7 +840,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( { if (global_plan.poses.empty()) { - ROS_ERROR("Received plan with zero length"); + robot::log_error("[%s:%d] Received plan with zero length", __FILE__, __LINE__); return false; } @@ -1179,25 +925,25 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( ++i; } } - catch (tf::LookupException &ex) + catch (tf3::LookupException &ex) { - ROS_ERROR("No Transform available Error: %s\n", ex.what()); + robot::log_error("[%s:%d] No Transform available Error: %s", __FILE__, __LINE__, ex.what()); return false; } - catch (tf::ConnectivityException &ex) + catch (tf3::ConnectivityException &ex) { - ROS_ERROR("Connectivity Error: %s\n", ex.what()); + robot::log_error("[%s:%d] Connectivity Error: %s", __FILE__, __LINE__, ex.what()); return false; } - catch (tf::ExtrapolationException &ex) + catch (tf3::ExtrapolationException &ex) { - ROS_ERROR("Extrapolation Error: %s\n", ex.what()); + robot::log_error("[%s:%d] Extrapolation Error: %s", __FILE__, __LINE__, ex.what()); if (global_plan.poses.size() > 0) - ROS_ERROR("Robot Frame: %s Plan Frame size %d: %s\n", robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str()); + robot::log_error("[%s:%d] Robot Frame: %s Plan Frame size %d: %s", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str()); return false; } - return true; + return true; } void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits( @@ -1241,29 +987,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const nav_2d_m double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - fabs(decel_lim_theta_) * dt)); // we do want to check whether or not the command is valid - if (avoid_obstacles_) - { - try - { - if (tc_ != NULL) - { - bool valid_cmd = tc_->checkTrajectory(pose.pose.x, pose.pose.y, pose.pose.theta, velocity.x, velocity.y, vel_yaw, vx, vy, vth); - // if we have a valid command, we'll pass it on, otherwise we'll command all zeros - if (valid_cmd) - { - ROS_DEBUG("Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth); - cmd_vel.x = vx; - cmd_vel.y = vy; - cmd_vel.theta = vth; - return true; - } - } - } - catch (const std::exception &e) - { - ROS_ERROR_STREAM(e.what() << '\n'); - } - } cmd_vel.x = vx; cmd_vel.y = vy; cmd_vel.theta = vth; @@ -1302,7 +1025,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( pose_cost != static_cast(costmap_2d::NO_INFORMATION) && pose_cost != static_cast(costmap_2d::FREE_SPACE)) { - const double inscribed_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); + const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius(); const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * std::log(pose_cost / (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius; @@ -1327,8 +1050,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( // (2) using t with the actual lookahead // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). - double dist_error_limit = costmap_ros_ != nullptr && costmap_ros_->getCostmap() != nullptr - ? 2.0 * costmap_ros_->getCostmap()->getResolution() + double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr + ? 2.0 * costmap_robot_->getCostmap()->getResolution() : 0.1; if (dist_error > dist_error_limit) { @@ -1414,9 +1137,9 @@ double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, co { unsigned int mx, my; unsigned char cost; - if (costmap_ros_->getCostmap()->worldToMap(x, y, mx, my)) + if (costmap_robot_->getCostmap()->worldToMap(x, y, mx, my)) { - cost = costmap_ros_->getCostmap()->getCost(mx, my); + cost = costmap_robot_->getCostmap()->getCost(mx, my); } return static_cast(cost); } @@ -1490,10 +1213,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose2DStamped pose) { const auto &plan_back_pose = compute_plan_.poses.back(); - // const double offset_max = this->min_path_distance_ + costmap_ros_->getCostmap()->getResolution() * 2.0; + // const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0; // const double offset_min = this->min_path_distance_; - auto points_rb = interpolateFootprint(pose.pose, costmap_ros_->getRobotFootprint(), costmap_ros_->getCostmap()->getResolution() * 2.0); + auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); for (const auto &point : points_rb) { double cost_goal = costAtPose(point.x, point.y); @@ -1506,19 +1229,17 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose if (y_rel > epsilon) { cost_left_side_ = std::max(cost_left_side_, cost_goal); - L_.points.push_back(point); } else if (y_rel < -epsilon) { cost_right_side_ = std::max(cost_right_side_, cost_goal); - R_.points.push_back(point); } } unsigned int step_footprint = 10; if ((unsigned int)(compute_plan_.poses.size() - 1) < 10) { - auto points = interpolateFootprint(plan_back_pose.pose, costmap_ros_->getRobotFootprint(), costmap_ros_->getCostmap()->getResolution() * 2.0); + auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); for (const auto &point : points) { double cost_goal = costAtPose(point.x, point.y); @@ -1531,12 +1252,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose if (y_rel > epsilon) { cost_left_goal_ = std::max(cost_left_goal_, cost_goal); - L_.points.push_back(point); } else if (y_rel < -epsilon) { cost_right_goal_ = std::max(cost_right_goal_, cost_goal); - R_.points.push_back(point); } else center_cost_ = std::max(center_cost_, cost_goal); @@ -1546,7 +1265,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose { for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint) { - auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_ros_->getRobotFootprint(), costmap_ros_->getCostmap()->getResolution() * 2.0); + auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); for (const auto &point : points) { double cost_goal = costAtPose(point.x, point.y); @@ -1559,12 +1278,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose if (y_rel > epsilon) { cost_left_goal_ = std::max(cost_left_goal_, cost_goal); - L_.points.push_back(point); } else if (y_rel < -epsilon) { cost_right_goal_ = std::max(cost_right_goal_, cost_goal); - R_.points.push_back(point); } else center_cost_ = std::max(center_cost_, cost_goal); @@ -1575,13 +1292,12 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose break; } } - if (enable_publish_) - { - cost_left_goals_pub_.publish(L_); - cost_right_goals_pub_.publish(R_); - } - R_.points.clear(); - L_.points.clear(); } -PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::PredictiveTrajectory, score_algorithm::ScoreAlgorithm) \ No newline at end of file +score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create() +{ + return std::make_shared(); +} + +// Register this planner as a GlobalPlanner plugin +BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory) \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp index 3112104..56df13e 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp @@ -1,25 +1,24 @@ +#include #include - -// other #include -#include +#include #include void mkt_algorithm::diff::RotateToGoal::initialize( - ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) + robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) { if (!initialized_) { - nh_ = ros::NodeHandle(nh, name); + nh_ = robot::NodeHandle(nh, name); name_ = name; tf_ = tf; traj_ = traj; - costmap_ros_ = costmap_ros; + costmap_robot_ = costmap_robot; this->getParams(); x_direction_ = y_direction_ = theta_direction_ = 0; this->initialized_ = true; - ROS_INFO("RotateToGoal Initialized successfully"); + robot::log_info("[%s:%d] RotateToGoal Initialized successfully", __FILE__, __LINE__); } } @@ -59,8 +58,7 @@ void mkt_algorithm::diff::RotateToGoal::getParams() nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); if (inflation_cost_scaling_factor_ <= 0.0) { - ROS_WARN("The value inflation_cost_scaling_factor is incorrectly set, " - "it should be >0. Disabling cost regulated linear velocity scaling."); + robot::log_warning("[%s:%d] The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__); use_cost_regulated_linear_velocity_scaling_ = false; } double control_frequency = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); @@ -96,7 +94,7 @@ bool mkt_algorithm::diff::RotateToGoal::prepare(const nav_2d_msgs::Pose2DStamped if (!initialized_) { - ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); + robot::log_error("This planner has not been initialized, please call initialize() before using this planner"); return false; } this->getParams(); @@ -128,4 +126,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::RotateToGoal::calculator( return result; } -PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::RotateToGoal, score_algorithm::ScoreAlgorithm) \ No newline at end of file +score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::RotateToGoal::create() +{ + return std::make_shared(); +} + +BOOST_DLL_ALIAS(mkt_algorithm::diff::RotateToGoal::create, MKTAlgorithmDiffRotateToGoal) \ No newline at end of file diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt b/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt index db8a467..f660ff8 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt +++ b/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt @@ -78,7 +78,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON) option(BUILD_TESTS "Build test programs" OFF) # Flags biên dịch -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +# Warning flags - disabled to suppress warnings during build +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp index 056d71c..0af764c 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp +++ b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp @@ -58,23 +58,23 @@ namespace two_points_planner unsigned char result = 0; // Bug - // if (!costmap_robot_) - // { - // std::cerr << "TwoPointsPlanner: Costmap is not initialized" << std::endl; - // return 0; - // } + if (!costmap_robot_) + { + std::cerr << "TwoPointsPlanner: Costmap is not initialized" << std::endl; + return 0; + } - // // check if the costmap has an inflation layer - // for (std::vector>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin(); - // layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end(); - // ++layer) - // { - // boost::shared_ptr inflation_layer = boost::dynamic_pointer_cast(*layer); - // if (!inflation_layer) - // continue; + // check if the costmap has an inflation layer + for (std::vector>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin(); + layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end(); + ++layer) + { + boost::shared_ptr inflation_layer = boost::dynamic_pointer_cast(*layer); + if (!inflation_layer) + continue; - // result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / costmap_robot_->getCostmap()->getResolution())); - // } + result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / costmap_robot_->getCostmap()->getResolution())); + } return result; } @@ -109,104 +109,104 @@ namespace two_points_planner return false; } - // bool do_init = false; - // if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() || - // current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY()) - // { - // printf("TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.\n", - // current_env_width_, current_env_height_, - // costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY()); - // do_init = true; - // } - // else if (footprint_ != costmap_robot_->getRobotFootprint()) - // { - // printf("TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.\n"); - // do_init = true; - // } + bool do_init = false; + if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() || + current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY()) + { + printf("TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.\n", + current_env_width_, current_env_height_, + costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY()); + do_init = true; + } + else if (footprint_ != costmap_robot_->getRobotFootprint()) + { + printf("TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.\n"); + do_init = true; + } - // if (do_init) - // { - // initialized_ = false; - // initialize(name_, costmap_robot_); - // } + if (do_init) + { + initialized_ = false; + initialize(name_, costmap_robot_); + } - // plan.clear(); - // plan.push_back(start); - // printf("TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)\n", - // start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y); + plan.clear(); + plan.push_back(start); + printf("TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)\n", + start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y); - // unsigned int mx_start, my_start; - // unsigned int mx_end, my_end; - // if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x, - // start.pose.position.y, - // mx_start, my_start) + unsigned int mx_start, my_start; + unsigned int mx_end, my_end; + if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x, + start.pose.position.y, + mx_start, my_start) - // || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x, - // goal.pose.position.y, - // mx_end, my_end)) - // { - // std::cerr << "TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'" << std::endl; - // return false; - // } - // unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start)); - // unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end)); - // if (start_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE) - // || end_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) - // { - // std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl; - // return false; - // } - // robot::Time plan_time = robot::Time::now(); - // // Tính toán khoảng cách giữa điểm bắt đầu và điểm đích - // const double dx = goal.pose.position.x - start.pose.position.x; - // const double dy = goal.pose.position.y - start.pose.position.y; - // double distance = std::sqrt(dx * dx + dy * dy); - // double theta; - // if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9) - // { - // theta = atan2(dy, dx); - // tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y, - // goal.pose.orientation.z, goal.pose.orientation.w); - // double goal_theta = tf3::getYaw(goal_quat); - // if(cos(goal_theta - theta) < 0) theta += M_PI; - // } - // else - // { - // std::cerr << "[two_points_planner] can not calculating theta from 'start point' or 'goal point'" << std::endl; - // return false; - // } + || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x, + goal.pose.position.y, + mx_end, my_end)) + { + std::cerr << "TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'" << std::endl; + return false; + } + unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start)); + unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end)); + if (start_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + || end_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) + { + std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl; + return false; + } + robot::Time plan_time = robot::Time::now(); + // Tính toán khoảng cách giữa điểm bắt đầu và điểm đích + const double dx = goal.pose.position.x - start.pose.position.x; + const double dy = goal.pose.position.y - start.pose.position.y; + double distance = std::sqrt(dx * dx + dy * dy); + double theta; + if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9) + { + theta = atan2(dy, dx); + tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y, + goal.pose.orientation.z, goal.pose.orientation.w); + double goal_theta = tf3::getYaw(goal_quat); + if(cos(goal_theta - theta) < 0) theta += M_PI; + } + else + { + std::cerr << "[two_points_planner] can not calculating theta from 'start point' or 'goal point'" << std::endl; + return false; + } - // // Lấy độ phân giải của costmap - // double resolution = costmap_robot_->getCostmap()->getResolution(); + // Lấy độ phân giải của costmap + double resolution = costmap_robot_->getCostmap()->getResolution(); - // // Tính số điểm cần chia - // int num_points = std::ceil(distance / resolution); + // Tính số điểm cần chia + int num_points = std::ceil(distance / resolution); - // // Chia nhỏ tuyến đường - // for (int i = 0; i <= num_points; i++) - // { - // double fraction = static_cast(i) / num_points; + // Chia nhỏ tuyến đường + for (int i = 0; i <= num_points; i++) + { + double fraction = static_cast(i) / num_points; - // geometry_msgs::PoseStamped pose; - // pose.header.stamp = plan_time; - // pose.header.frame_id = costmap_robot_->getGlobalFrameID(); - // pose.pose.position.x = start.pose.position.x + fraction * dx; - // pose.pose.position.y = start.pose.position.y + fraction * dy; - // pose.pose.position.z = start.pose.position.z; + geometry_msgs::PoseStamped pose; + pose.header.stamp = plan_time; + pose.header.frame_id = costmap_robot_->getGlobalFrameID(); + pose.pose.position.x = start.pose.position.x + fraction * dx; + pose.pose.position.y = start.pose.position.y + fraction * dy; + pose.pose.position.z = start.pose.position.z; - // // Nội suy hướng - // tf3::Quaternion interpolated_quat; - // interpolated_quat.setRPY(0, 0, theta); + // Nội suy hướng + tf3::Quaternion interpolated_quat; + interpolated_quat.setRPY(0, 0, theta); - // // Chuyển đổi trực tiếp từ tf3::Quaternion sang geometry_msgs::Quaternion - // pose.pose.orientation.x = interpolated_quat.x(); - // pose.pose.orientation.y = interpolated_quat.y(); - // pose.pose.orientation.z = interpolated_quat.z(); - // pose.pose.orientation.w = interpolated_quat.w(); + // Chuyển đổi trực tiếp từ tf3::Quaternion sang geometry_msgs::Quaternion + pose.pose.orientation.x = interpolated_quat.x(); + pose.pose.orientation.y = interpolated_quat.y(); + pose.pose.orientation.z = interpolated_quat.z(); + pose.pose.orientation.w = interpolated_quat.w(); - // plan.push_back(pose); - // } - // plan.push_back(goal); + plan.push_back(pose); + } + plan.push_back(goal); return true; } diff --git a/src/Libraries/common_msgs b/src/Libraries/common_msgs index bf1fc3d..6bac684 160000 --- a/src/Libraries/common_msgs +++ b/src/Libraries/common_msgs @@ -1 +1 @@ -Subproject commit bf1fc3df34fa79be25cb0e99b1bdb441ec3c7a9d +Subproject commit 6bac68429824cab4bbb1df3e259a24a8bf741b59 diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d index ddff754..fdfba18 160000 --- a/src/Libraries/costmap_2d +++ b/src/Libraries/costmap_2d @@ -1 +1 @@ -Subproject commit ddff75465ca49b04bf22d2fbe26f61aa4bff7acb +Subproject commit fdfba18bde0662ab5f042b938ecd011c5382ca7a diff --git a/src/Libraries/nav_2d_utils/CMakeLists.txt b/src/Libraries/nav_2d_utils/CMakeLists.txt index d7b3e7f..d57f829 100755 --- a/src/Libraries/nav_2d_utils/CMakeLists.txt +++ b/src/Libraries/nav_2d_utils/CMakeLists.txt @@ -32,6 +32,8 @@ target_link_libraries(conversions nav_msgs nav_grid nav_core2 + robot::node_handle + robot::console PRIVATE console_bridge::console_bridge Boost::system @@ -48,6 +50,8 @@ target_link_libraries(path_ops PUBLIC nav_2d_msgs geometry_msgs + robot::node_handle + robot::console ) add_library(polygons src/polygons.cpp src/footprint.cpp) @@ -60,6 +64,8 @@ target_link_libraries(polygons PUBLIC nav_2d_msgs geometry_msgs + robot::node_handle + robot::console PRIVATE ${xmlrpcpp_LIBRARIES} ) @@ -81,6 +87,8 @@ target_link_libraries(bounds PUBLIC nav_grid nav_core2 + robot::node_handle + robot::console ) add_library(tf_help src/tf_help.cpp) @@ -95,6 +103,8 @@ target_link_libraries(tf_help geometry_msgs nav_core2 tf3 + robot::node_handle + robot::console ) # Create an INTERFACE library that represents all nav_2d_utils libraries @@ -111,6 +121,8 @@ target_link_libraries(nav_2d_utils polygons bounds tf_help + robot::node_handle + robot::console ) # Install header files diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h index 8f92f24..b0f8c0a 100755 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h @@ -35,7 +35,7 @@ #ifndef NAV_2D_UTILS_FOOTPRINT_H #define NAV_2D_UTILS_FOOTPRINT_H -#include +#include #include namespace nav_2d_utils @@ -47,7 +47,7 @@ namespace nav_2d_utils * Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter * is present. */ -nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write = true); +nav_2d_msgs::Polygon2D footprintFromParams(robot::NodeHandle& nh, bool write = true); } // namespace nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h index 911ffbd..91f1af6 100755 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h @@ -41,7 +41,7 @@ #include #include #include -#include +#include namespace nav_2d_utils { @@ -59,7 +59,7 @@ public: * @param nh NodeHandle for creating subscriber * @param default_topic Name of the topic that will be loaded of the odom_topic param is not set. */ - explicit OdomSubscriber(YAML::Node& nh, std::string default_topic = "odom") + explicit OdomSubscriber(robot::NodeHandle& nh, std::string default_topic = "odom") { std::string odom_topic; // nh.param("odom_topic", odom_topic, default_topic); diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h index afae27e..b96a8f3 100755 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h @@ -52,7 +52,7 @@ namespace nav_2d_utils * @return Value of parameter if found, otherwise the default_value */ template -param_t searchAndGetParam(const YAML::Node& nh, const std::string& param_name, const param_t& default_value) +param_t searchAndGetParam(const robot::NodeHandle& nh, const std::string& param_name, const param_t& default_value) { std::string resolved_name; // if (nh.searchParam(param_name, resolved_name)) @@ -73,19 +73,19 @@ param_t searchAndGetParam(const YAML::Node& nh, const std::string& param_name, c * @return The value of the parameter or the default value */ template -param_t loadParameterWithDeprecation(const YAML::Node& nh, const std::string current_name, +param_t loadParameterWithDeprecation(const robot::NodeHandle& nh, const std::string current_name, const std::string old_name, const param_t& default_value) { param_t value; - if (nh[current_name] && nh[current_name].IsDefined()) + if (nh.hasParam(current_name)) { - value = nh[current_name].as(); + nh.getParam(current_name, value, default_value); return value; } - if (nh[old_name] && nh[old_name].IsDefined()) + if (nh.hasParam(old_name)) { std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl; - value = nh[old_name].as(); + nh.getParam(old_name, value, default_value); return value; } return default_value; @@ -98,14 +98,14 @@ param_t loadParameterWithDeprecation(const YAML::Node& nh, const std::string cur * @param old_name Deprecated parameter name */ template -void moveDeprecatedParameter(const YAML::Node& nh, const std::string current_name, const std::string old_name) +void moveDeprecatedParameter(const robot::NodeHandle& nh, const std::string current_name, const std::string old_name) { - if (!nh[old_name] || !nh[old_name].IsDefined()) return; + if (!nh.hasParam(old_name)) return; param_t value; std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl; - value = nh[old_name].as(); - nh[current_name] = value; + value = nh.param(old_name); + nh.setParam(current_name, value); } /** @@ -122,7 +122,7 @@ void moveDeprecatedParameter(const YAML::Node& nh, const std::string current_nam * @param should_delete If true, whether to delete the parameter from the old name */ template -void moveParameter(const YAML::Node& nh, std::string old_name, +void moveParameter(const robot::NodeHandle& nh, std::string old_name, std::string current_name, param_t default_value, bool should_delete = true) { // if (nh.hasParam(current_name)) diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h index 5e1b286..a46a335 100755 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h @@ -207,7 +207,7 @@ protected: // ROS Interface ros::ServiceServer switch_plugin_srv_; ros::Publisher current_plugin_pub_; - YAML::Node private_nh_; + robot::NodeHandle private_nh_; std::string ros_name_; // Switch Callback diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h index d7f6fde..80ca500 100755 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h +++ b/src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h @@ -89,7 +89,7 @@ nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string); // * @param search Whether to search up the namespace for the parameter name // * @return Loaded polygon // */ -// nav_2d_msgs::Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name, +// nav_2d_msgs::Polygon2D polygonFromParams(const robot::NodeHandle& nh, const std::string parameter_name, // bool search = true); /** @@ -116,7 +116,7 @@ XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool // * @param parameter_name Name of the parameter // * @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays // */ -// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name, +// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const robot::NodeHandle& nh, const std::string parameter_name, // bool array_of_arrays = true); /** diff --git a/src/Libraries/robot_cpp b/src/Libraries/robot_cpp index e3df693..1974d0d 160000 --- a/src/Libraries/robot_cpp +++ b/src/Libraries/robot_cpp @@ -1 +1 @@ -Subproject commit e3df6935432bcfdb14367bc99c2d114c385992a0 +Subproject commit 1974d0ddeea08a05b83363614d1a946db5dd07ee diff --git a/src/Navigations/Cores/nav_core2/CMakeLists.txt b/src/Navigations/Cores/nav_core2/CMakeLists.txt index 09cae9d..287af05 100755 --- a/src/Navigations/Cores/nav_core2/CMakeLists.txt +++ b/src/Navigations/Cores/nav_core2/CMakeLists.txt @@ -28,6 +28,8 @@ target_link_libraries(nav_core2 INTERFACE tf3 nav_grid nav_2d_msgs + robot::node_handle + robot::console ) # Set include directories diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h b/src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h index 8fde3f3..9b0485b 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h @@ -35,13 +35,13 @@ #ifndef NAV_CORE2_COSTMAP_H #define NAV_CORE2_COSTMAP_H +#include #include #include #include #include #include #include -#include namespace nav_core2 { @@ -71,7 +71,7 @@ public: * @param name The namespace for the costmap * @param tf A pointer to a transform listener */ - virtual void initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) {} + virtual void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) {} inline unsigned char getCost(const unsigned int x, const unsigned int y) { diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h b/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h index e5f5e69..5657018 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h @@ -68,7 +68,7 @@ public: * @param tf A pointer to a transform listener * @param costmap A pointer to the costmap */ - virtual void initialize(const YAML::Node& parent, const std::string& name, + virtual void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, Costmap::Ptr costmap) = 0; /** diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h b/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h index 1df51f6..c20e283 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h +++ b/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h @@ -76,7 +76,7 @@ namespace nav_core2 // virtual void initialize(YAML::Node &parent, const std::string &name, // TFListenerPtr tf, Costmap::Ptr costmap) = 0; - virtual void initialize(YAML::Node &parent, const std::string &name, + virtual void initialize(robot::NodeHandle &parent, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap) = 0; /** diff --git a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt b/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt index 3f46002..e34a722 100755 --- a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt +++ b/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt @@ -75,7 +75,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON) option(BUILD_TESTS "Build test programs" OFF) # Flags biên dịch -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +# Warning flags - disabled to suppress warnings during build +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h index ba612e9..194e8f4 100755 --- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h +++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h @@ -60,7 +60,7 @@ public: void initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false); // Standard Costmap Interface - void initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) override; + void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) override; nav_core2::Costmap::mutex_t* getMutex() override; // NavGrid Interface diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h index 2cad914..79edd91 100755 --- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h +++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h @@ -55,7 +55,7 @@ public: GlobalPlannerAdapter(); // Nav Core 2 Global Planner Interface - void initialize(const YAML::Node& parent, const std::string& name, + void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override; nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start, const nav_2d_msgs::Pose2DStamped& goal) override; diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h index 093acce..7b2930d 100755 --- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h +++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h @@ -35,6 +35,7 @@ #ifndef NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H #define NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H +#include #include #include #include @@ -197,8 +198,8 @@ namespace nav_core_adapter boost::recursive_mutex configuration_mutex_; // std::shared_ptr> dsrv_; - YAML::Node private_nh_; - YAML::Node adapter_nh_; + robot::NodeHandle private_nh_; + robot::NodeHandle adapter_nh_; // void reconfigureCB(nav_core_adapter::NavCoreAdapterConfig &config, uint32_t level); nav_core_adapter::NavCoreAdapterConfig last_config_; nav_core_adapter::NavCoreAdapterConfig default_config_; diff --git a/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp index a6ed199..be10d5f 100755 --- a/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp +++ b/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp @@ -72,10 +72,10 @@ void CostmapAdapter::initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool costmap_ = costmap_robot_->getCostmap(); } -void CostmapAdapter::initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) +void CostmapAdapter::initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) { // TODO: Implement this if needed - throw nav_core2::CostmapException("initialize with YAML::Node not implemented"); + throw nav_core2::CostmapException("initialize with robot::NodeHandle not implemented"); } nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex() diff --git a/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp index d2c13a9..643cbbf 100755 --- a/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp +++ b/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp @@ -53,7 +53,7 @@ namespace nav_core_adapter /** * @brief Load the nav_core global planner and initialize it */ - void GlobalPlannerAdapter::initialize(const YAML::Node& parent, const std::string& name, + void GlobalPlannerAdapter::initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) { costmap_ = costmap; diff --git a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp index e8ee33d..36a163a 100755 --- a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp +++ b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp @@ -32,6 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include #include @@ -79,23 +80,22 @@ namespace nav_core_adapter costmap_adapter_ = std::make_shared(); costmap_adapter_->initialize(costmap_robot); - YAML::Node nh; - private_nh_ = YAML::Node("~"); - adapter_nh_ = YAML::Node("~/" + name); - std::cout << "Adapter namespace: " << adapter_nh_["~"].as() << std::endl; - + robot::NodeHandle nh; + private_nh_ = robot::NodeHandle("~"); + adapter_nh_ = robot::NodeHandle(private_nh_, name); + robot::log_info("Adapter namespace: %s", private_nh_.getNamespace().c_str()); std::string planner_name; - if (adapter_nh_["planner_name"] && adapter_nh_["planner_name"].IsDefined()) + if (adapter_nh_.hasParam("planner_name")) { - planner_name = adapter_nh_["planner_name"].as(); + adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner")); } else { planner_name = nav_2d_utils::loadParameterWithDeprecation( adapter_nh_, "planner_name", "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner")); - adapter_nh_["planner_name"] = planner_name; + adapter_nh_.setParam("planner_name", planner_name); } - std::cout << "Loading plugin " << planner_name << std::endl; + robot::log_info("Loading plugin %s", planner_name.c_str()); // planner_ = planner_loader_.createInstance(planner_name); std::string path_file_so; @@ -104,10 +104,10 @@ namespace nav_core_adapter planner_ = planner_loader_(); if (!planner_) { - std::cerr << "Failed to load planner " << planner_name << std::endl; + robot::log_error("Failed to load planner %s", planner_name.c_str()); exit(EXIT_FAILURE); } - planner_->initialize(private_nh_, adapter_nh_["planner_name"].as(), tf_, costmap_robot_); + planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_); // odom_sub_ = std::make_shared(nh); // dsrv_ = std::make_shared>(configuration_mutex_, adapter_nh_); @@ -140,26 +140,17 @@ namespace nav_core_adapter std::cerr << "Failed to load planner " << planner_name << std::endl; exit(EXIT_FAILURE); } - new_planner->initialize(private_nh_, adapter_nh_["planner_name"].as(), - tf_, costmap_robot_); - if (!new_planner) - { - std::cerr << "Failed to load planner " << planner_name << std::endl; - exit(EXIT_FAILURE); - } - new_planner->initialize(private_nh_, adapter_nh_["planner_name"].as(), - tf_, costmap_robot_); - + new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_); if (planner_) planner_.reset(); planner_ = new_planner; last_config_.planner_name = planner_name; - std::cout << "Loaded new planner: " << planner_name << std::endl; + robot::log_info("Loaded new planner: %s", planner_name.c_str()); } catch (const std::exception &ex) { - std::cerr << "Failed to load planner " << planner_name << ": " << ex.what() << std::endl; + robot::log_error("Failed to load planner %s: %s", planner_name.c_str(), ex.what()); return false; } } diff --git a/src/Navigations/Packages/move_base/CMakeLists.txt b/src/Navigations/Packages/move_base/CMakeLists.txt index e992c7f..b233c9c 100644 --- a/src/Navigations/Packages/move_base/CMakeLists.txt +++ b/src/Navigations/Packages/move_base/CMakeLists.txt @@ -237,7 +237,9 @@ option(BUILD_TESTS "Build test programs" OFF) # ======================================================== # Compiler Flags # ======================================================== -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +# Warning flags - disabled to suppress warnings during build +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 7bfaa78..bb4b3b6 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -73,201 +73,196 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) // NodeHandle("~") will automatically load YAML files from config directory robot::NodeHandle nh("~"); private_nh_ = nh; + private_nh_.printAllParams(); recovery_trigger_ = PLANNING_R; - - robot::NodeHandle nh1 = robot::NodeHandle(nh); - nh1.setParam("local_costmap/obstacles/enabled", false); - // nh.printAllParams(); - // nh1.printAllParams(); - // nh.printAllParams(); - // // get some parameters that will be global to the move base node - // std::string global_planner, local_planner; - // private_nh_.getParam("base_global_planner", global_planner, std::string("")); - // printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str()); - // private_nh_.getParam("base_local_planner", local_planner, std::string("")); - // printf("[%s:%d] local_planner: %s\n", __FILE__, __LINE__, local_planner.c_str()); + // get some parameters that will be global to the move base node + std::string global_planner, local_planner; + private_nh_.getParam("base_global_planner", global_planner, std::string("")); + printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str()); + private_nh_.getParam("base_local_planner", local_planner, std::string("")); + printf("[%s:%d] local_planner: %s\n", __FILE__, __LINE__, local_planner.c_str()); - // // Handle nested YAML nodes for costmap config - // std::string robot_base_frame; - // private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint")); - // std::string global_frame; - // private_nh_.getParam("global_frame", global_frame, std::string("map")); - // robot_base_frame_ = robot_base_frame; - // global_frame_ = global_frame; - // double planner_frequency; - // private_nh_.getParam("planner_frequency", planner_frequency, 0.0); - // planner_frequency_ = planner_frequency; - // double controller_frequency; - // private_nh_.getParam("controller_frequency", controller_frequency, 20.0); - // controller_frequency_ = controller_frequency; - // double planner_patience; - // private_nh_.getParam("planner_patience", planner_patience, 5.0); - // planner_patience_ = planner_patience; - // private_nh_.getParam("controller_patience", controller_patience_, 15.0); - // double max_planning_retries; - // private_nh_.getParam("max_planning_retries", max_planning_retries, -1.0); - // max_planning_retries_ = max_planning_retries; - // double oscillation_timeout; - // private_nh_.getParam("oscillation_timeout", oscillation_timeout, 0.0); - // oscillation_timeout_ = oscillation_timeout; - // double oscillation_distance; - // private_nh_.getParam("oscillation_distance", oscillation_distance, 0.5); - // oscillation_distance_ = oscillation_distance; + // Handle nested YAML nodes for costmap config + std::string robot_base_frame; + private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint")); + std::string global_frame; + private_nh_.getParam("global_frame", global_frame, std::string("map")); + robot_base_frame_ = robot_base_frame; + global_frame_ = global_frame; + double planner_frequency; + private_nh_.getParam("planner_frequency", planner_frequency, 0.0); + planner_frequency_ = planner_frequency; + double controller_frequency; + private_nh_.getParam("controller_frequency", controller_frequency, 20.0); + controller_frequency_ = controller_frequency; + double planner_patience; + private_nh_.getParam("planner_patience", planner_patience, 5.0); + planner_patience_ = planner_patience; + private_nh_.getParam("controller_patience", controller_patience_, 15.0); + double max_planning_retries; + private_nh_.getParam("max_planning_retries", max_planning_retries, -1.0); + max_planning_retries_ = max_planning_retries; + double oscillation_timeout; + private_nh_.getParam("oscillation_timeout", oscillation_timeout, 0.0); + oscillation_timeout_ = oscillation_timeout; + double oscillation_distance; + private_nh_.getParam("oscillation_distance", oscillation_distance, 0.5); + oscillation_distance_ = oscillation_distance; - // double original_xy_goal_tolerance; - // private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2); - // original_xy_goal_tolerance_ = original_xy_goal_tolerance; - // double original_yaw_goal_tolerance; - // private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2); - // original_yaw_goal_tolerance_ = original_yaw_goal_tolerance; + double original_xy_goal_tolerance; + private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2); + original_xy_goal_tolerance_ = original_xy_goal_tolerance; + double original_yaw_goal_tolerance; + private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2); + original_yaw_goal_tolerance_ = original_yaw_goal_tolerance; - // // defined local planner name - // private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner")); - // private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner")); - // private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner")); - // private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner")); + // defined local planner name + private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner")); + private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner")); + private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner")); + private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner")); - // // parameters of make_plan service - // private_nh_.getParam("make_plan_clear_costmap", make_plan_clear_costmap_, false); - // private_nh_.getParam("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, false); - // private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0); - // min_approach_linear_velocity_ *= 1.2; - // // set up plan triple buffer - // planner_plan_ = new std::vector(); - // latest_plan_ = new std::vector(); - // controller_plan_ = new std::vector(); + // parameters of make_plan service + private_nh_.getParam("make_plan_clear_costmap", make_plan_clear_costmap_, false); + private_nh_.getParam("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, false); + private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0); + min_approach_linear_velocity_ *= 1.2; + // set up plan triple buffer + planner_plan_ = new std::vector(); + latest_plan_ = new std::vector(); + controller_plan_ = new std::vector(); - // // set up the planner's thread - // planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this)); + // set up the planner's thread + planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this)); - // // we'll assume the radius of the robot to be consistent with what's specified for the costmaps - // // From config param - // double inscribed_radius; - // private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2); - // double circumscribed_radius; - // private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3); - // inscribed_radius_ = inscribed_radius; - // circumscribed_radius_ = circumscribed_radius; - // private_nh_.getParam("clearing_radius", clearing_radius_, 0.0); - // double conservative_reset_dist; - // private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0); - // conservative_reset_dist_ = conservative_reset_dist; - // private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false); - // private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true); - // private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true); + // we'll assume the radius of the robot to be consistent with what's specified for the costmaps + // From config param + double inscribed_radius; + private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2); + double circumscribed_radius; + private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3); + inscribed_radius_ = inscribed_radius; + circumscribed_radius_ = circumscribed_radius; + private_nh_.getParam("clearing_radius", clearing_radius_, 0.0); + double conservative_reset_dist; + private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0); + conservative_reset_dist_ = conservative_reset_dist; + private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false); + private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true); + private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true); - // // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map - // planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_); - // planner_costmap_robot_->pause(); - // // initialize the global planner - // try - // { - // planner_loader_ = boost::dll::import_alias( - // "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so", - // global_planner, - // boost::dll::load_mode::append_decorations); + // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map + planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_); + planner_costmap_robot_->pause(); + // initialize the global planner + try + { + planner_loader_ = boost::dll::import_alias( + "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so", + global_planner, + boost::dll::load_mode::append_decorations); - // planner_ = planner_loader_(); - // if (!planner_) - // { - // robot::printf_red("[%s:%d] ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__); - // throw std::runtime_error("Failed to load global planner " + global_planner); - // } - // if(planner_->initialize(global_planner, planner_costmap_robot_)) - // printf("[%s:%d] Global planner initialized successfully\n", __FILE__, __LINE__); - // else - // robot::printf_red("[%s:%d] Global planner initialized failed\n", __FILE__, __LINE__); - // } - // catch (const std::exception &ex) - // { - // printf("[%s:%d] EXCEPTION in global planner: %s\n", __FILE__, __LINE__, ex.what()); - // throw std::runtime_error("Failed to create the " + global_planner + " planner"); - // } - // // create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map - // controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_); - // controller_costmap_robot_->pause(); - // // create a local planner - // try - // { - // std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Cores/nav_core_adapter/liblocal_planner_adapter.so"; - // controller_loader_ = - // boost::dll::import_alias( - // path_file_so, "LocalPlannerAdapter", boost::dll::load_mode::append_decorations); - // tc_ = controller_loader_(); - // if (!tc_) - // { - // robot::printf_red("[%s:%d] ERROR: controller_loader_() returned nullptr\n", __FILE__, __LINE__); - // throw std::runtime_error("Failed to load local planner " + local_planner); - // } - // // tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_); - // } - // catch (const std::exception &ex) - // { - // printf("[%s:%d] EXCEPTION in local planner: %s\n", __FILE__, __LINE__, ex.what()); - // throw std::runtime_error("Failed to create the " + local_planner + " planner"); - // } + planner_ = planner_loader_(); + if (!planner_) + { + robot::log_error("[%s:%d] ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__); + throw std::runtime_error("Failed to load global planner " + global_planner); + } + if(planner_->initialize(global_planner, planner_costmap_robot_)) + robot::log_info("[%s:%d] Global planner initialized successfully", __FILE__, __LINE__); + else + robot::log_error("[%s:%d] Global planner initialized failed", __FILE__, __LINE__); + } + catch (const std::exception &ex) + { + robot::log_error("[%s:%d] EXCEPTION in global planner: %s", __FILE__, __LINE__, ex.what()); + throw std::runtime_error("Failed to create the " + global_planner + " planner"); + } + // create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map + controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_); + controller_costmap_robot_->pause(); + // create a local planner + try + { + std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Cores/nav_core_adapter/liblocal_planner_adapter.so"; + controller_loader_ = + boost::dll::import_alias( + path_file_so, "LocalPlannerAdapter", boost::dll::load_mode::append_decorations); + tc_ = controller_loader_(); + if (!tc_) + { + robot::log_error("[%s:%d] ERROR: controller_loader_() returned nullptr", __FILE__, __LINE__); + throw std::runtime_error("Failed to load local planner " + local_planner); + } + // tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_); + } + catch (const std::exception &ex) + { + robot::log_error("[%s:%d] EXCEPTION in local planner: %s", __FILE__, __LINE__, ex.what()); + throw std::runtime_error("Failed to create the " + local_planner + " planner"); + } - // // Start actively updating costmaps based on sensor data - // planner_costmap_robot_->start(); - // controller_costmap_robot_->start(); + // Start actively updating costmaps based on sensor data + planner_costmap_robot_->start(); + controller_costmap_robot_->start(); - // try - // { - // old_linear_fwd_ = tc_->getTwistLinear(true); - // old_linear_bwd_ = tc_->getTwistLinear(false); + try + { + old_linear_fwd_ = tc_->getTwistLinear(true); + old_linear_bwd_ = tc_->getTwistLinear(false); - // old_angular_fwd_ = tc_->getTwistAngular(true); - // old_angular_rev_ = tc_->getTwistAngular(false); - // } - // catch (const std::exception &e) - // { - // std::cerr << e.what() << '\n'; - // } + old_angular_fwd_ = tc_->getTwistAngular(true); + old_angular_rev_ = tc_->getTwistAngular(false); + } + catch (const std::exception &e) + { + std::cerr << e.what() << '\n'; + } - // // // advertise a service for getting a plan - // // make_plan_srv_ = private_nh.advertiseService("make_plan", &move_base::MoveBase::planService, this); + // // advertise a service for getting a plan + // make_plan_srv_ = private_nh.advertiseService("make_plan", &move_base::MoveBase::planService, this); - // // // advertise a service for clearing the costmaps - // // clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &move_base::MoveBase::clearCostmapsService, this); + // // advertise a service for clearing the costmaps + // clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &move_base::MoveBase::clearCostmapsService, this); - // // if we shutdown our costmaps when we're deactivated... we'll do that now - // if (shutdown_costmaps_) - // { - // planner_costmap_robot_->stop(); - // controller_costmap_robot_->stop(); - // } + // if we shutdown our costmaps when we're deactivated... we'll do that now + if (shutdown_costmaps_) + { + planner_costmap_robot_->stop(); + controller_costmap_robot_->stop(); + } - // // load any user specified recovery behaviors, and if that fails load the defaults - // if (!loadRecoveryBehaviors(private_nh_)) - // { - // robot::printf_yellow("[%s:%d] Loading default recovery behaviors\n", __FILE__, __LINE__); - // loadDefaultRecoveryBehaviors(); - // } + // load any user specified recovery behaviors, and if that fails load the defaults + if (!loadRecoveryBehaviors(private_nh_)) + { + robot::log_warning("[%s:%d] Loading default recovery behaviors", __FILE__, __LINE__); + loadDefaultRecoveryBehaviors(); + } - // // initially, we'll need to make a plan - // state_ = move_base::PLANNING; + // initially, we'll need to make a plan + state_ = move_base::PLANNING; - // // we'll start executing recovery behaviors at the beginning of our list - // recovery_index_ = 0; - // nav_feedback_ = std::make_shared(); - // if (nav_feedback_) - // { - // nav_feedback_->navigation_state = move_base_core::State::PLANNING; - // nav_feedback_->is_ready = true; - // } - // else - // { - // robot::printf_red("[%s:%d] ERROR: nav_feedback_ is nullptr\n", __FILE__, __LINE__); - // } + // we'll start executing recovery behaviors at the beginning of our list + recovery_index_ = 0; + nav_feedback_ = std::make_shared(); + if (nav_feedback_) + { + nav_feedback_->navigation_state = move_base_core::State::PLANNING; + nav_feedback_->is_ready = true; + } + else + { + robot::log_error("[%s:%d] ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__); + } - // initialized_ = true; - // printf("[%s:%d] ========== End: initialize() - SUCCESS ==========\n", __FILE__, __LINE__); + initialized_ = true; + robot::log_info("[%s:%d] ========== End: initialize() - SUCCESS ==========", __FILE__, __LINE__); } else { - robot::printf_yellow("[%s:%d] Already initialized, skipping\n", __FILE__, __LINE__); + robot::log_warning("[%s:%d] Already initialized, skipping", __FILE__, __LINE__); } }