From 63b3effbb96fe8fa3e44125ef68390115b2bf8ff Mon Sep 17 00:00:00 2001 From: HiepLM Date: Fri, 5 Dec 2025 15:53:01 +0700 Subject: [PATCH] add move_base --- .gitignore | 2 + .../Packages/move_base/BUILD_INSTRUCTIONS.md | 142 ++ .../Packages/move_base/CMakeLists.txt | 257 ++++ .../move_base/include/move_base/move_base.h | 354 +++++ .../Packages/move_base/package.xml | 72 + .../Packages/move_base/src/move_base.cpp | 1233 +++++++++++++++++ .../Packages/move_base/src/move_base_main.cpp | 40 + 7 files changed, 2100 insertions(+) create mode 100644 src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md create mode 100644 src/Navigations/Packages/move_base/CMakeLists.txt create mode 100644 src/Navigations/Packages/move_base/include/move_base/move_base.h create mode 100644 src/Navigations/Packages/move_base/package.xml create mode 100644 src/Navigations/Packages/move_base/src/move_base.cpp create mode 100644 src/Navigations/Packages/move_base/src/move_base_main.cpp diff --git a/.gitignore b/.gitignore index cbd4eda..2a91aa3 100644 --- a/.gitignore +++ b/.gitignore @@ -201,6 +201,8 @@ PublishScripts/ **/[Pp]ackages/* # except build/, which is used as an MSBuild target. !**/[Pp]ackages/build/ +# except move_base package in Navigations +!src/Navigations/Packages/move_base/ # 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/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md b/src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md new file mode 100644 index 0000000..cc2b262 --- /dev/null +++ b/src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md @@ -0,0 +1,142 @@ +# Move Base - Dual Build System Support + +Package `move_base` có thể được build bằng cả **Catkin** và **Standalone CMake**. + +## Cách Build + +### 1. Build với Catkin (ROS Workspace) + +**⚠️ QUAN TRỌNG**: Thư mục gốc của project (`pnkx_nav_core`) có `CMakeLists.txt` ở root, +nên **KHÔNG THỂ** chạy `catkin_make` trực tiếp trong thư mục đó. Catkin workspace không được +phép có `CMakeLists.txt` ở root. + +**Giải pháp**: Tạo một catkin workspace riêng và link packages vào đó. + +#### Cách 1: Sử dụng script tự động (Khuyến nghị) + +```bash +# Chạy script setup từ thư mục gốc của project +cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core +./setup_catkin_workspace.sh + +# Script sẽ tạo workspace và link các packages có package.xml +# Sau đó build: +cd ../pnkx_nav_catkin_ws +catkin_make +source devel/setup.bash +``` + +#### Cách 2: Tạo workspace thủ công + +```bash +# Tạo catkin workspace +mkdir -p ~/pnkx_nav_catkin_ws/src +cd ~/pnkx_nav_catkin_ws/src + +# Link package move_base vào workspace +ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/src/Navigations/Packages/move_base move_base + +# Link các dependencies cần thiết (nếu có package.xml) +ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/src/Libraries/tf3 tf3 +ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/src/Libraries/nav_2d_utils nav_2d_utils + +# Build với catkin +cd ~/pnkx_nav_catkin_ws +catkin_make +source devel/setup.bash +``` + +**Lưu ý**: Khi build với catkin, package sẽ tự động detect và sử dụng: +- `catkin_package()` để export dependencies +- Catkin installation paths +- ROS message dependencies + +### 2. Build với Standalone CMake + +```bash +cd /path/to/move_base +mkdir -p build +cd build +cmake .. +make +``` + +**Lưu ý**: Khi build standalone, bạn cần đảm bảo: +- Tất cả dependencies đã được build trước +- CMake có thể tìm thấy các internal packages (move_base_core, nav_core, etc.) +- ROS packages (nếu cần) được cài đặt và có trong CMAKE_PREFIX_PATH + +## Cách CMakeLists.txt Detect Build Mode + +CMakeLists.txt tự động detect build mode bằng cách kiểm tra: +- `CATKIN_DEVEL_PREFIX` - biến môi trường của catkin +- `CATKIN_TOPLEVEL` - biến môi trường của catkin + +Nếu một trong hai biến này được định nghĩa, package sẽ build ở **Catkin mode**. +Ngược lại, sẽ build ở **Standalone CMake mode**. + +## Dependencies + +### ROS Packages (khi build với Catkin): +- roscpp, rospy +- std_msgs, geometry_msgs, nav_msgs +- tf2, tf2_ros +- actionlib, dynamic_reconfigure + +### Internal Packages (cần build trước): +- move_base_core +- nav_core +- costmap_2d +- xmlrpcpp +- node_handle +- tf3_sensor_msgs +- tf3_geometry_msgs + +### System Libraries: +- Boost +- yaml-cpp +- pthread, dl + +## Kiểm tra Build Mode + +Khi chạy `cmake`, bạn sẽ thấy message: +``` +Building move_base with Catkin +``` +hoặc +``` +Building move_base with Standalone CMake +``` + +## Troubleshooting + +### Lỗi: "catkin_make must be invoked in the root of workspace" +**Nguyên nhân**: Thư mục hiện tại có `CMakeLists.txt` ở root, catkin không cho phép điều này. + +**Giải pháp**: +- Tạo catkin workspace riêng (xem hướng dẫn ở trên) +- Hoặc đổi tên/ẩn `CMakeLists.txt` ở root khi build với catkin (không khuyến nghị) + +### Lỗi: "Cannot find package X" +- **Với Catkin**: + - Đảm bảo package X đã được build trong cùng workspace + - Đảm bảo package X có `package.xml` và được link vào `src/` của workspace + - Kiểm tra dependencies trong `package.xml` +- **Với Standalone**: + - Đảm bảo package X đã được build và cài đặt + - Thêm vào `CMAKE_PREFIX_PATH` nếu cần + +### Lỗi: "catkin_package() not found" +- Đảm bảo bạn đang build trong catkin workspace (không phải thư mục gốc có CMakeLists.txt) +- Kiểm tra `CATKIN_DEVEL_PREFIX` có được set không +- Source ROS setup: `source /opt/ros/noetic/setup.bash` + +### Lỗi: "Cannot find ROS packages" +- Cài đặt ROS Noetic: `sudo apt-get install ros-noetic-` +- Source ROS setup: `source /opt/ros/noetic/setup.bash` +- Kiểm tra `ROS_PACKAGE_PATH` environment variable + +### Lỗi: Internal packages không tìm thấy (move_base_core, nav_core, etc.) +- **Với Catkin**: Các internal packages cần có `package.xml` và được link vào workspace +- **Với Standalone**: Đảm bảo các packages đã được build trước trong project chính + diff --git a/src/Navigations/Packages/move_base/CMakeLists.txt b/src/Navigations/Packages/move_base/CMakeLists.txt new file mode 100644 index 0000000..e992c7f --- /dev/null +++ b/src/Navigations/Packages/move_base/CMakeLists.txt @@ -0,0 +1,257 @@ +cmake_minimum_required(VERSION 3.10) + +# ======================================================== +# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake +# ======================================================== + +# Detect if building with Catkin +if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) + set(BUILDING_WITH_CATKIN TRUE) + message(STATUS "Building move_base with Catkin") +else() + set(BUILDING_WITH_CATKIN FALSE) + message(STATUS "Building move_base with Standalone CMake") +endif() + +# Tên dự án +project(move_base VERSION 1.0.0 LANGUAGES CXX) + +# Chuẩn C++ +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +# ======================================================== +# Find Packages +# ======================================================== +find_package(Boost REQUIRED) + +# If building with Catkin, use catkin_package to find dependencies +if(BUILDING_WITH_CATKIN) + find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + geometry_msgs + nav_msgs + tf2 + tf2_ros + actionlib + dynamic_reconfigure + ) + + # Find internal packages (these should be built in the same workspace) + # Catkin will handle these through workspace dependencies + find_package(yaml-cpp REQUIRED) +endif() + +# ======================================================== +# Source Files +# ======================================================== +file(GLOB SOURCES "src/move_base.cpp") +file(GLOB HEADERS "include/move_base/move_base.h") + +# ======================================================== +# Include Directories +# ======================================================== +include_directories( + ${PROJECT_SOURCE_DIR}/include +) + +if(BUILDING_WITH_CATKIN) + include_directories( + ${catkin_INCLUDE_DIRS} + ) +endif() + +# ======================================================== +# RPATH settings: ưu tiên thư viện build tại chỗ +# ======================================================== +set(CMAKE_SKIP_BUILD_RPATH FALSE) +set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) +set(CMAKE_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") +set(CMAKE_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") +set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + +# ======================================================== +# Create Library +# ======================================================== +add_library(move_base SHARED ${SOURCES} ${HEADERS}) + +# ======================================================== +# Dependencies and Link Libraries +# ======================================================== +if(BUILDING_WITH_CATKIN) + # Catkin mode: use catkin dependencies + catkin_package( + INCLUDE_DIRS include + LIBRARIES move_base + CATKIN_DEPENDS + roscpp + rospy + std_msgs + geometry_msgs + nav_msgs + tf2 + tf2_ros + actionlib + dynamic_reconfigure + DEPENDS + yaml-cpp + Boost + ) + + # Link against catkin packages + target_link_libraries(move_base + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + yaml-cpp + ) + + # Internal packages (assumed to be in same workspace) + # These will be linked via catkin workspace dependencies + target_link_libraries(move_base + move_base_core + nav_core + costmap_2d + xmlrpcpp + robot::node_handle + robot::console + tf3_sensor_msgs + tf3_geometry_msgs + dl + pthread + ) + + # 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" + ) +else() + # Standalone CMake mode: link all dependencies manually + set(PACKAGES_DIR + geometry_msgs + std_msgs + move_base_core + nav_core + costmap_2d + yaml-cpp + xmlrpcpp + tf3_sensor_msgs + tf3_geometry_msgs + data_convert + dl + pthread + ) + + target_link_libraries(move_base + PUBLIC ${PACKAGES_DIR} + PUBLIC robot::node_handle robot::console + PRIVATE Boost::boost + ) + + # 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" + ) +endif() + +# ======================================================== +# Include Directories for Target +# ======================================================== +target_include_directories(move_base + PUBLIC + $ + $ +) + +if(BUILDING_WITH_CATKIN) + target_include_directories(move_base + PUBLIC ${catkin_INCLUDE_DIRS} + ) +endif() + +# ======================================================== +# Executable +# ======================================================== +add_executable(move_base_main src/move_base_main.cpp) +target_link_libraries(move_base_main + PRIVATE move_base + PRIVATE Boost::boost +) + +if(BUILDING_WITH_CATKIN) + target_link_libraries(move_base_main + PRIVATE ${catkin_LIBRARIES} + ) +endif() + +# ======================================================== +# Installation +# ======================================================== +if(BUILDING_WITH_CATKIN) + # Catkin installation + install(DIRECTORY include/move_base + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + ) + + install(TARGETS move_base move_base_main + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) +else() + # Standalone CMake installation + install(DIRECTORY include/move_base + DESTINATION include + FILES_MATCHING PATTERN "*.h" + ) + + install(TARGETS move_base move_base_main + EXPORT move_base-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + ) + + install(EXPORT move_base-targets + FILE move_base-targets.cmake + DESTINATION lib/cmake/move_base + ) +endif() + +# ======================================================== +# Build Options +# ======================================================== +option(BUILD_SHARED_LIBS "Build shared libraries" ON) +option(BUILD_TESTS "Build test programs" OFF) + +# ======================================================== +# Compiler Flags +# ======================================================== +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") + +# ======================================================== +# Status Messages +# ======================================================== +message(STATUS "=================================") +message(STATUS "Project: ${PROJECT_NAME}") +message(STATUS "Version: ${PROJECT_VERSION}") +message(STATUS "Build mode: ${BUILDING_WITH_CATKIN}") +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/Navigations/Packages/move_base/include/move_base/move_base.h b/src/Navigations/Packages/move_base/include/move_base/move_base.h new file mode 100644 index 0000000..a397a1e --- /dev/null +++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h @@ -0,0 +1,354 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Author: Kevin Lee + *********************************************************************/ +#ifndef NAV_MOVE_BASE_ACTION_H_ +#define NAV_MOVE_BASE_ACTION_H_ + +#include +#include +#include +#include + + +// interfaces headers +#include +#include +#include +#include + +// boost headers +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace move_base +{ + + enum MoveBaseState + { + PLANNING, + CONTROLLING, + CLEARING + }; + + enum RecoveryTrigger + { + PLANNING_R, + CONTROLLING_R, + OSCILLATION_R + }; + + struct MoveBaseConfig + { + std::string base_global_planner; + }; + + /** + * @class MoveBase + * @brief A class that uses the actionlib::ActionServer interface that moves the robot base to a goal location. + */ + class MoveBase : public move_base_core::BaseNavigation + { + public: + /** + * @brief Constructor for the actions + * @param name The name of the action + * @param tf A reference to a TransformListener + */ + MoveBase(TFListenerPtr tf); + MoveBase(); + + /** + * @brief Initialize the navigation system. + * @param tf Shared pointer to the TF listener or buffer. + */ + virtual void initialize(TFListenerPtr tf) override; + + /** + * @brief Set the robot's footprint (outline shape) in the global frame. + * This can be used for planning or collision checking. + * + * @param fprt A vector of points representing the robot's footprint polygon. + * The points should be ordered counter-clockwise. + */ + virtual void setRobotFootprint(const std::vector &fprt) override; + + /** + * @brief Send a goal for the robot to navigate to. + * @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 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. + * @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 std::string &maker, + 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 goal Target pose. + * @param xy_goal_tolerance Acceptable positional error (meters). + * @return True if command issued successfully. + */ + virtual bool moveStraightTo(const geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0) override; + + /** + * @brief Rotate in place to align with target orientation. + * @param goal Pose containing desired heading (only Z-axis used). + * @param yaw_goal_tolerance Acceptable angular error (radians). + * @return True if rotation command was sent successfully. + */ + virtual bool rotateTo(const geometry_msgs::PoseStamped &goal, + double yaw_goal_tolerance = 0.0) override; + + /** + * @brief Pause the robot's movement (e.g. during obstacle avoidance). + */ + virtual void pause() override; + + /** + * @brief Resume motion after a pause. + */ + virtual void resume() override; + + /** + * @brief Cancel the current goal and stop the robot. + */ + virtual void cancel() override; + + /** + * @brief Send direct linear velocity command. + * @param linear Linear velocity vector (x, y, z). + * @return True if the command was accepted. + */ + virtual bool setTwistLinear(const geometry_msgs::Vector3 &linear) override; + + /** + * @brief Send limited angular velocity command. + * @param angular Angular velocity vector (x, y, z). + * @return True if the command was accepted. + */ + virtual bool setTwistAngular(const geometry_msgs::Vector3 &angular) override; + + /** + * @brief Get the robot’s pose as a PoseStamped. + * @param pose Output parameter with the robot’s current pose. + * @return True if pose was successfully retrieved. + */ + virtual bool getRobotPose(geometry_msgs::PoseStamped &pose) override; + + /** + * @brief Get the robot’s pose as a 2D pose. + * @param pose Output parameter with the robot’s current 2D pose. + * @return True if pose was successfully retrieved. + */ + virtual bool getRobotPose(geometry_msgs::Pose2D &pose) override; + + /** + * @brief Destructor - Cleans up + */ + virtual ~MoveBase(); + + /** + * @brief Performs a control cycle + * @param goal A reference to the goal to pursue + * @return True if processing of the goal is done, false otherwise + */ + bool executeCycle(geometry_msgs::PoseStamped &goal); + + /** + * @brief Create a new MoveBase + * @return A shared pointer to the new MoveBase + */ + static move_base_core::BaseNavigation::Ptr create(); + + private: + + /** + * @brief Set velocity for yaw goal tolerance of the robot + * @param yaw_goal_tolerance the value command + */ + void setYawGoalTolerance(double yaw_goal_tolerance); + + /** + * @brief Set velocity for yaw goal tolerance of the robot + * @param xy_goal_tolerance the value command + */ + void setXyGoalTolerance(double xy_goal_tolerance); + + /** + * @brief Swaps the current global planner with a new one specified by name + * @param base_global_planner The name of the new global planner to use + */ + 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 + * @param plan Will be filled in with the plan made by the planner + * @return True if planning succeeds, false otherwise + */ + bool makePlan(const geometry_msgs::PoseStamped &goal, std::vector &plan); + + /** + * @brief Load the recovery behaviors for the navigation stack from the parameter server + * @return True if the recovery behaviors were loaded successfully, false otherwise + */ + bool loadRecoveryBehaviors(const robot::NodeHandle &node); + + /** + * @brief Loads the default recovery behaviors for the navigation stack + */ + void loadDefaultRecoveryBehaviors(); + + /** + * @brief Clears obstacles within a window around the robot + * @param size_x The x size of the window + * @param size_y The y size of the window + */ + void clearCostmapWindows(double size_x, double size_y); + + /** + * @brief Publishes a velocity command of zero to the base + */ + void publishZeroVelocity(); + + /** + * @brief Reset the state of the move_base action and send a zero velocity command to the base + */ + void resetState(); + + // void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal); + + void planThread(); + + // void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal); + + bool isQuaternionValid(const geometry_msgs::Quaternion &q); + + bool getRobotPose(geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap); + + double distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2); + + geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg); + + // /** + // * @brief This is used to wake the planner at periodic intervals. + // */ + // void wakePlanner(const robot::TimerEvent &event); + + private: + bool initialized_; + TFListenerPtr tf_; + robot::NodeHandle private_nh_; + + boost::function planner_loader_; + boost::function controller_loader_; + boost::function recovery_loader_; + + nav_core::BaseLocalPlanner::Ptr tc_; + nav_core::BaseGlobalPlanner::Ptr planner_; + std::vector recovery_behaviors_; + + costmap_2d::Costmap2DROBOT *controller_costmap_robot_; + costmap_2d::Costmap2DROBOT *planner_costmap_robot_; + std::string robot_base_frame_, global_frame_; + std::vector recovery_behavior_names_; + unsigned int recovery_index_; + + geometry_msgs::PoseStamped global_pose_; + double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_; + double planner_patience_, controller_patience_; + int32_t max_planning_retries_; + uint32_t planning_retries_; + double conservative_reset_dist_, clearing_radius_; + // ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_; + // ros::Subscriber goal_sub_; + // ros::ServiceServer make_plan_srv_, clear_costmaps_srv_; + bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_; + bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_; + double oscillation_timeout_, oscillation_distance_; + + MoveBaseState state_; + RecoveryTrigger recovery_trigger_; + + robot::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_; + geometry_msgs::PoseStamped oscillation_pose_; + + // set up plan triple buffer + std::vector *planner_plan_; + std::vector *latest_plan_; + std::vector *controller_plan_; + + // set up the planner's thread + bool runPlanner_; + boost::recursive_mutex planner_mutex_; + boost::condition_variable_any planner_cond_; + geometry_msgs::PoseStamped planner_goal_; + boost::thread *planner_thread_; + + // boost::recursive_mutex configuration_mutex_; + // dynamic_reconfigure::Server *dsrv_; + // void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level); + move_base::MoveBaseConfig last_config_; + move_base::MoveBaseConfig default_config_; + + bool setup_, p_freq_change_, c_freq_change_; + bool new_global_plan_; + bool cancel_ctr_; + bool pause_ctr_, paused_; + double min_approach_linear_velocity_{0.1}; + geometry_msgs::Vector3 old_linear_fwd_, old_linear_bwd_; + geometry_msgs::Vector3 old_angular_fwd_, old_angular_rev_; + double original_xy_goal_tolerance_, original_yaw_goal_tolerance_; + + // defined planner name + std::string position_planner_name_{"MKTLocalPlanner"}; + std::string docking_planner_name_{"NAVDockingLocalPlanner"}; + std::string go_straight_planner_name_{"NAVGoStraightLocalPlanner"}; + std::string rotate_planner_name_{"NAVRotateLocalPlanner"}; + }; + +} // namespace move_base + +#endif // NAV_MOVE_BASE_ACTION_H_ \ No newline at end of file diff --git a/src/Navigations/Packages/move_base/package.xml b/src/Navigations/Packages/move_base/package.xml new file mode 100644 index 0000000..f071fdd --- /dev/null +++ b/src/Navigations/Packages/move_base/package.xml @@ -0,0 +1,72 @@ + + + move_base + 1.0.0 + + The move_base package provides an implementation of an action (see the actionlib package) + that, given a goal in the world, will attempt to achieve it with a mobile base. + The move_base node links together a global and local planner to accomplish its global + navigation task. It supports any global planner adhering to the nav_core::BaseGlobalPlanner + interface and any local planner adhering to the nav_core::BaseLocalPlanner interface. + + Kevin Lee + Kevin Lee + BSD + + http://wiki.ros.org/move_base + + + catkin + + + roscpp + rospy + std_msgs + geometry_msgs + nav_msgs + tf2 + tf2_ros + actionlib + dynamic_reconfigure + message_generation + yaml-cpp + boost + + + move_base_core + nav_core + costmap_2d + xmlrpcpp + node_handle + tf3_sensor_msgs + tf3_geometry_msgs + + + roscpp + rospy + std_msgs + geometry_msgs + nav_msgs + tf2 + tf2_ros + actionlib + dynamic_reconfigure + message_runtime + yaml-cpp + boost + + + move_base_core + nav_core + costmap_2d + xmlrpcpp + node_handle + tf3_sensor_msgs + tf3_geometry_msgs + + + + + + + diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp new file mode 100644 index 0000000..c12c68b --- /dev/null +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -0,0 +1,1233 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Author: Kevin Lee + *********************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +move_base::MoveBase::MoveBase() +: initialized_(false), + planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), + planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), + runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), + pause_ctr_(false), paused_(false) +{ +} + +move_base::MoveBase::MoveBase(TFListenerPtr tf) +: initialized_(false), + planner_costmap_robot_(NULL), controller_costmap_robot_(NULL), + planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), + runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), + pause_ctr_(false), paused_(false) +{ + initialize(tf); +} + +move_base::MoveBase::~MoveBase() +{ + recovery_behaviors_.clear(); + + if (planner_costmap_robot_ != NULL) + delete planner_costmap_robot_; + + if (controller_costmap_robot_ != NULL) + delete controller_costmap_robot_; + + planner_thread_->interrupt(); + planner_thread_->join(); + + delete planner_thread_; + delete planner_plan_; + delete latest_plan_; + delete controller_plan_; + planner_.reset(); + tc_.reset(); + initialized_ = false; + tf_.reset(); + planner_plan_ = NULL; + latest_plan_ = NULL; + controller_plan_ = NULL; + planner_thread_ = NULL; + planner_costmap_robot_ = NULL; + controller_costmap_robot_ = NULL; +} + +void move_base::MoveBase::initialize(TFListenerPtr tf) +{ + printf("[%s:%d] ========== Begin: initialize() ==========\n", __FILE__, __LINE__); + if (!initialized_) + { + tf_ = tf; + private_nh_ = robot::NodeHandle("~"); + + recovery_trigger_ = PLANNING_R; + + // get some parameters that will be global to the move base node + printf("[%s:%d] Getting planner parameters\n", __FILE__, __LINE__); + std::string global_planner; + private_nh_.getParam("base_global_planner", global_planner, std::string("TwoPointsPlanner")); + std::string local_planner; + private_nh_.getParam("base_local_planner", local_planner, std::string("")); + printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str()); + 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; + printf("[%s:%d] robot_base_frame: %s, global_frame: %s\n", __FILE__, __LINE__, robot_base_frame_.c_str(), global_frame_.c_str()); + 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; + + // 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(); + + // 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); + + // 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"); + } + + // 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); + + 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 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(); + } + + // 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(); + } + + // 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__); + } + + initialized_ = true; + printf("[%s:%d] ========== End: initialize() - SUCCESS ==========\n", __FILE__, __LINE__); + } + else + { + robot::printf_yellow("[%s:%d] Already initialized, skipping\n", __FILE__, __LINE__); + } +} + +void move_base::MoveBase::swapPlanner(std::string base_global_planner) +{ +} + +void move_base::MoveBase::setRobotFootprint(const std::vector &fprt) +{ + if (planner_costmap_robot_) + planner_costmap_robot_->setUnpaddedRobotFootprint(fprt); + + if (controller_costmap_robot_) + controller_costmap_robot_->setUnpaddedRobotFootprint(fprt); +} + +bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance) +{ + if (setup_) + { + swapPlanner(default_config_.base_global_planner); + } + else + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("The system has not been installed yet"); + nav_feedback_->goal_checked = false; + } + return false; + } + + if (fabs(xy_goal_tolerance) > 0.001) + this->setXyGoalTolerance(fabs(xy_goal_tolerance)); + else + this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_)); + + if (fabs(yaw_goal_tolerance) > 0.001) + this->setYawGoalTolerance(fabs(yaw_goal_tolerance)); + else + this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_)); + + if (!tc_) + { + throw std::runtime_error("Null 'tc_' pointer encountered"); + } + if (!controller_costmap_robot_) + { + throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + } + boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); + + try + { + tc_->swapPlanner(position_planner_name_); + } + catch (const std::exception &e) + { + std::cerr << e.what() << "\n"; + throw std::exception(e); + } + + if (nav_feedback_) + { + nav_feedback_->navigation_state = move_base_core::State::PLANNING; + nav_feedback_->feed_back_str = std::string("Planning"); + nav_feedback_->goal_checked = false; + } + + if (cancel_ctr_) + cancel_ctr_ = false; + // move_base_msgs::MoveBaseActionGoal action_goal; + // action_goal.header.stamp = ros::Time::now(); + // action_goal.goal.target_pose = goal; + // action_goal_pub_.publish(action_goal); + + lock.unlock(); + return true; +} + +bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance, double yaw_goal_tolerance) +{ + std::string maker_sources; + // private_nh_.param("maker_sources", maker_sources, std::string("")); + std::stringstream ss(maker_sources); + std::string source; + bool has_maker = false; + while (ss >> source) + { + if (maker == source) + { + // private_nh_.setParam("maker_name", maker); + has_maker = true; + } + } + if (!has_maker) + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = move_base_core::State::REJECTED; + std::stringstream fb_ss; + fb_ss << "The system has not been '" << maker << "'"; + nav_feedback_->feed_back_str = fb_ss.str(); + nav_feedback_->goal_checked = false; + } + return false; + } + + if (setup_) + { + swapPlanner(default_config_.base_global_planner); + } + else + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("The system has not been installed yet"); + nav_feedback_->goal_checked = false; + } + return false; + } + + if (fabs(xy_goal_tolerance) > 0.001) + this->setXyGoalTolerance(fabs(xy_goal_tolerance)); + else + this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_)); + + if (fabs(yaw_goal_tolerance) > 0.001) + this->setYawGoalTolerance(fabs(yaw_goal_tolerance)); + else + this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_)); + + if (!tc_) + { + throw std::runtime_error("Null 'tc_' pointer encountered"); + } + if (!controller_costmap_robot_) + { + throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + } + boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); + try + { + tc_->swapPlanner(docking_planner_name_); + } + catch (const std::exception &e) + { + std::cerr << e.what() << "\n"; + throw std::exception(e); + } + + if (nav_feedback_) + { + nav_feedback_->navigation_state = move_base_core::State::PLANNING; + nav_feedback_->feed_back_str = std::string("Planning"); + nav_feedback_->goal_checked = false; + } + if (cancel_ctr_) + cancel_ctr_ = false; + + // move_base_msgs::MoveBaseActionGoal action_goal; + // action_goal.header.stamp = ros::Time::now(); + // action_goal.goal.target_pose = goal; + // action_goal_pub_.publish(action_goal); + lock.unlock(); + return true; +} + +bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal, double xy_goal_tolerance) +{ + if (setup_) + { + std::string global_planner = "TwoPointsPlanner"; + swapPlanner(global_planner); + } + else + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("The system has not been installed yet"); + nav_feedback_->goal_checked = false; + } + return false; + } + + if (fabs(xy_goal_tolerance) > 0.001) + this->setXyGoalTolerance(fabs(xy_goal_tolerance)); + else + this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_)); + + if (!tc_) + { + throw std::runtime_error("Null 'tc_' pointer encountered"); + } + if (!controller_costmap_robot_) + { + throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + } + + boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); + tc_->swapPlanner(go_straight_planner_name_); + + if (nav_feedback_) + { + nav_feedback_->navigation_state = move_base_core::State::PLANNING; + nav_feedback_->feed_back_str = std::string("Planning"); + nav_feedback_->goal_checked = false; + } + if (cancel_ctr_) + cancel_ctr_ = false; + // move_base_msgs::MoveBaseActionGoal action_goal; + // action_goal.header.stamp = ros::Time::now(); + // action_goal.goal.target_pose = goal; + // action_goal_pub_.publish(action_goal); + + lock.unlock(); + return true; +} + +bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, double yaw_goal_tolerance) +{ + if (setup_) + { + std::string global_planner = "TwoPointsPlanner"; + swapPlanner(global_planner); + } + else + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("The system has not been installed yet"); + nav_feedback_->goal_checked = false; + } + return false; + } + + this->setXyGoalTolerance(0.25); + + if (fabs(yaw_goal_tolerance) > 0.001) + this->setYawGoalTolerance(fabs(yaw_goal_tolerance)); + else + this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_)); + + if (!tc_) + { + throw std::runtime_error("Null 'tc_' pointer encountered"); + } + if (!controller_costmap_robot_) + { + throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + } + boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); + tc_->swapPlanner(rotate_planner_name_); + + if (nav_feedback_) + { + nav_feedback_->navigation_state = move_base_core::State::PLANNING; + nav_feedback_->feed_back_str = std::string("Planning"); + nav_feedback_->goal_checked = false; + } + if (cancel_ctr_) + cancel_ctr_ = false; + geometry_msgs::PoseStamped pose; + if (!this->getRobotPose(pose)) + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("Coudn't get robot pose"); + nav_feedback_->goal_checked = false; + } + return false; + } + + // move_base_msgs::MoveBaseActionGoal action_goal; + // action_goal.header.stamp = ros::Time::now(); + // action_goal.goal.target_pose = goal; + // action_goal.goal.target_pose.pose.position.x = pose.pose.position.x + 0.05 * cos(tf2::getYaw(pose.pose.orientation)); + // action_goal.goal.target_pose.pose.position.y = pose.pose.position.y + 0.05 * sin(tf2::getYaw(pose.pose.orientation)); + // action_goal_pub_.publish(action_goal); + + lock.unlock(); + return true; +} + +void move_base::MoveBase::pause() +{ + if (!controller_costmap_robot_) + { + throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + } + boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); + if (!tc_) + { + throw std::runtime_error("Null 'tc_' pointer encountered"); + } + + 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'; + } + pause_ctr_ = true; + paused_ = false; +} + +void move_base::MoveBase::resume() +{ + if (!controller_costmap_robot_) + { + throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + } + boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); + if (!tc_) + { + throw std::runtime_error("Null 'tc_' pointer encountered"); + } + tc_->setTwistLinear(old_linear_fwd_); + tc_->setTwistLinear(old_linear_bwd_); + + tc_->setTwistAngular(old_angular_fwd_); + tc_->setTwistAngular(old_angular_rev_); + + pause_ctr_ = false; + paused_ = false; +} + +void move_base::MoveBase::cancel() +{ + if (!controller_costmap_robot_) + { + throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + } + boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); + cancel_ctr_ = true; +} + +bool move_base::MoveBase::getRobotPose(geometry_msgs::PoseStamped &pose) +{ + if (!initialized_) + return false; + this->getRobotPose(pose, planner_costmap_robot_); + return true; +} + +bool move_base::MoveBase::getRobotPose(geometry_msgs::Pose2D &pose) +{ + if (!initialized_) + return false; + geometry_msgs::PoseStamped global_pose; + this->getRobotPose(global_pose, planner_costmap_robot_); + pose.x = global_pose.pose.position.x; + pose.y = global_pose.pose.position.y; + + tf3::Quaternion quaternion = tf3::Quaternion( + global_pose.pose.orientation.x, + global_pose.pose.orientation.y, + global_pose.pose.orientation.z, + global_pose.pose.orientation.w); + + pose.theta = tf3::getYaw(quaternion); + return true; +} + +bool move_base::MoveBase::setTwistLinear(const geometry_msgs::Vector3 &linear) +{ + try + { + if (tc_ && !cancel_ctr_) + { + if (tc_->islock()) + return true; + + return tc_->setTwistLinear(linear); + } + else if (tc_ && cancel_ctr_) + { + tc_->unlock(); + return false; + } + else + { + throw std::runtime_error("Null 'tc_' pointer encountered"); + } + } + catch (const std::exception &e) + { + return false; + } +} + +bool move_base::MoveBase::setTwistAngular(const geometry_msgs::Vector3 &angular) +{ + if (tc_ && !cancel_ctr_) + { + if (tc_->islock()) + return true; + return tc_->setTwistAngular(angular); + } + else if (tc_ && cancel_ctr_) + { + return false; + } + else + { + throw std::runtime_error("Null 'tc_' pointer encountered"); + } + return false; +} + +void move_base::MoveBase::setYawGoalTolerance(double yaw_goal_tolerance) +{ + double yaw = 0.0; + if (private_nh_.getParam("yaw_goal_tolerance", yaw, 0.0)) + { + private_nh_.setParam("yaw_goal_tolerance", yaw_goal_tolerance); + std::cout << private_nh_.getNamespace() << " yaw_goal_tolerance " << yaw_goal_tolerance << std::endl; + } + else + std::cerr << "yaw_goal_tolerance couldn't get namespace is " << private_nh_.getNamespace() << std::endl; +} + +void move_base::MoveBase::setXyGoalTolerance(double xy_goal_tolerance) +{ + double xy = 0.0; + if (private_nh_.getParam("xy_goal_tolerance", xy, 0.0)) + { + private_nh_.setParam("xy_goal_tolerance", xy_goal_tolerance); + std::cout << private_nh_.getNamespace() << " xy_goal_tolerance " << xy_goal_tolerance << std::endl; + } + else + std::cerr << "xy_goal_tolerance couldn't get namespace is " << private_nh_.getNamespace() << std::endl; +} + +// bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp) +// { +// } + +// bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp) +// { + +// } + +bool move_base::MoveBase::makePlan(const geometry_msgs::PoseStamped &goal, std::vector &plan) +{ + // make sure to set the plan to be empty initially + plan.clear(); + + // since this gets called on handle activate + if (!planner_costmap_robot_) + { + std::cerr << "Planner costmap ROS is NULL, unable to create global plan" << std::endl; + return false; + } + + boost::unique_lock lock(*(planner_costmap_robot_->getCostmap()->getMutex())); + + // get the starting pose of the robot + geometry_msgs::PoseStamped global_pose; + if (!getRobotPose(global_pose, planner_costmap_robot_)) + { + std::cerr << "Unable to get starting pose of robot, unable to create global plan" << std::endl; + return false; + } + + const geometry_msgs::PoseStamped &start = global_pose; + + // if the planner fails or returns a zero length plan, planning failed + if (!planner_->makePlan(start, goal, plan) || plan.empty()) + { + std::cout << "Failed to find a plan to point (" << goal.pose.position.x << ", " << goal.pose.position.y << ")" << std::endl; + return false; + } + lock.unlock(); + return true; +} + +bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node) +{ + YAML::Node behavior_list = node.getParamValue("recovery_behaviors"); + if (behavior_list && behavior_list.IsSequence()) + { + // Validate all behaviors first + for (size_t i = 0; i < behavior_list.size(); ++i) + { + YAML::Node behavior = behavior_list[i].as(); + if (!behavior.IsMap()) + { + std::cerr << "Recovery behaviors must be specified as maps. We'll use the default recovery behaviors instead." << std::endl; + return false; + } + + if (!behavior["name"] || !behavior["type"]) + { + std::cerr << "Recovery behaviors must have a name and a type. Using the default recovery behaviors instead." << std::endl; + return false; + } + + // check for recovery behaviors with the same name + std::string name_i = behavior["name"].as(); + for (size_t j = i + 1; j < behavior_list.size(); ++j) + { + YAML::Node behavior_j = behavior_list[j].as(); + if (behavior_j.IsMap() && behavior_j["name"]) + { + std::string name_j = behavior_j["name"].as(); + if (name_i == name_j) + { + std::cerr << "A recovery behavior with the name " << name_i << " already exists, this is not allowed. Using the default recovery behaviors instead." << std::endl; + return false; + } + } + } + } + + // if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors + for (size_t i = 0; i < behavior_list.size(); ++i) + { + try + { + YAML::Node behavior = behavior_list[i].as(); + // Load recovery behavior using boost::dll::import_alias + // behavior["type"] should be the factory function name (e.g., "create_plugin") + std::string behavior_type = behavior["type"].as(); + std::string behavior_name = behavior["name"].as(); + std::string path_file_so; + // Load the factory function from the shared library + auto recovery_loader = boost::dll::import_alias( + path_file_so, behavior_type, boost::dll::load_mode::append_decorations); + + // Create instance using the factory function + std::shared_ptr behavior_ptr(recovery_loader()); + + // shouldn't be possible, but it won't hurt to check + if (behavior_ptr == nullptr) + { + std::cerr << "The factory function returned a null pointer without throwing an exception. This should not happen" << std::endl; + return false; + } + // initialize the recovery behavior with its name + behavior_ptr->initialize(behavior_name, tf_.get(), planner_costmap_robot_, controller_costmap_robot_); + recovery_behavior_names_.push_back(behavior_name); + recovery_behaviors_.push_back(behavior_ptr); + } + catch (const std::exception &ex) + { + std::cerr << "Failed to load recovery behavior plugin: " << behavior_list[i]["type"].as() + << ". Error: " << ex.what() << std::endl; + return false; + } + } + } + else if (behavior_list) + { + std::cerr << "The recovery behavior specification must be a list. We'll use the default recovery behaviors instead." << std::endl; + return false; + } + else + { + // if no recovery_behaviors are specified, we'll just load the defaults + return false; + } + + // if we've made it here... we've constructed a recovery behavior list successfully + return true; +} + +void move_base::MoveBase::loadDefaultRecoveryBehaviors() +{ + recovery_behaviors_.clear(); + recovery_behavior_names_.clear(); + + try + { + // Note: Parameters should be set via YAML config file, not programmatically + // The recovery behaviors will read their parameters from the YAML node during initialization + + // first, we'll load a recovery behavior to clear the costmap (conservative reset) + try + { + std::string path_file_so; + auto clear_costmap_loader = boost::dll::import_alias( + path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations); + std::shared_ptr cons_clear(clear_costmap_loader()); + cons_clear->initialize("conservative_reset", tf_.get(), planner_costmap_robot_, controller_costmap_robot_); + recovery_behavior_names_.push_back("conservative_reset"); + recovery_behaviors_.push_back(cons_clear); + } + catch (const std::exception &ex) + { + std::cerr << "Failed to load clear_costmap_recovery plugin: " << ex.what() << std::endl; + } + + // next, we'll load a recovery behavior to rotate in place + std::shared_ptr rotate; + if (clearing_rotation_allowed_) + { + try + { + std::string path_file_so; + auto rotate_loader = boost::dll::import_alias( + path_file_so, "rotate_recovery", boost::dll::load_mode::append_decorations); + rotate = std::shared_ptr(rotate_loader()); + rotate->initialize("rotate_recovery", tf_.get(), planner_costmap_robot_, controller_costmap_robot_); + recovery_behavior_names_.push_back("rotate_recovery"); + recovery_behaviors_.push_back(rotate); + } + catch (const std::exception &ex) + { + std::cerr << "Failed to load rotate_recovery plugin: " << ex.what() << std::endl; + } + } + + // next, we'll load a recovery behavior that will do an aggressive reset of the costmap + try + { + std::string path_file_so; + auto clear_costmap_loader = boost::dll::import_alias( + path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations); + std::shared_ptr ags_clear(clear_costmap_loader()); + ags_clear->initialize("aggressive_reset", tf_.get(), planner_costmap_robot_, controller_costmap_robot_); + recovery_behavior_names_.push_back("aggressive_reset"); + recovery_behaviors_.push_back(ags_clear); + } + catch (const std::exception &ex) + { + std::cerr << "Failed to load clear_costmap_recovery plugin for aggressive reset: " << ex.what() << std::endl; + } + + // we'll rotate in-place one more time + if (clearing_rotation_allowed_ && rotate) + { + recovery_behaviors_.push_back(rotate); + recovery_behavior_names_.push_back("rotate_recovery"); + } + } + catch (const std::exception &ex) + { + std::cerr << "Failed to load default recovery behaviors. Error: " << ex.what() << std::endl; + } + + return; +} + +void move_base::MoveBase::clearCostmapWindows(double size_x, double size_y) +{ + geometry_msgs::PoseStamped global_pose; + + // clear the planner's costmap + getRobotPose(global_pose, planner_costmap_robot_); + + std::vector clear_poly; + double x = global_pose.pose.position.x; + double y = global_pose.pose.position.y; + geometry_msgs::Point pt; + + pt.x = x - size_x / 2; + pt.y = y - size_y / 2; + clear_poly.push_back(pt); + + pt.x = x + size_x / 2; + pt.y = y - size_y / 2; + clear_poly.push_back(pt); + + pt.x = x + size_x / 2; + pt.y = y + size_y / 2; + clear_poly.push_back(pt); + + pt.x = x - size_x / 2; + pt.y = y + size_y / 2; + clear_poly.push_back(pt); + + planner_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE); + + // clear the controller's costmap + getRobotPose(global_pose, controller_costmap_robot_); + + clear_poly.clear(); + x = global_pose.pose.position.x; + y = global_pose.pose.position.y; + + pt.x = x - size_x / 2; + pt.y = y - size_y / 2; + clear_poly.push_back(pt); + + pt.x = x + size_x / 2; + pt.y = y - size_y / 2; + clear_poly.push_back(pt); + + pt.x = x + size_x / 2; + pt.y = y + size_y / 2; + clear_poly.push_back(pt); + + pt.x = x - size_x / 2; + pt.y = y + size_y / 2; + clear_poly.push_back(pt); + + controller_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE); +} + +void move_base::MoveBase::publishZeroVelocity() +{ +} + +void move_base::MoveBase::resetState() +{ + // Disable the planner thread + boost::unique_lock lock(planner_mutex_); + runPlanner_ = false; + lock.unlock(); + + // Reset statemachine + state_ = move_base::PLANNING; + + recovery_index_ = 0; + recovery_trigger_ = PLANNING_R; + paused_ = false; + publishZeroVelocity(); + + // if we shutdown our costmaps when we're deactivated... we'll do that now + if (shutdown_costmaps_) + { + std::cout << "Stopping costmaps" << std::endl; + planner_costmap_robot_->stop(); + controller_costmap_robot_->stop(); + } +} + +// void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal); + +void move_base::MoveBase::planThread() +{ + // std::cout << "Starting planner thread..." << std::endl; + // YAML::Node n("~"); + // robot::Timer timer; + // bool wait_for_wake = false; + // boost::unique_lock lock(planner_mutex_); + // while (true) + // { + // // check if we should run the planner (the mutex is locked) + // while (wait_for_wake || !runPlanner_) + // { + // // if we should not be running the planner then suspend this thread + // std::cout << "Planner thread is suspending" << std::endl; + // planner_cond_.wait(lock); + // wait_for_wake = false; + // } + // robot::Time start_time = robot::Time::now(); + + // // time to plan! get a copy of the goal and unlock the mutex + // geometry_msgs::PoseStamped temp_goal = planner_goal_; + // lock.unlock(); + // std::cout << "Planning..." << std::endl; + // // run planner + // planner_plan_->clear(); + // // ROS_INFO("MakePlan goal %f %f", temp_goal.pose.position.x, temp_goal.pose.position.y); + // bool gotPlan = makePlan(temp_goal, *planner_plan_); + + // if (gotPlan) + // { + // std::cout << "Got Plan with " << planner_plan_->size() << " points!" << std::endl; + // // pointer swap the plans under mutex (the controller will pull from latest_plan_) + // std::vector *temp_plan = planner_plan_; + + // lock.lock(); + // planner_plan_ = latest_plan_; + // latest_plan_ = temp_plan; + // last_valid_plan_ = robot::Time::now(); + // planning_retries_ = 0; + // new_global_plan_ = true; + + // std::cout << "Generated a plan from the base_global_planner" << std::endl; + + // // make sure we only start the controller if we still haven't reached the goal + // if (runPlanner_) + // { + // state_ = move_base::CONTROLLING; + // if (nav_feedback_) + // nav_feedback_->navigation_state = move_base_core::State::CONTROLLING; + // } + // if (planner_frequency_ <= 0) + // runPlanner_ = false; + // lock.unlock(); + // } + // // if we didn't get a plan and we are in the planning state (the robot isn't moving) + // else if (state_ == move_base::PLANNING) + // { + // std::cout << "No Plan..." << std::endl; + // robot::Time attempt_end = last_valid_plan_ + robot::Duration(planner_patience_); + + // // check if we've tried to make a plan for over our time limit or our maximum number of retries + // // issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_ + // // is negative (the default), it is just ignored and we have the same behavior as ever + // lock.lock(); + // planning_retries_++; + // if (runPlanner_ && + // (robot::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))) + // { + // // we'll move into our obstacle clearing mode + // state_ = move_base::CLEARING; + // if (nav_feedback_) + // nav_feedback_->navigation_state = move_base_core::State::CLEARING; + + // runPlanner_ = false; // proper solution for issue #523 + // publishZeroVelocity(); + // recovery_trigger_ = PLANNING_R; + // } + + // lock.unlock(); + // } + + // // take the mutex for the next iteration + // lock.lock(); + + // // setup sleep interface if needed + // if (planner_frequency_ > 0) + // { + // robot::Duration sleep_time = (start_time + robot::Duration(1.0 / planner_frequency_)) - robot::Time::now(); + // if (sleep_time > robot::Duration(0.0)) + // { + // wait_for_wake = true; + // timer = robot::Timer(sleep_time, &move_base::MoveBase::wakePlanner, this); + // } + // } + // } +} + +// void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal); + +bool move_base::MoveBase::isQuaternionValid(const geometry_msgs::Quaternion &q) +{ + // first we need to check if the quaternion has nan's or infs + if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)) + { + std::cerr << "Quaternion has nans or infs... discarding as a navigation goal" << std::endl; + return false; + } + + tf3::Quaternion tf_q(q.x, q.y, q.z, q.w); + + // next, we need to check if the length of the quaternion is close to zero + if (tf_q.length2() < 1e-6) + { + std::cerr << "Quaternion has length close to zero... discarding as navigation goal" << std::endl; + return false; + } + + // next, we'll normalize the quaternion and check that it transforms the vertical vector correctly + tf_q.normalize(); + + tf3::Vector3 up(0, 0, 1); + + double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle())); + + if (fabs(dot - 1) > 1e-3) + { + std::cerr << "Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical." << std::endl; + return false; + } + + return true; +} + +bool move_base::MoveBase::getRobotPose(geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap) +{ + + tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose); + geometry_msgs::PoseStamped robot_pose; + tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose); + robot_pose.header.frame_id = robot_base_frame_; + robot_pose.header.stamp = robot::Time(); // latest available + robot::Time current_time = robot::Time::now(); // save time for checking tf delay later + + // get robot pose on the given costmap frame + try + { + tf3::TransformStampedMsg transform = tf_->lookupTransform(costmap->getGlobalFrameID(), robot_base_frame_, data_convert::convertTime(current_time)); + tf3::doTransform(robot_pose, global_pose, transform); + } + catch (tf3::LookupException &ex) + { + std::cerr << "Move Base No Transform available Error looking up robot pose: " << ex.what() << std::endl; + return false; + } + catch (tf3::ConnectivityException &ex) + { + std::cerr << "Connectivity Error looking up robot pose: " << ex.what() << std::endl; + return false; + } + catch (tf3::ExtrapolationException &ex) + { + std::cerr << "Extrapolation Error looking up robot pose: " << ex.what() << std::endl; + return false; + } + + // check if global_pose time stamp is within costmap transform tolerance + if (!global_pose.header.stamp.isZero() && + current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance()) + { + std::cerr << "Transform timeout for " << costmap->getName().c_str() << ". " + "Current time: " << current_time.toSec() << ", pose stamp: " << global_pose.header.stamp.toSec() << ", tolerance: " << costmap->getTransformTolerance() << std::endl; + return false; + } + + return true; +} + +double move_base::MoveBase::distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2) +{ + return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y); +} + +geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg) +{ + std::string global_frame = planner_costmap_robot_->getGlobalFrameID(); + geometry_msgs::PoseStamped goal_pose, global_pose; + goal_pose = goal_pose_msg; + + // just get the latest available transform... for accuracy they should send + // goals in the frame of the planner + goal_pose.header.stamp = robot::Time(); // latest available + + try + { + tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, data_convert::convertTime(goal_pose.header.stamp)); + tf3::doTransform(goal_pose, global_pose, transform); + } + catch (tf3::LookupException &ex) + { + std::cerr << "Move Base No Transform available Error looking up robot pose: " << ex.what() << std::endl; + return goal_pose_msg; + } + catch (tf3::ConnectivityException &ex) + { + std::cerr << "Connectivity Error looking up robot pose: " << ex.what() << std::endl; + return goal_pose_msg; + } + catch (tf3::ExtrapolationException &ex) + { + std::cerr << "Extrapolation Error looking up robot pose: " << ex.what() << std::endl; + return goal_pose_msg; + } + + return global_pose; +} + +// void wakePlanner(const robot::TimerEvent &event) +// { +// } + +move_base_core::BaseNavigation::Ptr move_base::MoveBase::create() +{ + return std::make_shared(); +} + +BOOST_DLL_ALIAS(move_base::MoveBase::create, MoveBase) \ No newline at end of file diff --git a/src/Navigations/Packages/move_base/src/move_base_main.cpp b/src/Navigations/Packages/move_base/src/move_base_main.cpp new file mode 100644 index 0000000..a269f2e --- /dev/null +++ b/src/Navigations/Packages/move_base/src/move_base_main.cpp @@ -0,0 +1,40 @@ +/* + * Copyright (c) 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +int main(int argc, char** argv) +{ + std::shared_ptr tf = std::make_shared(); + move_base::MoveBase move_base; + move_base.initialize(tf); + std::cout << "Move Base Main Function :" << robot::Time::now().toSec() << std::endl; + return(0); +} \ No newline at end of file