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/
|
||||
# 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
|
||||
|
|
|
|||
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