HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

udpdate

Browse Source
This commit is contained in:
HiepLM
2025-12-25 11:04:24 +07:00
parent 3c728db668
commit a9a2445305
29 changed files with 228 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
View File

@@ -9,6 +9,9 @@
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Vector3.h>
// robot protocol msgs
#include <robot_protocol_msgs/Order.h>
// tf
#include <tf3/buffer_core.h>
#include <tf3/utils.h>
@@ -60,35 +63,35 @@ namespace move_base_core
switch (state)
{
case State::PENDING:
return "PENDING"; // Chờ xử lý
return "PENDING"; // Chờ xử lý
case State::ACTIVE:
return "ACTIVE"; // Đang hoạt động
return "ACTIVE"; // Đang hoạt động
case State::PREEMPTED:
return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới
return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới
case State::SUCCEEDED:
return "SUCCEEDED"; // Thành công
return "SUCCEEDED"; // Thành công
case State::ABORTED:
return "ABORTED"; // Bị lỗi
return "ABORTED"; // Bị lỗi
case State::REJECTED:
return "REJECTED"; // Từ chối bởi planner hoặc controller
return "REJECTED"; // Từ chối bởi planner hoặc controller
case State::PREEMPTING:
return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu
return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu
case State::RECALLING:
return "RECALLING"; // Đang huỷ bỏ nội bộ
return "RECALLING"; // Đang huỷ bỏ nội bộ
case State::RECALLED:
return "RECALLED"; // Đã được thu hồi
return "RECALLED"; // Đã được thu hồi
case State::LOST:
return "LOST"; // Mất trạng thái
return "LOST"; // Mất trạng thái
case State::PLANNING:
return "PLANNING"; // Đang lập kế hoạch đường đi
return "PLANNING"; // Đang lập kế hoạch đường đi
case State::CONTROLLING:
return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan
return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan
case State::CLEARING:
return "CLEARING"; // Đang dọn dẹp bản đồ / costmap
return "CLEARING"; // Đang dọn dẹp bản đồ / costmap
case State::PAUSED:
return "PAUSED"; // Tạm dừng
return "PAUSED"; // Tạm dừng
default:
return "UNKNOWN_STATE"; // Không xác định
return "UNKNOWN_STATE"; // Không xác định
}
}
@@ -165,7 +168,7 @@ namespace move_base_core
* @param fprt A vector of points representing the robot's footprint polygon.
* The points should be ordered counter-clockwise.
* Example:
*
*
^ Y
|
| P3(-0.3, 0.2) P2(0.3, 0.2)
@@ -196,6 +199,19 @@ namespace move_base_core
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Send a goal for the robot to navigate to.
* @param msg Order message.
* @param goal Target pose in the global frame.
* @param xy_goal_tolerance Acceptable error in X/Y (meters).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo(const robot_protocol_msgs::Order &msg,
const geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Send a docking goal to a predefined marker (e.g. charger).
* @param maker Marker name or ID.
@@ -209,6 +225,19 @@ namespace move_base_core
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Send a docking goal to a predefined marker (e.g. charger).
* @param msg Order message.
* @param goal Target pose for docking.
* @param xy_goal_tolerance Acceptable XY error (meters).
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo(const robot_protocol_msgs::Order &msg,
const geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Move straight toward the target position (X-axis).
* @param goal Target pose.

12
src/Navigations/Cores/nav_core_adapter/CMakeLists.txt
View File

@@ -66,19 +66,13 @@ set(_NAV_CORE_ADAPTER_RPATH
)
set_target_properties(costmap_adapter PROPERTIES
BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
LINK_FLAGS "-Wl,--disable-new-dtags"
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
set_target_properties(local_planner_adapter PROPERTIES
BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
LINK_FLAGS "-Wl,--disable-new-dtags"
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
set_target_properties(global_planner_adapter PROPERTIES
BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}"
LINK_FLAGS "-Wl,--disable-new-dtags"
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
# Cài đặt header files

3
src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp
View File

@@ -93,10 +93,9 @@ namespace nav_core_adapter
planner_name = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
adapter_nh_.setParam("planner_name", planner_name);
}
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
planner_loader_ = boost::dll::import_alias<nav_core2::LocalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();

4
src/Navigations/Packages/move_base/CMakeLists.txt
View File

@@ -157,9 +157,7 @@ else()
# Set RPATH để ưu tiên thư viện build cục bộ
set_target_properties(move_base PROPERTIES
BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp"
INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp"
LINK_FLAGS "-Wl,--disable-new-dtags"
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
endif()

44
src/Navigations/Packages/move_base/include/move_base/move_base.h
View File

@@ -27,6 +27,7 @@
#include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_msgs/GetPlan.h>
#include <robot_protocol_msgs/Order.h>
#include <robot/node_handle.h>
#include <robot/console.h>
@@ -94,6 +95,19 @@ namespace move_base
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) override;
/**
* @brief Send a goal for the robot to navigate to.
* @param msg Order message.
* @param goal Target pose in the global frame.
* @param xy_goal_tolerance Acceptable error in X/Y (meters).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo(const robot_protocol_msgs::Order &msg,
const geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) override;
/**
* @brief Send a docking goal to a predefined marker (e.g. charger).
* @param maker Marker name or ID.
@@ -107,8 +121,22 @@ namespace move_base
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) override;
/**
* @brief Send a docking goal to a predefined marker (e.g. charger).
* @param msg Order message.
* @param goal Target pose for docking.
* @param xy_goal_tolerance Acceptable XY error (meters).
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo(const robot_protocol_msgs::Order &msg,
const geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) override;
/**
* @brief Move straight toward the target position (X-axis).
* @param msg Order message.
* @param goal Target pose.
* @param xy_goal_tolerance Acceptable positional error (meters).
* @return True if command issued successfully.
@@ -206,22 +234,6 @@ namespace move_base
*/
void swapPlanner(std::string base_global_planner);
// /**
// * @brief A service call that clears the costmaps of obstacles
// * @param req The service request
// * @param resp The service response
// * @return True if the service call succeeds, false otherwise
// */
// bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
// /**
// * @brief A service call that can be made when the action is inactive that will return a plan
// * @param req The goal request
// * @param resp The plan request
// * @return True if planning succeeded, false otherwise
// */
// bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
/**
* @brief Make a new global plan
* @param goal The goal to plan to

7
src/Navigations/Packages/move_base/plugins.xml Normal file
View File

@@ -0,0 +1,7 @@
<class_libraries>
<library path="lib/libmove_base">
<class type="move_base::MoveBase" base_class_type="move_base_core::BaseNavigation">
<description></description>
</class>
</library>
</class_libraries>

62
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -15,6 +15,7 @@
#include <boost/thread.hpp>
#include <boost/dll/import.hpp>
#include <boost/dll/alias.hpp>
#include <robot/plugin_loader_helper.h>
move_base::MoveBase::MoveBase()
: initialized_(false),
@@ -156,34 +157,24 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
// we'll assume the radius of the robot to be consistent with what's specified for the costmaps
// From config param
double inscribed_radius;
robot::log_info("[%s:%d] inscribed_radius: %f", __FILE__, __LINE__, 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);
robot::log_info("[%s:%d] circumscribed_radius: %f", __FILE__, __LINE__, circumscribed_radius);
inscribed_radius_ = inscribed_radius;
circumscribed_radius_ = circumscribed_radius;
private_nh_.getParam("clearing_radius", clearing_radius_, 0.0);
robot::log_info("[%s:%d] clearing_radius: %f", __FILE__, __LINE__, clearing_radius_);
double conservative_reset_dist;
private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0);
robot::log_info("[%s:%d] conservative_reset_dist: %f", __FILE__, __LINE__, conservative_reset_dist);
conservative_reset_dist_ = conservative_reset_dist;
robot::log_info("[%s:%d] shutdown_costmaps: %f", __FILE__, __LINE__, shutdown_costmaps_);
private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false);
robot::log_info("[%s:%d] clearing_rotation_allowed: %f", __FILE__, __LINE__, clearing_rotation_allowed_);
private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
robot::log_info("[%s:%d] recovery_behavior_enabled: %f", __FILE__, __LINE__, recovery_behavior_enabled_);
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
robot::log_info("[%s:%d] create the ros wrapper for the planner's costmap...");
try
{
planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
robot::log_info("[%s:%d] planner_costmap_robot_->pause()");
planner_costmap_robot_->pause();
robot::log_info("[%s:%d] planner_costmap_robot_->pause()");
}
catch (const std::exception &ex)
{
@@ -193,10 +184,10 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
// initialize the global planner
try
{
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath("CustomPlanner");
planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
"/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);
path_file_so, global_planner, boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();
if (!planner_)
@@ -214,13 +205,22 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
robot::log_error("[%s:%d]\n 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();
try
{
controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
controller_costmap_robot_->pause();
}
catch (const std::exception &ex)
{
robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what());
throw std::runtime_error("Failed to create the controller_costmap_robot_");
}
// 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";
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath("LocalPlannerAdapter");
controller_loader_ =
boost::dll::import_alias<nav_core::BaseLocalPlanner::Ptr()>(
path_file_so, local_planner, boost::dll::load_mode::append_decorations);
@@ -363,15 +363,18 @@ bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double
if (cancel_ctr_)
cancel_ctr_ = false;
// move_base_msgs::MoveBaseActionGoal action_goal;
// action_goal.header.stamp = robot::Time::now();
// action_goal.goal.target_pose = goal;
// action_goal_pub_.publish(action_goal);
lock.unlock();
return true;
}
bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
const geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
return false;
}
bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
@@ -454,14 +457,17 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::
if (cancel_ctr_)
cancel_ctr_ = false;
// move_base_msgs::MoveBaseActionGoal action_goal;
// action_goal.header.stamp = robot::Time::now();
// action_goal.goal.target_pose = goal;
// action_goal_pub_.publish(action_goal);
lock.unlock();
return true;
}
bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
const geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
return false;
}
bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
{
if (setup_)
@@ -505,10 +511,6 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal,
}
if (cancel_ctr_)
cancel_ctr_ = false;
// move_base_msgs::MoveBaseActionGoal action_goal;
// action_goal.header.stamp = robot::Time::now();
// action_goal.goal.target_pose = goal;
// action_goal_pub_.publish(action_goal);
lock.unlock();
return true;