Add Algorithms packages
This commit is contained in:
parent
7a26f537f8
commit
5db993383b
4
.gitignore
vendored
4
.gitignore
vendored
|
|
@ -203,6 +203,10 @@ PublishScripts/
|
||||||
!**/[Pp]ackages/build/
|
!**/[Pp]ackages/build/
|
||||||
# except move_base package in Navigations
|
# except move_base package in Navigations
|
||||||
!src/Navigations/Packages/move_base/
|
!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
|
# Uncomment if necessary however generally it will be regenerated when needed
|
||||||
#!**/[Pp]ackages/repositories.config
|
#!**/[Pp]ackages/repositories.config
|
||||||
# NuGet v3's project.json files produces more ignorable files
|
# NuGet v3's project.json files produces more ignorable files
|
||||||
|
|
|
||||||
95
src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt
Executable file
95
src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt
Executable file
|
|
@ -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
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include>
|
||||||
|
)
|
||||||
|
|
||||||
|
# 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 "=================================")
|
||||||
|
|
@ -0,0 +1,83 @@
|
||||||
|
|
||||||
|
#ifndef TWO_POINTS_PLANNER_H_INCLUDED
|
||||||
|
#define TWO_POINTS_PLANNER_H_INCLUDED
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <vector>
|
||||||
|
#include <geometry_msgs/PoseStamped.h>
|
||||||
|
#include <nav_core/base_global_planner.h>
|
||||||
|
#include <costmap_2d/costmap_2d_robot.h>
|
||||||
|
#include <robot/node_handle.h>
|
||||||
|
|
||||||
|
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<geometry_msgs::PoseStamped>& 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<geometry_msgs::Point> footprint_;
|
||||||
|
unsigned int current_env_width_;
|
||||||
|
unsigned int current_env_height_;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace two_points_planner
|
||||||
|
|
||||||
|
#endif // TWO_POINTS_PLANNER_H_INCLUDED
|
||||||
|
|
@ -0,0 +1,221 @@
|
||||||
|
#include <two_points_planner/two_points_planner.h>
|
||||||
|
#include <costmap_2d/inflation_layer.h>
|
||||||
|
#include <boost/dll/alias.hpp>
|
||||||
|
#include <nav_msgs/Path.h>
|
||||||
|
#include <tf3/LinearMath/Quaternion.h>
|
||||||
|
#include <tf3/utils.h>
|
||||||
|
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||||
|
#include <robot/time.h>
|
||||||
|
#include <robot/console.h>
|
||||||
|
#include <cmath>
|
||||||
|
#include <cstdio>
|
||||||
|
|
||||||
|
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<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin();
|
||||||
|
// layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end();
|
||||||
|
// ++layer)
|
||||||
|
// {
|
||||||
|
// boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*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<geometry_msgs::PoseStamped>& 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<double>(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<TwoPointsPlanner>();
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace two_points_planner
|
||||||
|
|
||||||
|
// Register this planner as a GlobalPlanner plugin
|
||||||
|
BOOST_DLL_ALIAS(two_points_planner::TwoPointsPlanner::create, TwoPointsPlanner)
|
||||||
Loading…
Reference in New Issue
Block a user