diff --git a/src/TurtleBot-RRT-Star/CMakeLists.txt b/src/TurtleBot-RRT-Star/CMakeLists.txt new file mode 100755 index 0000000..4700401 --- /dev/null +++ b/src/TurtleBot-RRT-Star/CMakeLists.txt @@ -0,0 +1,95 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_rrtstar_planner) + +# Default to C99 +set(CMAKE_C_STANDARD 99) + + +# Default to C++14 +set(CMAKE_CXX_STANDARD 14) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_core REQUIRED) +find_package(pluginlib REQUIRED) + +include_directories( + include +) + +set(library_name ${PROJECT_NAME}_plugin) + +set(dependencies + rclcpp + rclcpp_action + rclcpp_lifecycle + std_msgs + visualization_msgs + nav2_util + nav2_msgs + nav_msgs + geometry_msgs + builtin_interfaces + tf2_ros + nav2_costmap_2d + nav2_core + pluginlib +) + +add_library(${library_name} SHARED + src/rrtstar_planner.cpp +) + +ament_target_dependencies(${library_name} + ${dependencies} +) + +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + + +pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(FILES global_planner_plugin.xml + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + +install(DIRECTORY scripts/ + DESTINATION share/${PROJECT_NAME}/scripts +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/src/TurtleBot-RRT-Star/global_planner_plugin.xml b/src/TurtleBot-RRT-Star/global_planner_plugin.xml new file mode 100755 index 0000000..aa3959e --- /dev/null +++ b/src/TurtleBot-RRT-Star/global_planner_plugin.xml @@ -0,0 +1,5 @@ + + + This is a plugin for RRT Star path planning. + + diff --git a/src/TurtleBot-RRT-Star/include/nav2_rrtstar_planner/rrtstar_planner.hpp b/src/TurtleBot-RRT-Star/include/nav2_rrtstar_planner/rrtstar_planner.hpp new file mode 100755 index 0000000..ea20f35 --- /dev/null +++ b/src/TurtleBot-RRT-Star/include/nav2_rrtstar_planner/rrtstar_planner.hpp @@ -0,0 +1,60 @@ +#ifndef NAV2_RRTSTAR_PLANNER__RRTSTAR_PLANNER_HPP_ +#define NAV2_RRTSTAR_PLANNER__RRTSTAR_PLANNER_HPP_ + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "tf2_ros/buffer.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" + +namespace nav2_rrtstar_planner { + +struct Vertex { + double x, y, cost; + Vertex* parent; + Vertex(double x_val, double y_val, Vertex* p = nullptr, double travel_distance = 0) : + x(x_val), y(y_val), parent(p), cost(travel_distance) {} +}; + +class RRTStar : public nav2_core::GlobalPlanner { +public: + RRTStar() = default; + ~RRTStar() override = default; + + void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr& parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + void cleanup() override; + void activate() override; + void deactivate() override; + + nav_msgs::msg::Path createPlan(const geometry_msgs::msg::PoseStamped& start, + const geometry_msgs::msg::PoseStamped& goal) override; + +protected: + std::shared_ptr tf_; + nav2_util::LifecycleNode::SharedPtr node_; + nav2_costmap_2d::Costmap2D* costmap_; + std::string global_frame_; + std::string name_; + int max_iterations_; + double interpolation_resolution_; + std::vector> tree_; + double ball_radius_constant_; + + double calculate_distance(double x, double y, const Vertex& vertex); + Vertex* nearest_neighbor(double x, double y); + bool connectible(const Vertex& start, const Vertex& end); + void calculateBallRadiusConstant(); + double calculateBallRadius(int tree_size, int dimensions, double max_connection_distance); + std::vector findVerticesInsideCircle(double center_x, double center_y, double radius); + double calculate_cost_from_start(const Vertex& vertex); +}; + +} // namespace nav2_rrtstar_planner + +#endif // NAV2_RRTSTAR_PLANNER__RRTSTAR_PLANNER_HPP_ \ No newline at end of file diff --git a/src/TurtleBot-RRT-Star/launch/bringup_localization_with_initial_pose.launch.py b/src/TurtleBot-RRT-Star/launch/bringup_localization_with_initial_pose.launch.py new file mode 100644 index 0000000..a684cb0 --- /dev/null +++ b/src/TurtleBot-RRT-Star/launch/bringup_localization_with_initial_pose.launch.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 +""" +Launch localization with automatic AMCL initial pose setting. +No need to manually click 2D Pose Estimate in RViz. +""" + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, TimerAction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +import os + +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + bringup_dir = get_package_share_directory('nav2_bringup') + script_path = os.path.join( + get_package_share_directory('nav2_rrtstar_planner'), + 'scripts', + 'publish_initial_pose.py' + ) + + # Launch arguments + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + map_yaml_file = LaunchConfiguration('map') + + # Include original localization_launch.py + localization_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'localization_launch.py') + ), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'map': map_yaml_file + }.items() + ) + + # Auto-publish initial pose (0, 0) after 3 seconds delay + auto_initial_pose = TimerAction( + period=3.0, + actions=[ + ExecuteProcess( + cmd=['python3', script_path], + output='screen' + ) + ] + ) + + ld = LaunchDescription() + ld.add_action(localization_launch) + ld.add_action(auto_initial_pose) + + return ld diff --git a/src/TurtleBot-RRT-Star/nav2_params.yaml b/src/TurtleBot-RRT-Star/nav2_params.yaml new file mode 100755 index 0000000..8dc3f8c --- /dev/null +++ b/src/TurtleBot-RRT-Star/nav2_params.yaml @@ -0,0 +1,364 @@ +amcl: + ros__parameters: + use_sim_time: True + # 设置初始位姿（x, y, z, yaw） + set_initial_pose: True + initial_pose: + x: 0.0 + y: 0.0 + z: 0.0 + yaw: 0.0 + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 1.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.25 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 0.5 + use_sim_time: True + planner_plugins: ['GridBased'] + # GridBased: + # plugin: 'nav2_navfn_planner/NavfnPlanner' + # tolerance: 0.5 + # use_astar: false + # allow_unknown: true + + planner_plugin_types: ['nav2_rrtstar_planner::RRTStar'] # For Foxy and earlier + planner_plugin_ids: ['GridBased'] # For Foxy and earlier + plugins: ['GridBased'] # For Galactic and later + use_sim_time: True + GridBased: + plugin: nav2_rrtstar_planner/RRTStar # For Galactic and later + interpolation_resolution: 0.01 + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/src/TurtleBot-RRT-Star/package.xml b/src/TurtleBot-RRT-Star/package.xml new file mode 100755 index 0000000..118575b --- /dev/null +++ b/src/TurtleBot-RRT-Star/package.xml @@ -0,0 +1,38 @@ + + + + nav2_rrtstar_planner + 1.0.0 + RRT Star path planner. + echo + BSD-3-Clause + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_lifecycle + std_msgs + visualization_msgs + nav2_util + nav2_msgs + nav_msgs + geometry_msgs + builtin_interfaces + tf2_ros + nav2_costmap_2d + nav2_core + pluginlib + nav2_bringup + random + vector + limits + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/src/TurtleBot-RRT-Star/scripts/publish_initial_pose.py b/src/TurtleBot-RRT-Star/scripts/publish_initial_pose.py new file mode 100755 index 0000000..4bf9f98 --- /dev/null +++ b/src/TurtleBot-RRT-Star/scripts/publish_initial_pose.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +"""Publish initial pose for AMCL localization.""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseWithCovarianceStamped + + +class InitialPosePublisher(Node): + def __init__(self): + super().__init__('initial_pose_publisher') + self.publisher = self.create_publisher( + PoseWithCovarianceStamped, + '/initialpose', + 10 + ) + self.timer = self.create_timer(1.0, self.publish_pose) + self.get_logger().info('Initial pose publisher started') + self.published = False + + def publish_pose(self): + if self.published: + return + + msg = PoseWithCovarianceStamped() + msg.header.stamp.sec = 0 + msg.header.stamp.nanosec = 0 + msg.header.frame_id = 'map' + msg.pose.pose.position.x = 0.0 + msg.pose.pose.position.y = 0.0 + msg.pose.pose.position.z = 0.0 + msg.pose.pose.orientation.x = 0.0 + msg.pose.pose.orientation.y = 0.0 + msg.pose.pose.orientation.z = 0.0 + msg.pose.pose.orientation.w = 1.0 + msg.pose.covariance = [ + 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942 + ] + + self.publisher.publish(msg) + self.get_logger().info('Published initial pose (0, 0)') + self.published = True + # Shutdown after publishing + self.get_logger().info('Shutting down initial pose publisher') + raise rclpy.shutdown() + + +def main(args=None): + rclpy.init(args=args) + try: + node = InitialPosePublisher() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/TurtleBot-RRT-Star/src/rrtstar_planner.cpp b/src/TurtleBot-RRT-Star/src/rrtstar_planner.cpp new file mode 100755 index 0000000..d7dc836 --- /dev/null +++ b/src/TurtleBot-RRT-Star/src/rrtstar_planner.cpp @@ -0,0 +1,321 @@ +#include +#include +#include +#include "nav2_util/node_utils.hpp" +#include +#include +#include + +#include "nav2_rrtstar_planner/rrtstar_planner.hpp" + +namespace nav2_rrtstar_planner +{ + +void RRTStar::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) +{ + node_ = parent.lock(); + if (!node_) { + RCLCPP_ERROR(rclcpp::get_logger("RRTStar"), "Failed to lock parent node in configure; parent is expired."); + return; + } + name_ = name; + tf_ = tf; + costmap_ = costmap_ros->getCostmap(); + global_frame_ = costmap_ros->getGlobalFrameID(); + max_iterations_ = 1000; + + // Parameter initialization + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".interpolation_resolution", rclcpp::ParameterValue(0.01)); + node_->get_parameter(name_ + ".interpolation_resolution", interpolation_resolution_); +} + +void RRTStar::cleanup() +{ + RCLCPP_INFO( + node_->get_logger(), "CleaningUp plugin %s of type NavfnPlanner", + name_.c_str()); +} + +void RRTStar::activate() +{ + RCLCPP_INFO( + node_->get_logger(), "Activating plugin %s of type NavfnPlanner", + name_.c_str()); +} + +void RRTStar::deactivate() +{ + RCLCPP_INFO( + node_->get_logger(), "Deactivating plugin %s of type NavfnPlanner", + name_.c_str()); +} + +void RRTStar::calculateBallRadiusConstant() { + double resolution = costmap_->getResolution(); + double cellArea = resolution * resolution; + unsigned int numFreeCells = 0; + + for (unsigned int x = 0; x < costmap_->getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < costmap_->getSizeInCellsY(); y++) { + if (costmap_->getCost(x, y) == nav2_costmap_2d::FREE_SPACE) { + numFreeCells++; + } + } + } + + double freeVolume = cellArea * numFreeCells; + int dimensions = 2; + double vUnitBall = M_PI; + ball_radius_constant_ = 2.0 * (1 + 1.0 / dimensions) * std::pow((freeVolume / vUnitBall), (1.0 / dimensions)); +} + +double RRTStar::calculateBallRadius(int tree_size, int dimensions, double max_connection_distance) { + double term1 = (ball_radius_constant_ * std::log(tree_size)) / tree_size; + double term2 = std::pow(term1, 1.0 / dimensions); + return std::min(term2, max_connection_distance); +} + +std::vector RRTStar::findVerticesInsideCircle(double center_x, double center_y, double radius) { + std::vector vertices_inside_circle; + double radius_squared = radius * radius; + + for (int i = 0; i < tree_.size(); ++i) { + // Dereference unique_ptr to access x and y + double distance_squared = std::pow((*tree_[i]).x - center_x, 2) + std::pow((*tree_[i]).y - center_y, 2); + if (distance_squared <= radius_squared) { + vertices_inside_circle.push_back(i); + } + } + return vertices_inside_circle; +} + + +double RRTStar::calculate_distance(double x, double y, const Vertex& vertex) { + return std::sqrt(std::pow(vertex.x - x, 2) + std::pow(vertex.y - y, 2)); +} + +Vertex* RRTStar::nearest_neighbor(double x, double y) { + Vertex* nearest_vertex = nullptr; + double min_dist = std::numeric_limits::infinity(); + + for (const auto& vertex : tree_) { + // Dereference unique_ptr to pass Vertex reference to calculate_distance + double dist = calculate_distance(x, y, *vertex); + if (dist < min_dist) { + min_dist = dist; + nearest_vertex = vertex.get(); // Set to raw pointer of the unique_ptr + } + } + return nearest_vertex; +} + + +bool RRTStar::connectible(const Vertex& start, const Vertex& end) { + double resolution = interpolation_resolution_; + double steps = std::ceil(std::hypot(end.x - start.x, end.y - start.y) / resolution); + if (steps > 0){ + double x_increment = (end.x - start.x) / steps; + double y_increment = (end.y - start.y) / steps; + + double x = start.x, y = start.y; + for (int i = 0; i < steps; ++i) { + unsigned int mx, my; + if (!costmap_->worldToMap(x, y, mx, my)) return false; + if (costmap_->getCost(mx, my) != nav2_costmap_2d::FREE_SPACE) return false; + x += x_increment; + y += y_increment; + } + } + return true; +} + +double RRTStar::calculate_cost_from_start(const Vertex& vertex) { + double total_cost = 0.0; + + const Vertex* cur_ver = &vertex; + + while (cur_ver != nullptr) { + total_cost += cur_ver->cost; + cur_ver = cur_ver->parent; + } + + return total_cost; +} + +nav_msgs::msg::Path RRTStar::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + nav_msgs::msg::Path global_path; + + // Checking if the goal and start state is in the global frame + if (start.header.frame_id != global_frame_) { + RCLCPP_ERROR( + node_->get_logger(), "Planner will only accept start position from %s frame", + global_frame_.c_str()); + return global_path; + } + + if (goal.header.frame_id != global_frame_) { + RCLCPP_INFO( + node_->get_logger(), "Planner will only accept goal position from %s frame", + global_frame_.c_str()); + return global_path; + } + + global_path.poses.clear(); + global_path.header.stamp = node_->now(); + global_path.header.frame_id = global_frame_; + + // Set up a random position generator + calculateBallRadiusConstant(); + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> x_dis(costmap_->getOriginX(), costmap_->getOriginX() + costmap_->getSizeInCellsX() * costmap_->getResolution()); + std::uniform_real_distribution<> y_dis(costmap_->getOriginY(), costmap_->getOriginY() + costmap_->getSizeInCellsY() * costmap_->getResolution()); + + // Add start position to the tree + tree_.clear(); + tree_.reserve(max_iterations_); + auto start_vertex = std::make_unique(start.pose.position.x, start.pose.position.y); + start_vertex->cost = 0; + tree_.emplace_back(std::move(start_vertex)); + + // Create vertex for the end point + Vertex end_vertex(goal.pose.position.x, goal.pose.position.y); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = goal.pose.position.x; + pose.pose.position.y = goal.pose.position.y; + pose.pose.position.z = 0.0; + pose.pose.orientation = goal.pose.orientation; + global_path.poses.insert(global_path.poses.begin(), pose); + + for (int i = 1; i <= max_iterations_ - 1; ++i) { + // Generate a random point + double rand_x = x_dis(gen); + double rand_y = y_dis(gen); + auto new_position = std::make_unique(rand_x, rand_y); + + // Find nearest neighbor and assign its parent to new_position + Vertex* nearest = nearest_neighbor(rand_x, rand_y); + new_position->parent = nearest; // Use raw pointer to nearest vertex + new_position->cost = calculate_distance(nearest->x, nearest->y, *new_position); + + if (connectible(*nearest, *new_position)) { + // Perform rewire operation + double ball_radius = calculateBallRadius(tree_.size(), 2, 2.0); + + std::vector vertices_inside_circle = findVerticesInsideCircle(new_position->x, new_position->y, ball_radius); + tree_.emplace_back(std::move(new_position)); + + RCLCPP_INFO(node_->get_logger(), "New vertex x: %.4f", tree_.back()->x); + RCLCPP_INFO(node_->get_logger(), "New vertex y: %.4f", tree_.back()->y); + + double total_cost_for_new_position = calculate_cost_from_start(*tree_.back()); + + int num_of_rewiring = 0; + + // Check if there is a better route from start towards the new position + for (size_t j = 0; j < vertices_inside_circle.size(); ++j) { + int index = vertices_inside_circle[j]; + double potential_cost = calculate_cost_from_start(*tree_[index]) + calculate_distance(tree_.back()->x, tree_.back()->y, *tree_[index]); + if (potential_cost < total_cost_for_new_position && connectible(*tree_.back(), *tree_[index])) { + tree_.back()->parent = tree_[index].get(); + tree_.back()->cost = calculate_distance(tree_.back()->x, tree_.back()->y, *tree_[index]); + total_cost_for_new_position = potential_cost; + num_of_rewiring += 1; + } + } + + // Check if any existing vertex may benefit from being connected by the new position + for (size_t j = 0; j < vertices_inside_circle.size(); ++j) { + int index = vertices_inside_circle[j]; + double current_cost = calculate_cost_from_start(*tree_[index]); + double potential_cost = calculate_cost_from_start(*tree_.back()) + calculate_distance(tree_.back()->x, tree_.back()->y, *tree_[index]); + if (potential_cost < current_cost && connectible(*tree_.back(), *tree_[index])) { + tree_[index]->parent = tree_.back().get(); + tree_[index]->cost = calculate_distance(tree_.back()->x, tree_.back()->y, *tree_[index]); + num_of_rewiring += 1; + } + } + } else { + i -= 1; + } + } + + // Find optimal way to the goal + double ball_radius = 2 * calculateBallRadius(tree_.size(), 2, 2.0); + std::vector vertices_inside_circle = findVerticesInsideCircle(goal.pose.position.x, goal.pose.position.y, ball_radius); + + while (true) { + double min_cost = std::numeric_limits::infinity(); + + for (size_t j = 0; j < vertices_inside_circle.size(); ++j) { + int index = vertices_inside_circle[j]; + double potential_cost = calculate_cost_from_start(*tree_[index]) + calculate_distance(goal.pose.position.x, goal.pose.position.y, *tree_[index]); + if (potential_cost < min_cost && connectible(end_vertex, *tree_[index])) { + end_vertex.parent = tree_[index].get(); + end_vertex.cost = calculate_distance(goal.pose.position.x, goal.pose.position.y, *tree_[index]); + min_cost = potential_cost; + } + } + + if (min_cost < 10000) { + auto end_vertex_ptr = std::make_unique(end_vertex); + tree_.emplace_back(std::move(end_vertex_ptr)); + + Vertex* cur_ver = &end_vertex; + while (cur_ver) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = cur_ver->x; + pose.pose.position.y = cur_ver->y; + pose.pose.position.z = 0.0; + + global_path.poses.insert(global_path.poses.begin(), pose); + + // Add waypoints between the random points + if (cur_ver->parent != nullptr) { + double steps = std::ceil(std::hypot(cur_ver->x - cur_ver->parent->x, cur_ver->y - cur_ver->parent->y) * 10); + double x_increment = (cur_ver->parent->x - cur_ver->x) / steps; + double y_increment = (cur_ver->parent->y - cur_ver->y) / steps; + + double x = cur_ver->x; + double y = cur_ver->y; + + for (int i = 0; i < steps - 1; ++i) { + x += x_increment; + y += y_increment; + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = 0.0; + + global_path.poses.insert(global_path.poses.begin(), pose); + } + } + cur_ver = cur_ver->parent; + } + break; + } + + if (ball_radius > 100) { + break; + } + + ball_radius += 0.5; + vertices_inside_circle = findVerticesInsideCircle(goal.pose.position.x, goal.pose.position.y, ball_radius); + } + + return global_path; +} + + +} // namespace nav2_rrtstar_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_rrtstar_planner::RRTStar, nav2_core::GlobalPlanner)