diff --git a/README.md b/README.md index b278f36..56ce6bc 100644 --- a/README.md +++ b/README.md @@ -1,18 +1,60 @@ -# 开始拉取代码 +* 基于 RRT * 的路径规划与导航 +实现功能：替换 Nav2 规划器为RRT-Star自定义插件，实现渐近最优路径规划，支持从 RViz2 设置目标点完成自主导航。RRT* 被用于导航与路径规划，它具有渐近最优性， 即随着采样点数量增加，解会收敛到最优路径，同时能高效探索高维空间。 + +## 1. 环境要求 +- Ubuntu 22.04 +- ROS2 Humble +- Gazebo Sim Harmonic --- -``` -git clone http://git-test.databall.tech:3000/hq/office_RRT_star.git -cd office_RRT_star +### 编译 -# add your files to repo -git push +```bash +cd /workspace +colcon build --symlink-install +source /opt/ros/humble/setup.bash +source install/setup.bash ``` ---- + +### 注意事项 +之后每打开一个新终端，都建议先执行： + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash +export TURTLEBOT3_MODEL=burger +export NAV2_MAP_PATH=/workspace/src/turtlebot3_simulations/turtlebot3_gazebo/map/office_map.yaml ``` -cd existing_repo -git remote add origin http://git-test.databall.tech:3000/hq/office_RRT_star.git -git branch -M main -git push -uf origin main -``` \ No newline at end of file + +为了方便，也可以写入 `~/.bashrc`： + +```bash +echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc +echo 'source /workspace/install/setup.bash' >> ~/.bashrc +echo 'export TURTLEBOT3_MODEL=burger' >> ~/.bashrc +echo 'export NAV2_MAP_PATH=/workspace/src/turtlebot3_simulations/turtlebot3_gazebo/map/office_map.yaml' >> ~/.bashrc +source ~/.bashrc +``` + +1. 每个终端都要按照3.2.1前置准备操作 +2. 启动仿真， Nav2导航栈， AMCL节点，rviz +```bash +./gzsim_run.sh + +ros2 launch nav2_bringup navigation_launch.py \ + use_sim_time:=True \ + params_file:=$HOME/ros_ws/src/vlm-semantic-nav2/TurtleBot-RRT-Star/nav2_params.yaml \ + map:=$NAV2_MAP_PATH + +ros2 launch nav2_rrtstar_planner bringup_localization_with_initial_pose.launch.py \ + use_sim_time:=true \ + map:=$NAV2_MAP_PATH + +ros2 run rviz2 rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz --ros-args -p use_sim_time:=true +``` + +点击 RViz2 中Nav2 Goal，在地图上选择目标位置，调整箭头设置机器人到达后的朝向，确认后机器人将通过 RRT * 算法生成最优路径并自主导航RViz2 中将实时显示 RRT * 规划的全局路径。 +![图6](assets/picture06.png) + + diff --git a/assets/picture06.png b/assets/picture06.png new file mode 100644 index 0000000..46675fa Binary files /dev/null and b/assets/picture06.png differ diff --git a/gzsim_run.sh b/gzsim_run.sh new file mode 100755 index 0000000..e0a23a6 --- /dev/null +++ b/gzsim_run.sh @@ -0,0 +1,68 @@ +#!/usr/bin/env bash +set -euo pipefail + + +ROOT="/workspace" + +sleep 5 + +set +u +source /opt/ros/humble/setup.bash +source "${ROOT}/install/setup.bash" +set -u + +LOG_DIR="${ROOT}/autorun_logs/autorun_$(date +%Y%m%d_%H%M%S)" +mkdir -p "$LOG_DIR" + +kill_port_listeners() { + local port="$1" + local pids + pids="$(ss -ltnp 2>/dev/null | grep ":${port} " | sed -E 's/.*pid=([0-9]+).*/\1/' | sort -u || true)" + if [[ -n "${pids}" ]]; then + echo "[run_all] port ${port} already in use, killing: ${pids}" + kill ${pids} 2>/dev/null || true + sleep 1 + fi +} + +kill_port_listeners 9002 + +export TURTLEBOT3_MODEL=burger +ros2 launch turtlebot3_gazebo turtlebot3_office.launch.py > "${LOG_DIR}/tb3_gzsim.log" 2>&1 & +GZSIM_PID=$! +# gz sim -v 4 -s -r empty.sdf > "${LOG_DIR}/empty_gzsim.log" 2>&1 & +# GZSIM_PID=$! + +echo "[autorun] tb3_gzsim pid=${GZSIM_PID}, log=${LOG_DIR}/tb3_gzsim.log" + +sleep 8 + +if ps -p "$GZSIM_PID" > /dev/null; then + echo "[autorun] tb3_gzsim is still running" +else + echo "[autorun] tb3_gzsim exited early. Log:" + cat "${LOG_DIR}/tb3_gzsim.log" +fi + +echo "[autorun] starting websocket ..." + +gz launch "${ROOT}/src/websocket.gzlaunch" > "${LOG_DIR}/websocket.log" 2>&1 & +WEBSOCKET_PID=$! + +echo "[autorun] websocket pid=${WEBSOCKET_PID}, log=${LOG_DIR}/websocket.log" + +sleep 10 + +if ps -p "$WEBSOCKET_PID" > /dev/null; then + echo "[autorun] websocket is still running" +else + echo "[autorun] websocket exited early. Log:" + cat "${LOG_DIR}/websocket.log" + exit 1 +fi + +echo "[autorun] all services started." +echo "[autorun] tb3_gzsim pid=${GZSIM_PID}" +echo "[autorun] websocket pid=${WEBSOCKET_PID}" + +wait diff --git a/src/TurtleBot-RRT-Star/.DS_Store b/src/TurtleBot-RRT-Star/.DS_Store new file mode 100644 index 0000000..0ea5f7c Binary files /dev/null and b/src/TurtleBot-RRT-Star/.DS_Store differ 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) diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/CHANGELOG.rst b/src/turtlebot3_simulations/turtlebot3_gazebo/CHANGELOG.rst new file mode 100755 index 0000000..09601f1 --- /dev/null +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/CHANGELOG.rst @@ -0,0 +1,187 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package turtlebot3_gazebo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.8 (2025-07-10) +------------------ +* None + +2.3.6 (2025-06-19) +------------------ +* None + +2.3.4 (2025-05-28) +------------------ +* None + +2.3.0 (2025-02-17) +------------------ +* Added multi-robot launch functionality +* Updated robot mesh in Gazebo and RViz +* Added launch file for TurtleBot3 Autorace 2020 +* Added plugins to the models of Autorace 2020 +* Contributors: Hyungyu Kim + +2.2.6 (2022-05-26) +------------------ +* ROS 2 Humble Hawksbill supported +* Contributors: Will Son + +2.2.5 (2021-08-25) +------------------ +* Release for ROS 2 Rolling +* Contributors: Will Son + +2.2.4 (2021-06-14) +------------------ +* Release for ROS 2 Galactic +* Separate world and robot models(#162) +* Clean up unncessary files +* Use turtlebot3_common mesh modeling +* Independent turtlebot3_simulations package +* Contributors: Joep Tool, Will Son + +2.2.3 (2021-04-12) +------------------ +* Update required keyword arguments +* Clear up exec_depend +* Fix Waffle Pi wheel inertia +* Contributors: ruffsl, Will Son + +2.2.2 (2021-02-24) +------------------ +* Remove shared objects built in older version +* Contributors: Will Son + +2.2.1 (2021-01-13) +------------------ +* Eloquent Elusor EOL +* Add missing imu joint in sdf +* Append Gazebo model path +* Portable fix, launch description revise +* Ament lint applied +* Contributors: minwoominwoominwoo7, Rayman, seanyen, ashe kim, Will Son + +2.2.0 (2020-06-29) +------------------ +* TurtleBot3 Drive node implementation +* Additional Gazebo maps added +* argument tags in the sdf file replaced with remapping tags +* Low polygon 3D modeling applied for simulation +* Contributors: Ryan Shim, Mikael Arguedas, Will Son + +2.1.0 (2019-09-10) +------------------ +* Added turtlebot3_house and related world, model files +* Contributors: Ryan Shim + +2.0.1 (2019-09-05) +------------------ +* Modified dependency packages +* Modified launch directory +* Added a launch file for robot state publisher +* Contributors: Darby Lim, Pyo + +2.0.0 (2019-08-20) +------------------ +* Supported ROS 2 Dashing Diademata +* Updated the CHANGELOG and version to release binary packages +* Contributors: Darby Lim, Pyo + +1.3.0 (2020-06-29) +------------------ +* Turtlebot3 Autorace 2020 implemented +* Remove the plugin_path from gazebo_ros export +* Remove *nix path separator +* Contributors: Ashe Kim, Ben Wolsieffer, Sean Yen + +1.2.0 (2019-01-22) +------------------ +* moved into `#65 `_ +* modified ML stage +* delete unused param +* update algorithm and modified variable more clearly +* Contributors: Darby Lim, Gilbert, Louise Poubel, Pyo + +1.1.0 (2018-07-20) +------------------ +* modified uri path +* modified autorace +* delete remap +* Contributors: Darby Lim, Gilbert, Pyo + +1.0.2 (2018-06-01) +------------------ +* added mission.launch modified model.sdf +* deleted turtlebot3's gazebo plugins +* modified autorace gazebo +* merged pull request `#53 `_ `#52 `_ `#51 `_ `#50 `_ `#49 `_ +* Contributors: Gilbert, Darby Lim, Pyo + +1.0.1 (2018-05-30) +------------------ +* resolving dependency issues: + http://build.ros.org/job/Kbin_dj_dJ64__turtlebot3_gazebo__debian_jessie_amd64__binary/2/ +* Contributors: Pyo + +1.0.0 (2018-05-29) +------------------ +* added world for turtlebot3_autorace +* added world for turtlebot3_machine_learning +* merged pull request `#46 `_ from AuTURBO/develop + add turtlebot3_autorace world' +* merged pull request `#48 `_ `#47 `_ `#44 `_ `#42 `_ `#41 `_ +* Contributors: Darby Lim, Gilbert, hyunoklee, Pyo + +0.2.4 (2018-03-14) +------------------ +* None + +0.2.3 (2018-03-14) +------------------ +* solved DuplicateVersionsException error +* Contributors: Pyo + +0.2.2 (2018-03-14) +------------------ +* None + +0.2.1 (2018-03-14) +------------------ +* added worlds for gazebo and turtlebot3 +* Contributors: Darby Lim + +0.2.0 (2018-03-13) +------------------ +* added slam with multiple tb3 +* added multi example +* added turtlebot3_house +* modified cmake file +* modified spwn model name +* modified multi slam param +* modified camera position +* modified folder name +* Contributors: Darby Lim + +0.1.7 (2017-08-16) +------------------ +* renamed missed the install rule (worlds -> models) +* Contributors: Darby Lim, Tully Foote + +0.1.6 (2017-08-14) +------------------ +* modified folder name and model path +* updated rviz and add static tf publisher for depth camera +* Contributors: Darby Lim + +0.1.5 (2017-06-09) +------------------ +* modified make files for dependencies +* updated turtlebot3 sim +* updated world config +* Contributors: Darby Lim + +0.1.4 (2017-05-23) +------------------ +* added as new meta-packages and version update (0.1.4) +* Contributors: Darby Lim, Pyo diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt b/src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt new file mode 100755 index 0000000..2a1f27b --- /dev/null +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt @@ -0,0 +1,139 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) +project(turtlebot3_gazebo) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +if(MSVC) + add_compile_definitions(_USE_MATH_DEFINES) +endif() + +################################################################################ +# Find ament packages and libraries for ament and system dependencies +# NOTE: Migrating from Gazebo Classic to Gazebo Sim (gz-8 / Harmonic) +################################################################################ +find_package(ament_cmake REQUIRED) + +# ROS 2 core packages +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) + +# Gazebo Sim (Harmonic) libraries +find_package(gz-sim8 REQUIRED) +find_package(gz-msgs10 REQUIRED) +find_package(gz-transport13 REQUIRED) +find_package(gz-physics7 REQUIRED) +find_package(gz-math7 REQUIRED) +find_package(gz-common5 REQUIRED) +find_package(gz-plugin2 REQUIRED) +find_package(sdformat14 REQUIRED) + +# Optional: ros_gz bridge packages (for launch integration) +find_package(ros_gz_bridge QUIET) +find_package(ros_gz_sim QUIET) + +################################################################################ +# Build +################################################################################ + +include_directories( + include + ${gz-sim8_INCLUDE_DIRS} + ${gz-msgs10_INCLUDE_DIRS} + ${gz-transport13_INCLUDE_DIRS} + ${gz-physics7_INCLUDE_DIRS} + ${gz-math7_INCLUDE_DIRS} + ${gz-common5_INCLUDE_DIRS} + ${gz-plugin2_INCLUDE_DIRS} + ${sdformat14_INCLUDE_DIRS} +) + +set(dependencies + "geometry_msgs" + "nav_msgs" + "rclcpp" + "sensor_msgs" + "tf2" +) + +# --- Executable: turtlebot3_drive (ROS 2 navigation node, no gazebo deps) --- +set(EXEC_NAME "turtlebot3_drive") +add_executable(${EXEC_NAME} src/turtlebot3_drive.cpp) +ament_target_dependencies(${EXEC_NAME} ${dependencies}) + +# --- Shared Libraries: Custom Gazebo Sim System Plugins --- +# NOTE: These plugins originally used the Gazebo Classic API (gazebo::ModelPlugin) +# with headers like . They must be rewritten to use the +# gz-sim System API (ISystemPreUpdate, EntityComponentManager, etc.) +# before they can compile. Default OFF until porting is done. +option(BUILD_GAZEBO_PLUGINS "Build custom Gazebo Sim system plugins" OFF) + +if(BUILD_GAZEBO_PLUGINS) + set(GZ_PLUGIN_LIBRARIES + gz-sim8::gz-sim8 + gz-msgs10::gz-msgs10 + gz-transport13::gz-transport13 + gz-physics7::gz-physics7 + gz-math7::gz-math7 + gz-common5::gz-common5 + gz-plugin2::gz-plugin2 + sdformat14::sdformat14 + ) + + add_library(traffic_light_plugin SHARED src/traffic_light_plugin.cpp) + target_link_libraries(traffic_light_plugin ${GZ_PLUGIN_LIBRARIES}) + + add_library(traffic_bar_plugin SHARED src/traffic_bar_plugin.cpp) + target_link_libraries(traffic_bar_plugin ${GZ_PLUGIN_LIBRARIES}) + + add_library(obstacle1 SHARED src/obstacle1.cpp) + target_link_libraries(obstacle1 ${GZ_PLUGIN_LIBRARIES}) + + add_library(obstacle2 SHARED src/obstacle2.cpp) + target_link_libraries(obstacle2 ${GZ_PLUGIN_LIBRARIES}) + + add_library(obstacles SHARED src/obstacles.cpp) + target_link_libraries(obstacles ${GZ_PLUGIN_LIBRARIES}) +endif() + +################################################################################ +# Install +################################################################################ +install(TARGETS ${EXEC_NAME} + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch config models rviz urdf worlds scripts gui + DESTINATION share/${PROJECT_NAME}/ +) + +# Install Python scripts with correct permissions +install(PROGRAMS scripts/scan_frame_fix.py scripts/camera_topic_remap.py + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +################################################################################ +# Export & Macro for ament package +################################################################################ +ament_export_include_directories(include) +ament_export_dependencies(geometry_msgs) +ament_export_dependencies(nav_msgs) +ament_export_dependencies(rclcpp) +ament_export_dependencies(sensor_msgs) +ament_export_dependencies(tf2) +ament_package() diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/config/turtlebot3_gz_bridge.yaml b/src/turtlebot3_simulations/turtlebot3_gazebo/config/turtlebot3_gz_bridge.yaml new file mode 100644 index 0000000..7727668 --- /dev/null +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/config/turtlebot3_gz_bridge.yaml @@ -0,0 +1,69 @@ +# TurtleBot3 Burger - Gz Sim to ROS 2 Bridge Configuration +# For use with ros_gz_bridge bridge_node +# +# Key migration notes (Gazebo Classic -> Gazebo Sim): +# 1. gz-sim-diff-drive publishes Odometry but NOT TF (unlike Classic). +# TF is bridged from /world//pose/info (gz.msgs.Pose_V -> TFMessage). +# 2. Sensor frame_ids use Gazebo scoped names (e.g. burger/base_scan/...). +# Overridden below via 'frame_id' field to match URDF/robot_state_publisher tree. + +# === TF: bridge Gazebo model tf to ROS2 /tf === +# /model/burger/tf publishes gz.msgs.Pose_V (NOT Pose!) +# ros_gz_bridge converts Pose_V -> TFMessage correctly. +- ros_topic_name: /tf + ros_type_name: tf2_msgs/msg/TFMessage + gz_topic_name: /model/burger/tf + gz_type_name: gz.msgs.Pose_V + direction: GZ_TO_ROS + +# === Sensor bridges === +- ros_topic_name: /scan_raw + ros_type_name: sensor_msgs/msg/LaserScan + gz_topic_name: /world/default/model/burger/link/base_scan/sensor/hls_lfcd_lds/scan + gz_type_name: gz.msgs.LaserScan + direction: GZ_TO_ROS + +- ros_topic_name: /intel_realsense_r200_rgb/image_raw + ros_type_name: sensor_msgs/msg/Image + gz_topic_name: /world/default/model/burger/link/realsense_link/sensor/intel_realsense_r200_rgb/image + gz_type_name: gz.msgs.Image + direction: GZ_TO_ROS + +- ros_topic_name: /intel_realsense_r200_rgb/camera_info + ros_type_name: sensor_msgs/msg/CameraInfo + gz_topic_name: /world/default/model/burger/link/realsense_link/sensor/intel_realsense_r200_rgb/camera_info + gz_type_name: gz.msgs.CameraInfo + direction: GZ_TO_ROS + +# === Depth image (L16 workaround for Humble) === +# Gazebo depth camera with L16 publishes on /depth_image topic +# as gz.msgs.Image type (NOT /image). This bypasses unsupported gz.msgs.DepthImage. +- ros_topic_name: /intel_realsense_r200_depth/depth/image_raw + ros_type_name: sensor_msgs/msg/Image + gz_topic_name: /world/default/model/burger/link/realsense_link/sensor/intel_realsense_r200_depth/depth_image + gz_type_name: gz.msgs.Image + direction: GZ_TO_ROS + +- ros_topic_name: /intel_realsense_r200_depth/camera_info + ros_type_name: sensor_msgs/msg/CameraInfo + gz_topic_name: /world/default/model/burger/link/realsense_link/sensor/intel_realsense_r200_depth/camera_info + gz_type_name: gz.msgs.CameraInfo + direction: GZ_TO_ROS + +- ros_topic_name: /clock + ros_type_name: rosgraph_msgs/msg/Clock + gz_topic_name: /clock + gz_type_name: gz.msgs.Clock + direction: GZ_TO_ROS + +- ros_topic_name: /cmd_vel + ros_type_name: geometry_msgs/msg/Twist + gz_topic_name: /model/burger/cmd_vel + gz_type_name: gz.msgs.Twist + direction: ROS_TO_GZ + +- ros_topic_name: /odom + ros_type_name: nav_msgs/msg/Odometry + gz_topic_name: /model/burger/odometry + gz_type_name: gz.msgs.Odometry + direction: GZ_TO_ROS diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/gui/office_gui.config b/src/turtlebot3_simulations/turtlebot3_gazebo/gui/office_gui.config new file mode 100644 index 0000000..629f569 --- /dev/null +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/gui/office_gui.config @@ -0,0 +1,270 @@ + + + + + + + + 1400 + 900 +