hq
/
ros2_office_vlm
1
0
Fork 2
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update

This commit is contained in:
Xu Shiyuan 2026-05-07 15:20:22 +08:00
parent b772ccbed9
commit 97dd5d4ca4
8 changed files with 1003 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
src/TurtleBot-RRT-Star/CMakeLists.txt Executable file
View File

@ -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()

5
src/TurtleBot-RRT-Star/global_planner_plugin.xml Executable file
View File

@ -0,0 +1,5 @@
<library path="nav2_rrtstar_planner_plugin">
<class name="nav2_rrtstar_planner/RRTStar" type="nav2_rrtstar_planner::RRTStar" base_class_type="nav2_core::GlobalPlanner">
<description>This is a plugin for RRT Star path planning.</description>
</class>
</library>

60
src/TurtleBot-RRT-Star/include/nav2_rrtstar_planner/rrtstar_planner.hpp Executable file
View File

@ -0,0 +1,60 @@
#ifndef NAV2_RRTSTAR_PLANNER__RRTSTAR_PLANNER_HPP_
#define NAV2_RRTSTAR_PLANNER__RRTSTAR_PLANNER_HPP_
#include <string>
#include <memory>
#include <vector>
#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<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> 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<tf2_ros::Buffer> 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<std::unique_ptr<Vertex>> 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<int> 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_

54
src/TurtleBot-RRT-Star/launch/bringup_localization_with_initial_pose.launch.py Normal file
View File

@ -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

364
src/TurtleBot-RRT-Star/nav2_params.yaml Executable file
View File

@ -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

38
src/TurtleBot-RRT-Star/package.xml Executable file
View File

@ -0,0 +1,38 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_rrtstar_planner</name>
<version>1.0.0</version>
<description>RRT Star path planner.</description>
<maintainer email="echo@126.com">echo</maintainer>
<license>BSD-3-Clause</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>rclcpp_lifecycle</depend>
<depend>std_msgs</depend>
<depend>visualization_msgs</depend>
<depend>nav2_util</depend>
<depend>nav2_msgs</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>builtin_interfaces</depend>
<depend>tf2_ros</depend>
<depend>nav2_costmap_2d</depend>
<depend>nav2_core</depend>
<depend>pluginlib</depend>
<depend>nav2_bringup</depend>
<depend>random</depend>
<depend>vector</depend>
<depend>limits</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
<nav2_core plugin="${prefix}/global_planner_plugin.xml" />
</export>
</package>

66
src/TurtleBot-RRT-Star/scripts/publish_initial_pose.py Executable file
View File

@ -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()

321
src/TurtleBot-RRT-Star/src/rrtstar_planner.cpp Executable file
View File

@ -0,0 +1,321 @@
#include <cmath>
#include <string>
#include <memory>
#include "nav2_util/node_utils.hpp"
#include <random>
#include <vector>
#include <limits>
#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<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> 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<int> RRTStar::findVerticesInsideCircle(double center_x, double center_y, double radius) {
std::vector<int> 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<double>::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<Vertex>(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<Vertex>(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<int> 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<int> vertices_inside_circle = findVerticesInsideCircle(goal.pose.position.x, goal.pose.position.y, ball_radius);
while (true) {
double min_cost = std::numeric_limits<double>::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<Vertex>(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)