diff --git a/.gitignore b/.gitignore index cfac8dd..0e1d37f 100644 --- a/.gitignore +++ b/.gitignore @@ -421,3 +421,6 @@ FodyWeavers.xsd build install devel +obstacle +/pnkx_nav_core/config/hybrid_local_planner_params.yaml + diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a0f509..8d5303f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,6 +134,22 @@ if (NOT TARGET pnkx_local_planner) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner) endif() +if (NOT TARGET robot_angles) + add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/angles) +endif() + +if (NOT TARGET grid_map_core) + add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/grid_map_core) +endif() + +if (NOT TARGET robot_base_local_planner) + add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/base_local_planner) +endif() + +if (NOT TARGET hybrid_local_planner) + add_subdirectory(${CMAKE_SOURCE_DIR}/obstacle/hybrid_local_planner) +endif() + if (NOT TARGET robot_actionlib_msgs) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_actionlib_msgs) endif() diff --git a/config/config/obstacle_layer_params.yaml b/config/config/obstacle_layer_params.yaml index 8fa749a..af1fab9 100644 --- a/config/config/obstacle_layer_params.yaml +++ b/config/config/obstacle_layer_params.yaml @@ -1,4 +1,5 @@ obstacle_layer: + enabled: true track_unknown_space: true transform_tolerance: 0.2 topic: "map" diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index 429860c..39116cf 100755 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -1,4 +1,5 @@ -position_planner_name: PNKXLocalPlanner +# position_planner_name: PNKXLocalPlanner +position_planner_name: HybridLocalPlanner docking_planner_name: PNKXDockingLocalPlanner go_straight_planner_name: PNKXGoStraightLocalPlanner rotate_planner_name: PNKXRotateLocalPlanner @@ -26,23 +27,23 @@ virtual_walls_map: lethal_cost_threshold: 100 obstacles: - observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing - f_scan_marking: - topic: /f_scan - data_type: LaserScan - clearing: false - marking: true - inf_is_valid: false - min_obstacle_height: 0.0 - max_obstacle_height: 0.25 - f_scan_clearing: - topic: /f_scan - data_type: LaserScan - clearing: true - marking: false - inf_is_valid: false - min_obstacle_height: 0.0 - max_obstacle_height: 0.25 + observation_sources: b_scan_marking b_scan_clearing #f_scan_marking f_scan_clearing + # f_scan_marking: + # topic: /f_scan + # data_type: LaserScan + # clearing: false + # marking: true + # inf_is_valid: false + # min_obstacle_height: 0.0 + # max_obstacle_height: 0.25 + # f_scan_clearing: + # topic: /f_scan + # data_type: LaserScan + # clearing: true + # marking: false + # inf_is_valid: false + # min_obstacle_height: 0.0 + # max_obstacle_height: 0.25 b_scan_marking: topic: /b_scan data_type: LaserScan diff --git a/config/hybrid_local_planner_params.yaml b/config/hybrid_local_planner_params.yaml new file mode 100644 index 0000000..408042f --- /dev/null +++ b/config/hybrid_local_planner_params.yaml @@ -0,0 +1,54 @@ +LocalPlannerAdapter: + library_path: liblocal_planner_adapter + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.03 + min_approach_linear_velocity: 0.06 + +HybridLocalPlanner: +# base_local_planner: "hybrid_local_planner/HybridLocalPlanner" +# HybridLocalPlanner: + library_path: libhybrid_local_planner + odom_topic: odom + # Trajectory + + max_global_plan_lookahead_dist: 4.0 + global_plan_viapoint_sep: 0.5 + global_plan_prune_distance: 0.0 + global_plan_goal_sep: 0.05 + + # Robot + + max_vel_x: 1.0 + max_vel_x_backwards: -0.2 + max_vel_theta: 0.5 + acc_lim_x: 1.0 + acc_lim_theta: 2.0 + turn_around_priority: True + stop_dist: 0.5 + dec_dist: 1.0 + + + # GoalTolerance + + xy_goal_tolerance: 0.05 + yaw_goal_tolerance: 0.05 + + # Optimization + + # PP Parameters + w_vel: 0.9 + w_omega: 1.12 + # DWA Parameters + robot_max_v: 0.4 + robot_max_w: 0.4 + enable_backward_motion: False + w_targetheading: 1.7 + w_velocity: 0.2 + w_clearance: 0.2 + w_pathDistance: 0.05 + time_horizon: 3.0 + velocity_resolution: 0.015 + + segment_transition_threshold: 0.01 # Ngưỡng khoảng cách chuyển segment + calibration_factor: 1.5 # Hệ số hiệu chuẩn + use_obstacle_avoidance: true # Bật tắt tránh vật cản \ No newline at end of file diff --git a/examples/NavigationExample/NavigationAPI.cs b/examples/NavigationExample/NavigationAPI.cs index d7ed979..1becdc2 100644 --- a/examples/NavigationExample/NavigationAPI.cs +++ b/examples/NavigationExample/NavigationAPI.cs @@ -140,6 +140,10 @@ namespace NavigationExample [return: MarshalAs(UnmanagedType.I1)] public static extern bool navigation_move_to_order(NavigationHandle handle, Order order, PoseStamped goal); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.I1)] + public static extern bool navigation_move_to_nodes_edges(NavigationHandle handle, IntPtr nodes, UIntPtr node_count, IntPtr edges, UIntPtr edge_count, PoseStamped goal); + [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)] [return: MarshalAs(UnmanagedType.I1)] public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal); diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index e088a7a..a483249 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -405,8 +405,10 @@ namespace NavigationExample goal.pose.orientation.w = 1.0; - Console.WriteLine("Docking to docking_point"); - NavigationAPI.navigation_dock_to_order(navHandle, order, "charger", goal); + // Console.WriteLine("Docking to docking_point"); + // NavigationAPI.navigation_dock_to_order(navHandle, order, "charger", goal); + + NavigationAPI.navigation_move_to_nodes_edges(navHandle, order.nodes, order.nodes_count, order.edges, order.edges_count, goal); // NavigationAPI.navigation_move_to_order(navHandle, order, goal); NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0); @@ -493,4 +495,3 @@ namespace NavigationExample } } } - diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index 80acdbd..d225bf1 100755 Binary files a/examples/NavigationExample/libnav_c_api.so and b/examples/NavigationExample/libnav_c_api.so differ diff --git a/src/APIs/c_api/include/nav_c_api.h b/src/APIs/c_api/include/nav_c_api.h index 1df99b7..c508ffe 100644 --- a/src/APIs/c_api/include/nav_c_api.h +++ b/src/APIs/c_api/include/nav_c_api.h @@ -141,6 +141,18 @@ extern "C" */ bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal); + /** + * @brief Send a goal for the robot to navigate to + * @param handle Navigation handle + * @param nodes Nodes array + * @param node_count Number of nodes in the array + * @param edges Edges array + * @param edge_count Number of edges in the array + * @param goal Target pose in the global frame + * @return true if goal was accepted and sent successfully + */ + bool navigation_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal); + /** * @brief Send a docking goal to a predefined marker * @param handle Navigation handle diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index fa212cf..a6e7d75 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -171,6 +171,7 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po try { + std::shared_ptr nav_ptr = std::shared_ptr( reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); @@ -181,10 +182,12 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po footprint.reserve(point_count); for (size_t i = 0; i < point_count; ++i) { + robot_geometry_msgs::Point pt; pt.x = points[i].x; pt.y = points[i].y; pt.z = points[i].z; + printf("footprint x: %f, y: %f\n", pt.x, pt.y); footprint.push_back(pt); } nav_ptr->setRobotFootprint(footprint); @@ -284,6 +287,40 @@ extern "C" bool navigation_move_to_order(NavigationHandle handle, const Order or } } +extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal) +{ + if (!handle) + { + return false; + } + + try + { + std::shared_ptr nav_ptr = + std::shared_ptr( + reinterpret_cast(handle), [](::robot::move_base_core::BaseNavigation *) {}); + if (!nav_ptr) + return false; + robot::log_error("navigation_move_to_order goal %f %f", goal.pose.position.x, goal.pose.position.y); + Order order; + order.nodes = const_cast(nodes); + order.nodes_count = node_count; + order.edges = const_cast(edges); + order.edges_count = edge_count; + robot_protocol_msgs::Order cpp_order = convert2CppOrder(order); + robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); + return nav_ptr->moveTo(cpp_order, cpp_goal); + } + catch (const std::exception &e) + { + return false; + } + catch (...) + { + printf("[%s:%d]\n Error: Failed to move to order\n", __FILE__, __LINE__); + return false; + } +} extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal) { if (!handle || !marker) diff --git a/src/Navigations/Packages/move_base/src/convert_data.cpp b/src/Navigations/Packages/move_base/src/convert_data.cpp index c63f778..8e8f12d 100644 --- a/src/Navigations/Packages/move_base/src/convert_data.cpp +++ b/src/Navigations/Packages/move_base/src/convert_data.cpp @@ -1,6 +1,7 @@ #include #include #include +#include char* move_base::ConvertData::cost_translation_table_ = NULL; @@ -41,7 +42,19 @@ move_base::ConvertData::~ConvertData() // prepare grid_ message for publication. void move_base::ConvertData::prepareGrid() { - // boost::unique_lock lock(*(costmap_->getMutex())); + printf("Preparing costmap data for publication\n"); + + std::ofstream file("/home/robotics/sonvh/pnkx_nav_core/obstacle/hybrid_local_planner/cfg/costmap_data.txt"); + + if (!file.is_open()) + { + std::cout << "Cannot open file!" << std::endl; + return; + } + + // boost::unique_lock + // lock(*(costmap_->getCostmap()->getMutex())); + double resolution = costmap_->getCostmap()->getResolution(); grid_.header.frame_id = global_frame_; @@ -53,22 +66,44 @@ void move_base::ConvertData::prepareGrid() double wx, wy; costmap_->getCostmap()->mapToWorld(0, 0, wx, wy); + grid_.info.origin.position.x = wx - resolution / 2; grid_.info.origin.position.y = wy - resolution / 2; grid_.info.origin.position.z = 0.0; grid_.info.origin.orientation.w = 1.0; + saved_origin_x_ = costmap_->getCostmap()->getOriginX(); saved_origin_y_ = costmap_->getCostmap()->getOriginY(); grid_.data.resize(grid_.info.width * grid_.info.height); + file << "frame: " << global_frame_ << std::endl; + file << "time: " << robot::Time::now().toSec() << std::endl; + file << "resolution: " << resolution << std::endl; + file << "width: " << grid_.info.width << std::endl; + file << "height: " << grid_.info.height << std::endl; + file << "origin_x: " << grid_.info.origin.position.x << std::endl; + file << "origin_y: " << grid_.info.origin.position.y << std::endl; + unsigned char* data = costmap_->getCostmap()->getCharMap(); - for (unsigned int i = 0; i < grid_.data.size(); i++) + + for (unsigned int y = 0; y < grid_.info.height; y++) { - grid_.data[i] = cost_translation_table_[ data[ i ]]; + for (unsigned int x = 0; x < grid_.info.width; x++) + { + unsigned int i = y * grid_.info.width + x; + + grid_.data[i] = cost_translation_table_[data[i]]; + + file << int(grid_.data[i]) << " "; + } + file << "\n"; } + + file.close(); } + void move_base::ConvertData::updateCostmap(robot_nav_msgs::OccupancyGrid &grid, robot_map_msgs::OccupancyGridUpdate &update, bool &is_updated) { boost::unique_lock lock(*(costmap_->getCostmap()->getMutex())); diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 1b4f501..1471ce4 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -760,11 +760,11 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na // robot_pose.header.frame_id = robot_base_frame_; // robot_pose.header.stamp = robot::Time(); // robot::Time current_time = robot::Time::now(); // save time for checking tf delay later - + // // Convert robot::Time to tf3::Time // tf3::Time tf3_current_time = data_convert::convertTime(current_time); // tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time()); - + // std::string error_msg; // if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg)) // {