diff --git a/.gitignore b/.gitignore index 2a91aa3..138c11e 100644 --- a/.gitignore +++ b/.gitignore @@ -203,6 +203,10 @@ PublishScripts/ !**/[Pp]ackages/build/ # except move_base package in Navigations !src/Navigations/Packages/move_base/ +# except packages in Algorithms +!src/Algorithms/Packages/ +!src/Algorithms/Packages/global_planners/ +!src/Algorithms/Packages/local_planners/ # Uncomment if necessary however generally it will be regenerated when needed #!**/[Pp]ackages/repositories.config # NuGet v3's project.json files produces more ignorable files diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt b/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt new file mode 100755 index 0000000..db8a467 --- /dev/null +++ b/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt @@ -0,0 +1,95 @@ +cmake_minimum_required(VERSION 3.10) + +# Tên dự án +project(two_points_planner VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +# Build type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +# Find system dependencies +find_package(Boost REQUIRED COMPONENTS filesystem system) + +# Thư mục include +include_directories( + ${PROJECT_SOURCE_DIR}/include +) + +# Tìm tất cả file source +file(GLOB SOURCES "src/two_points_planner.cpp") +file(GLOB HEADERS "include/two_points_planner/*.h") + +# Dependencies packages (internal libraries) +set(PACKAGES_DIR + costmap_2d + nav_core + geometry_msgs + nav_msgs + std_msgs + tf3 + tf3_geometry_msgs +) + +# Tạo thư viện shared (.so) +add_library(two_points_planner SHARED ${SOURCES} ${HEADERS}) + +# Link libraries +target_link_libraries(two_points_planner + PUBLIC ${PACKAGES_DIR} + PUBLIC robot::node_handle robot::console + PRIVATE Boost::boost +) + +# Set include directories +target_include_directories(two_points_planner + PUBLIC + $ + $ +) + +# Cài đặt header files +install(DIRECTORY include/two_points_planner + DESTINATION include + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +# Cài đặt library +install(TARGETS two_points_planner + EXPORT two_points_planner-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) +# Export targets +install(EXPORT two_points_planner-targets + FILE two_points_planner-targets.cmake + DESTINATION lib/cmake/two_points_planner +) + +# Tùy chọn build +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") +set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") + +# In thông tin cấu hình +message(STATUS "=================================") +message(STATUS "Project: two_points_planner") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") +message(STATUS "Build type: ${CMAKE_BUILD_TYPE}") +message(STATUS "Headers found:") +foreach(hdr ${HEADERS}) + message(STATUS " - ${hdr}") +endforeach() +message(STATUS "=================================") diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h b/src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h new file mode 100755 index 0000000..8fc77d0 --- /dev/null +++ b/src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h @@ -0,0 +1,83 @@ + +#ifndef TWO_POINTS_PLANNER_H_INCLUDED +#define TWO_POINTS_PLANNER_H_INCLUDED + +#include +#include +#include +#include +#include +#include + +namespace two_points_planner +{ + +/** + * @class TwoPointsPlanner + * @brief Plugin-based flexible global_planner + */ +class TwoPointsPlanner : public nav_core::BaseGlobalPlanner +{ +public: + /** + * @brief Default constructor for the NavFnROS object + */ + TwoPointsPlanner(); + + virtual ~TwoPointsPlanner(){}; + + /** + * @brief Constructor for the TwoPointsPlanner object + * @param name The name of this planner + * @param costmap_robot A pointer to the ROS wrapper of the costmap to use + */ + TwoPointsPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot); + + /** + * @brief Initialization function for the TwoPointsPlanner object + * @param name The name of this planner + * @param costmap_robot A pointer to the ROS wrapper of the costmap to use + */ + virtual bool initialize(std::string name, + costmap_2d::Costmap2DROBOT* costmap_robot); + + /** + * @brief Given a goal pose in the world, compute a plan + * @param start The start pose + * @param goal The goal pose + * @param plan The plan... filled by the planner + * @return True if a valid plan was found, false otherwise + */ + virtual bool makePlan(const geometry_msgs::PoseStamped& start, + const geometry_msgs::PoseStamped& goal, + std::vector& plan); + + /** + * @brief Create a new TwoPointsPlanner instance + * @return A pointer to the new TwoPointsPlanner instance + */ + static nav_core::BaseGlobalPlanner::Ptr create(); + +protected: + + unsigned char computeCircumscribedCost(); + unsigned char costMapCostToSBPLCost(unsigned char newcost); + + bool initialized_; + unsigned char lethal_obstacle_; + unsigned char inscribed_inflated_obstacle_; + unsigned char circumscribed_cost_; + + bool allow_unknown_; + + std::string name_; + costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */ + std::vector footprint_; + unsigned int current_env_width_; + unsigned int current_env_height_; + +}; + +} // namespace two_points_planner + +#endif // TWO_POINTS_PLANNER_H_INCLUDED \ No newline at end of file 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 new file mode 100755 index 0000000..056d71c --- /dev/null +++ b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp @@ -0,0 +1,221 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace two_points_planner +{ + TwoPointsPlanner::TwoPointsPlanner() : initialized_(false), costmap_robot_(NULL) {} + + TwoPointsPlanner::TwoPointsPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) + : initialized_(false), costmap_robot_(NULL) + { + initialize(name, costmap_robot); + } + + bool TwoPointsPlanner::initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) + { + if (!initialized_) + { + robot::NodeHandle nh_priv_("~/" + name); + printf("TwoPointsPlanner: Name is %s\n", name.c_str()); + + int lethal_obstacle; + nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20); + lethal_obstacle_ = (unsigned char)lethal_obstacle; + inscribed_inflated_obstacle_ = lethal_obstacle_ - 1; + + nh_priv_.getParam("allow_unknown", allow_unknown_, true); + + name_ = name; + costmap_robot_ = costmap_robot; + + // Bug + if(!costmap_robot_ || !costmap_robot_->getCostmap()) + { + robot::printf_yellow("[%s:%d]TwoPointsPlanner Initialized failed\n", __FILE__, __LINE__); + return false; + } + current_env_width_ = costmap_robot_->getCostmap()->getSizeInCellsX(); + current_env_height_ = costmap_robot_->getCostmap()->getSizeInCellsY(); + footprint_ = costmap_robot_->getRobotFootprint(); + + printf("TwoPointsPlanner Initialized successfully\n"); + initialized_ = true; + return true; + } + } + + unsigned char TwoPointsPlanner::computeCircumscribedCost() + { + unsigned char result = 0; + + // Bug + // 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; + + // result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / costmap_robot_->getCostmap()->getResolution())); + // } + return result; + } + + // Taken from Sachin's sbpl_cart_planner + // This rescales the costmap according to a rosparam which sets the obstacle cost + unsigned char TwoPointsPlanner::costMapCostToSBPLCost(unsigned char newcost) + { + if (newcost == costmap_2d::LETHAL_OBSTACLE || (!allow_unknown_ && newcost == costmap_2d::NO_INFORMATION)) + return lethal_obstacle_; + else if (newcost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + return inscribed_inflated_obstacle_; + else if (newcost == 0) + return 0; + else if (newcost == costmap_2d::NO_INFORMATION) + return costmap_2d::FREE_SPACE; + else + { + unsigned char cost = newcost; + if (cost == 0) + cost = 1; + return cost; + } + } + + bool TwoPointsPlanner::makePlan(const geometry_msgs::PoseStamped& start, + const geometry_msgs::PoseStamped& goal, + std::vector& plan) + { + if (!initialized_) + { + std::cerr << "TwoPointsPlanner: Global planner is not initialized" << std::endl; + 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; + // } + + // 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); + + // 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; + // } + + // // 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); + + // // 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; + + // // 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(); + + // plan.push_back(pose); + // } + // plan.push_back(goal); + return true; + } + + nav_core::BaseGlobalPlanner::Ptr TwoPointsPlanner::create() + { + return std::make_shared(); + } + +} // namespace two_points_planner + +// Register this planner as a GlobalPlanner plugin +BOOST_DLL_ALIAS(two_points_planner::TwoPointsPlanner::create, TwoPointsPlanner) \ No newline at end of file