diff --git a/.gitignore b/.gitignore index e69de29..ed514b8 100644 --- a/.gitignore +++ b/.gitignore @@ -0,0 +1,17 @@ + +# Python dependencies (installed on-demand) +.site-packages/ + +# ROS2 build artifacts +build/ +install/ +log/ +logs/ + +# Python cache +__pycache__/ +*.pyc +*.pyo + +# YOLO model (auto-downloaded on first run) +yolov8n.pt diff --git a/autorun.sh b/autorun.sh index 48c73a3..b14a8b5 100755 --- a/autorun.sh +++ b/autorun.sh @@ -3,4 +3,4 @@ # 浏览器使用镜像内 foundation 已在 /opt/gzweb（8000）起的页面；本脚本仍起 gz sim、websocket.sdf(9002)、Nav2 等。 set -euo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" -exec bash "${ROOT}/run_all.sh" mapping --gzweb --no-gzweb-vite +exec bash "${ROOT}/run_all.sh" mapping --explore --gzweb --no-gzweb-vite --vision diff --git a/m-explore-ros2/explore/config/params.yaml b/m-explore-ros2/explore/config/params.yaml index f8fa6a3..c410ce1 100755 --- a/m-explore-ros2/explore/config/params.yaml +++ b/m-explore-ros2/explore/config/params.yaml @@ -5,10 +5,10 @@ costmap_topic: map costmap_updates_topic: map_updates visualize: true - planner_frequency: 0.2 #0.15 + planner_frequency: 1.0 #0.15 progress_timeout: 40.0 potential_scale: 1.0 #3.0 orientation_scale: 0.0 gain_scale: 3.0 #1.0 transform_tolerance: 0.3 - min_frontier_size: 0.5 #0.75 + min_frontier_size: 0.3 #0.75 diff --git a/nav2_params_fast.yaml b/nav2_params_fast.yaml new file mode 100644 index 0000000..19ef540 --- /dev/null +++ b/nav2_params_fast.yaml @@ -0,0 +1,350 @@ +amcl: + ros__parameters: + use_sim_time: True + 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 + wait_for_service_timeout: 1000 + # '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.5 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.5 + 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: 3.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.55 + 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: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +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.5, 0.0, 1.0] + min_velocity: [-0.5, 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/requirements.txt b/requirements.txt new file mode 100644 index 0000000..54956f8 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,6 @@ +# YOLO vision 依赖（安装到 /workspace/.site-packages 以持久化） +# 使用方法：pip install -r requirements.txt -t /workspace/.site-packages + +ultralytics +opencv-python +numpy<2 diff --git a/restart_nav2_explore.sh b/restart_nav2_explore.sh new file mode 100755 index 0000000..5125afd --- /dev/null +++ b/restart_nav2_explore.sh @@ -0,0 +1,24 @@ +#!/usr/bin/env bash +source /opt/ros/humble/setup.bash +source /workspace/install/setup.bash + +echo "[restart] Killing old processes..." +pkill -f "explore_lite/lib/explore_lite" 2>/dev/null || true +pkill -f "nav2_smoother/smoother_server" 2>/dev/null || true +pkill -f "nav2_behaviors/behavior_server" 2>/dev/null || true +pkill -f "nav2_waypoint_follower/waypoint_follower" 2>/dev/null || true +pkill -f "nav2_velocity_smoother/velocity_smoother" 2>/dev/null || true +pkill -f "nav2_controller/controller_server" 2>/dev/null || true +pkill -f "nav2_planner/planner_server" 2>/dev/null || true +pkill -f "nav2_bt_navigator/bt_navigator" 2>/dev/null || true +pkill -f "nav2_lifecycle_manager/lifecycle_manager" 2>/dev/null || true +sleep 2 + +echo "[restart] Starting Nav2 with fast params..." +nohup ros2 launch nav2_bringup navigation_launch.py use_sim_time:=true params_file:=/workspace/nav2_params_fast.yaml > /tmp/nav2.log 2>&1 & + +echo "[restart] Starting explore_lite with fast params..." +nohup ros2 launch explore_lite explore.launch.py use_sim_time:=true > /tmp/explore.log 2>&1 & + +echo "[restart] Done. Logs: /tmp/nav2.log /tmp/explore.log" +echo "[restart] Verify in 5s with: ros2 node list | grep -E \"explore|nav2\"" diff --git a/run_all.sh b/run_all.sh index 4d374a9..d65843e 100755 --- a/run_all.sh +++ b/run_all.sh @@ -1,347 +1,363 @@ -#!/usr/bin/env bash -set -euo pipefail -export DISPLAY=:1 - -# Unified launcher for TurtleBot3 office workflow: -# - Terminal A: simulation / bridge / (optional SLAM) / RViz -# - Terminal B: Nav2 -# - Optional C: explore_lite -# - Optional websocket + gzweb-demo frontend -# -# Modes: -# mapping : online SLAM mapping -# localization : map-based localization/navigation -# -# Mapping mode interactive commands: -# s : save map and switch to localization mode -# q : stop all -# -# Examples: -# ./run_all.sh mapping --explore --gzweb -# ./run_all.sh localization --gzweb --map /abs/path/office_map.yaml - -ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" -MODE="${1:-mapping}" -shift || true - -ROBOT_MODEL="${ROBOT_MODEL:-waffle}" -USE_SIM_TIME="${USE_SIM_TIME:-true}" -GZ_PARTITION="${GZ_PARTITION:-tb3_office_gz}" -WORLD_PATH_DEFAULT="${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/worlds/office_gz_dartsim.sdf" -MAP_YAML_DEFAULT="${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/map/office_map.yaml" -MAP_BASENAME_DEFAULT="${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/map/office_map" -SLEEP_NAV2="${SLEEP_NAV2:-18}" -EXPLORE_WAIT_TIMEOUT="${EXPLORE_WAIT_TIMEOUT:-420}" - -WORLD_PATH="${WORLD_PATH:-$WORLD_PATH_DEFAULT}" -MAP_YAML="${NAV2_MAP_PATH:-$MAP_YAML_DEFAULT}" -MAP_BASENAME="${MAP_BASENAME:-$MAP_BASENAME_DEFAULT}" - -WITH_EXPLORE=false -WITH_GZWE=false -NO_RVIZ=false -# 与 --gzweb 同用：只起 gz websocket(9002) 等资源，不起 office 内 npm run dev；页面用镜像内 /opt/gzweb（foundation） -NO_GZWEB_VITE=false - - -while [[ $# -gt 0 ]]; do - case "$1" in - --explore) WITH_EXPLORE=true; shift ;; - --gzweb) WITH_GZWEB=true; shift ;; - --no-gzweb-vite) NO_GZWEB_VITE=true; shift ;; - --no-rviz) NO_RVIZ=true; shift ;; - --map) MAP_YAML="$2"; shift 2 ;; - --world) WORLD_PATH="$2"; shift 2 ;; - --help|-h) - cat <<'EOF' -Usage: - ./run_all.sh mapping [--explore] [--gzweb] [--no-gzweb-vite] [--no-rviz] [--world PATH] - ./run_all.sh localization [--gzweb] [--no-gzweb-vite] [--no-rviz] [--map PATH] [--world PATH] - - --no-gzweb-vite 需与 --gzweb 同时使用：不启动 workspace 内 gzweb-demo 的 Vite；浏览器用 /opt/gzweb（8000）。 -EOF - exit 0 - ;; - *) - echo "[run_all] unknown argument: $1" >&2 - exit 1 - ;; - esac -done - -if [[ "$MODE" != "mapping" && "$MODE" != "localization" ]]; then - echo "[run_all] mode must be mapping or localization" >&2 - exit 1 -fi - -if [[ ! -f "$WORLD_PATH" ]]; then - echo "[run_all] world not found: $WORLD_PATH" >&2 - exit 1 -fi - -if [[ "$MODE" == "localization" && ! -f "$MAP_YAML" ]]; then - echo "[run_all] map yaml not found: $MAP_YAML" >&2 - exit 1 -fi - -set +u -source /opt/ros/humble/setup.bash - -# 仅在 install 目录缺失时构建（避免每次启动都重复构建，同时修复未 source ROS 环境导致的 ament_cmake 找不到） -if [[ ! -d "${ROOT}/install" ]]; then - colcon build --symlink-install -fi - -source "${ROOT}/install/setup.bash" -set -u -export TURTLEBOT3_MODEL="$ROBOT_MODEL" - -LOG_DIR="${ROOT}/logs/run_all_$(date +%Y%m%d_%H%M%S)" -mkdir -p "$LOG_DIR" -mkdir -p "$(dirname "$MAP_BASENAME")" - -PIDS=() - -start_bg() { - local name="$1"; shift - local logfile="${LOG_DIR}/${name}.log" - echo "[run_all] starting ${name} ..." - "$@" >"$logfile" 2>&1 & - local pid=$! - PIDS+=("$pid") - echo "[run_all] ${name} pid=${pid}, log=${logfile}" -} - -cleanup() { - echo "[run_all] stopping all processes..." - for pid in "${PIDS[@]:-}"; do - kill "$pid" 2>/dev/null || true - done -} -trap cleanup EXIT INT TERM - -wait_for_topic() { - local topic="$1" - local timeout="${2:-60}" - local i=0 - while (( i < timeout )); do - if ros2 topic list 2>/dev/null | grep -Fxq "${topic}"; then - return 0 - fi - sleep 1 - ((i+=1)) - done - return 1 -} - -wait_for_port() { - local port="$1" - local timeout="${2:-30}" - local i=0 - while (( i < timeout )); do - if ss -ltn 2>/dev/null | grep -q ":${port} "; then - return 0 - fi - sleep 1 - ((i+=1)) - done - return 1 -} - -wait_for_explore_stop_log() { - local logfile="$1" - local timeout="${2:-900}" - local i=0 - while (( i < timeout )); do - if [[ -f "$logfile" ]] && grep -Eq "All frontiers traversed/tried out, stopping\\.|Exploration stopped\\." "$logfile"; then - return 0 - fi - sleep 1 - ((i+=1)) - done - return 1 -} - -wait_for_explore_stop_log_with_timer() { - local logfile="$1" - local timeout="${2:-900}" - local start_ts - start_ts="$(date +%s)" - local i=0 - while (( i < timeout )); do - if [[ -f "$logfile" ]] && grep -Eq "All frontiers traversed/tried out, stopping\\.|Exploration stopped\\." "$logfile"; then - return 0 - fi - if (( i % 10 == 0 )); then - local now_ts elapsed - now_ts="$(date +%s)" - elapsed=$(( now_ts - start_ts )) - echo "[run_all] explore elapsed: ${elapsed}s" - fi - sleep 1 - ((i+=1)) - done - return 1 -} - -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 -} - -show_rviz_arg="show_rviz:=true" -if [[ "$NO_RVIZ" == true ]]; then - show_rviz_arg="show_rviz:=false" -fi - -if [[ "$MODE" == "mapping" ]]; then - start_bg "A_sim" \ - ros2 launch turtlebot3_gazebo turtlebot3_office_gz.launch.py \ - robot_model:="$ROBOT_MODEL" \ - use_sim_time:="$USE_SIM_TIME" \ - start_slam:=true \ - "$show_rviz_arg" \ - gz_partition:="$GZ_PARTITION" \ - gz_world_name:=default \ - world:="$WORLD_PATH" -else - start_bg "A_sim" \ - ros2 launch turtlebot3_gazebo turtlebot3_office_gz.launch.py \ - robot_model:="$ROBOT_MODEL" \ - use_sim_time:="$USE_SIM_TIME" \ - start_slam:=false \ - "$show_rviz_arg" \ - gz_partition:="$GZ_PARTITION" \ - gz_world_name:=default \ - world:="$WORLD_PATH" -fi - -# --gzweb：gz launch(9002) + 可选 office 内 gzweb-demo 的 Vite（5173）。 -# --gzweb --no-gzweb-vite：不起 office Vite；页面用 foundation 已在 /opt/gzweb:8000 起的实例（见 run_all_fix.sh）。 -if [[ "$WITH_GZWEB" == true ]]; then - export GZ_PARTITION - OFFICE_MODEL_ROOT="${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_office" - RESOURCE_PATHS=( - "${HOME}/.gazebo/models" - "${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/models" - "${OFFICE_MODEL_ROOT}" - ) - for d in "${OFFICE_MODEL_ROOT}"/*/materials/scripts "${OFFICE_MODEL_ROOT}"/*/materials/textures; do - [[ -d "${d}" ]] && RESOURCE_PATHS+=("${d}") - done - RESOURCE_PATH_JOINED="$(IFS=:; echo "${RESOURCE_PATHS[*]}")" - export GZ_SIM_RESOURCE_PATH="${RESOURCE_PATH_JOINED}" - # Keep compatibility with components resolving textures via legacy lookup vars. - export GZ_FILE_PATH="${RESOURCE_PATH_JOINED}" - export GAZEBO_RESOURCE_PATH="${RESOURCE_PATH_JOINED}" - - if [[ "$NO_GZWEB_VITE" != true ]]; then - if [[ ! -d "${ROOT}/gzweb-demo/gzweb-demo/node_modules/gzweb" ]]; then - echo "[run_all] gzweb demo dependencies missing, running npm install once..." - bash -lc "cd '${ROOT}/gzweb-demo/gzweb-demo' && npm install" - fi - else - echo "[run_all] --no-gzweb-vite: skipping office gzweb-demo npm install; use /opt/gzweb (port 8000) for UI." - fi - - kill_port_listeners 9002 - kill_port_listeners 5173 - - echo "[run_all] waiting for /clock before websocket..." - if wait_for_topic "/clock" 25; then - start_bg "D_websocket" gz launch "${ROOT}/websocket.sdf" - if [[ "$NO_GZWEB_VITE" != true ]]; then - start_bg "E_gzweb_frontend" bash -lc "cd '${ROOT}/gzweb-demo/gzweb-demo' && npm run dev:host" - echo "[run_all] office gzweb-demo Vite: http://localhost:5173" - fi - else - echo "[run_all] WARN: /clock not detected in time; starting websocket (and optional frontend) anyway." - start_bg "D_websocket" gz launch "${ROOT}/websocket.sdf" - if [[ "$NO_GZWEB_VITE" != true ]]; then - start_bg "E_gzweb_frontend" bash -lc "cd '${ROOT}/gzweb-demo/gzweb-demo' && npm run dev:host" - fi - fi - - if wait_for_port 9002 12; then - echo "[run_all] websocket server ready on ws://localhost:9002" - else - echo "[run_all] ERROR: websocket server failed on 9002; check ${LOG_DIR}/D_websocket.log" - fi -fi - -echo "[run_all] waiting ${SLEEP_NAV2}s before starting Nav2..." -sleep "$SLEEP_NAV2" - -if [[ "$MODE" == "mapping" ]]; then - start_bg "B_nav2" ros2 launch nav2_bringup navigation_launch.py use_sim_time:="$USE_SIM_TIME" -else - start_bg "B_nav2" \ - ros2 launch nav2_bringup bringup_launch.py \ - use_sim_time:="$USE_SIM_TIME" \ - map:="$MAP_YAML" \ - slam:=false -fi - -if [[ "$WITH_EXPLORE" == true && "$MODE" == "mapping" ]]; then - start_bg "C_explore" ros2 launch explore_lite explore.launch.py use_sim_time:="$USE_SIM_TIME" - last_index=$(( ${#PIDS[@]} - 1 )) - EXPLORE_PID="${PIDS[$last_index]}" - EXPLORE_LOG="${LOG_DIR}/C_explore.log" -fi - -echo -echo "[run_all] started. Mode: ${MODE}" -echo "[run_all] logs: ${LOG_DIR}" -echo - -if [[ "$MODE" == "mapping" ]]; then - if [[ "$WITH_GZWEB" == true ]]; then - if [[ "$NO_GZWEB_VITE" == true ]]; then - echo "[run_all] UI: foundation /opt/gzweb on port 8000 (no office Vite)." - elif wait_for_port 5173 25; then - echo "[run_all] office gzweb-demo ready: http://localhost:5173" - else - echo "[run_all] WARN: office gzweb frontend not ready yet, check ${LOG_DIR}/E_gzweb_frontend.log" - fi - fi - - if [[ "${WITH_EXPLORE}" == true && -n "${EXPLORE_PID:-}" ]]; then - echo "[run_all] exploration is running (pid=${EXPLORE_PID}), waiting for stop signal (timeout=${EXPLORE_WAIT_TIMEOUT}s)..." - if wait_for_explore_stop_log_with_timer "${EXPLORE_LOG}" "${EXPLORE_WAIT_TIMEOUT}"; then - echo "[run_all] detected exploration stop from log, finalizing explore process..." - else - echo "[run_all] WARN: explore stop not detected within timeout; forcing stop and saving current map." - fi - kill "${EXPLORE_PID}" 2>/dev/null || true - wait "${EXPLORE_PID}" 2>/dev/null || true - echo "[run_all] explore_lite stopped, now saving map." - fi - - if [[ "${WITH_EXPLORE}" == true ]]; then - echo "[run_all] auto-saving map to ${MAP_BASENAME}.yaml/.pgm ..." - ros2 run nav2_map_server map_saver_cli -f "$MAP_BASENAME" --ros-args -p use_sim_time:="$USE_SIM_TIME" - echo "[run_all] map saved successfully:" - echo " - ${MAP_BASENAME}.yaml" - echo " - ${MAP_BASENAME}.pgm" - echo "[run_all] mapping workflow completed, exiting now." - exit 0 - fi - - cat <&2 + exit 1 + ;; + esac +done + +if [[ "$MODE" != "mapping" && "$MODE" != "localization" ]]; then + echo "[run_all] mode must be mapping or localization" >&2 + exit 1 +fi + +if [[ ! -f "$WORLD_PATH" ]]; then + echo "[run_all] world not found: $WORLD_PATH" >&2 + exit 1 +fi + +if [[ "$MODE" == "localization" && ! -f "$MAP_YAML" ]]; then + echo "[run_all] map yaml not found: $MAP_YAML" >&2 + exit 1 +fi + +set +u +source /opt/ros/humble/setup.bash + +# 仅在 install 目录缺失时构建（避免每次启动都重复构建，同时修复未 source ROS 环境导致的 ament_cmake 找不到） +if [[ ! -d "${ROOT}/install" ]]; then + colcon build --symlink-install +fi + +source "${ROOT}/install/setup.bash" +set -u +export TURTLEBOT3_MODEL="$ROBOT_MODEL" + +LOG_DIR="${ROOT}/logs/run_all_$(date +%Y%m%d_%H%M%S)" +mkdir -p "$LOG_DIR" +mkdir -p "$(dirname "$MAP_BASENAME")" + +PIDS=() + +start_bg() { + local name="$1"; shift + local logfile="${LOG_DIR}/${name}.log" + echo "[run_all] starting ${name} ..." + "$@" >"$logfile" 2>&1 & + local pid=$! + PIDS+=("$pid") + echo "[run_all] ${name} pid=${pid}, log=${logfile}" +} + +cleanup() { + echo "[run_all] stopping all processes..." + for pid in "${PIDS[@]:-}"; do + kill "$pid" 2>/dev/null || true + done +} +trap cleanup EXIT INT TERM + +wait_for_topic() { + local topic="$1" + local timeout="${2:-60}" + local i=0 + while (( i < timeout )); do + if ros2 topic list 2>/dev/null | grep -Fxq "${topic}"; then + return 0 + fi + sleep 1 + ((i+=1)) + done + return 1 +} + +wait_for_port() { + local port="$1" + local timeout="${2:-30}" + local i=0 + while (( i < timeout )); do + if ss -ltn 2>/dev/null | grep -q ":${port} "; then + return 0 + fi + sleep 1 + ((i+=1)) + done + return 1 +} + +wait_for_explore_stop_log() { + local logfile="$1" + local timeout="${2:-900}" + local i=0 + while (( i < timeout )); do + if [[ -f "$logfile" ]] && grep -Eq "All frontiers traversed/tried out, stopping\\.|Exploration stopped\\." "$logfile"; then + return 0 + fi + sleep 1 + ((i+=1)) + done + return 1 +} + +wait_for_explore_stop_log_with_timer() { + local logfile="$1" + local timeout="${2:-900}" + local start_ts + start_ts="$(date +%s)" + local i=0 + while (( i < timeout )); do + if [[ -f "$logfile" ]] && grep -Eq "All frontiers traversed/tried out, stopping\\.|Exploration stopped\\." "$logfile"; then + return 0 + fi + if (( i % 10 == 0 )); then + local now_ts elapsed + now_ts="$(date +%s)" + elapsed=$(( now_ts - start_ts )) + echo "[run_all] explore elapsed: ${elapsed}s" + fi + sleep 1 + ((i+=1)) + done + return 1 +} + +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 +} + +show_rviz_arg="show_rviz:=true" +if [[ "$NO_RVIZ" == true ]]; then + show_rviz_arg="show_rviz:=false" +fi + +if [[ "$MODE" == "mapping" ]]; then + start_bg "A_sim" \ + ros2 launch turtlebot3_gazebo turtlebot3_office_gz.launch.py \ + robot_model:="$ROBOT_MODEL" \ + use_sim_time:="$USE_SIM_TIME" \ + start_slam:=true \ + "$show_rviz_arg" \ + gz_partition:="$GZ_PARTITION" \ + gz_world_name:=default \ + world:="$WORLD_PATH" +else + start_bg "A_sim" \ + ros2 launch turtlebot3_gazebo turtlebot3_office_gz.launch.py \ + robot_model:="$ROBOT_MODEL" \ + use_sim_time:="$USE_SIM_TIME" \ + start_slam:=false \ + "$show_rviz_arg" \ + gz_partition:="$GZ_PARTITION" \ + gz_world_name:=default \ + world:="$WORLD_PATH" +fi + +# --gzweb：gz launch(9002) + 可选 office 内 gzweb-demo 的 Vite（5173）。 +# --gzweb --no-gzweb-vite：不起 office Vite；页面用 foundation 已在 /opt/gzweb:8000 起的实例（见 run_all_fix.sh）。 +if [[ "$WITH_GZWEB" == true ]]; then + export GZ_PARTITION + OFFICE_MODEL_ROOT="${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_office" + RESOURCE_PATHS=( + "${HOME}/.gazebo/models" + "${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/models" + "${OFFICE_MODEL_ROOT}" + ) + for d in "${OFFICE_MODEL_ROOT}"/*/materials/scripts "${OFFICE_MODEL_ROOT}"/*/materials/textures; do + [[ -d "${d}" ]] && RESOURCE_PATHS+=("${d}") + done + RESOURCE_PATH_JOINED="$(IFS=:; echo "${RESOURCE_PATHS[*]}")" + export GZ_SIM_RESOURCE_PATH="${RESOURCE_PATH_JOINED}" + # Keep compatibility with components resolving textures via legacy lookup vars. + export GZ_FILE_PATH="${RESOURCE_PATH_JOINED}" + export GAZEBO_RESOURCE_PATH="${RESOURCE_PATH_JOINED}" + + if [[ "$NO_GZWEB_VITE" != true ]]; then + if [[ ! -d "${ROOT}/gzweb-demo/gzweb-demo/node_modules/gzweb" ]]; then + echo "[run_all] gzweb demo dependencies missing, running npm install once..." + bash -lc "cd '${ROOT}/gzweb-demo/gzweb-demo' && npm install" + fi + else + echo "[run_all] --no-gzweb-vite: skipping office gzweb-demo npm install; use /opt/gzweb (port 8000) for UI." + fi + + kill_port_listeners 9002 + kill_port_listeners 5173 + + echo "[run_all] waiting for /clock before websocket..." + if wait_for_topic "/clock" 25; then + start_bg "D_websocket" gz launch "${ROOT}/websocket.sdf" + if [[ "$NO_GZWEB_VITE" != true ]]; then + start_bg "E_gzweb_frontend" bash -lc "cd '${ROOT}/gzweb-demo/gzweb-demo' && npm run dev:host" + echo "[run_all] office gzweb-demo Vite: http://localhost:5173" + fi + else + echo "[run_all] WARN: /clock not detected in time; starting websocket (and optional frontend) anyway." + start_bg "D_websocket" gz launch "${ROOT}/websocket.sdf" + if [[ "$NO_GZWEB_VITE" != true ]]; then + start_bg "E_gzweb_frontend" bash -lc "cd '${ROOT}/gzweb-demo/gzweb-demo' && npm run dev:host" + fi + fi + + if wait_for_port 9002 12; then + echo "[run_all] websocket server ready on ws://localhost:9002" + else + echo "[run_all] ERROR: websocket server failed on 9002; check ${LOG_DIR}/D_websocket.log" + fi +fi + +echo "[run_all] waiting ${SLEEP_NAV2}s before starting Nav2..." +sleep "$SLEEP_NAV2" + +if [[ "$MODE" == "mapping" ]]; then + start_bg "B_nav2" ros2 launch nav2_bringup navigation_launch.py use_sim_time:="$USE_SIM_TIME" params_file:=/workspace/nav2_params_fast.yaml +else + start_bg "B_nav2" \ + ros2 launch nav2_bringup bringup_launch.py params_file:=/workspace/nav2_params_fast.yaml \ + use_sim_time:="$USE_SIM_TIME" \ + map:="$MAP_YAML" \ + slam:=false +fi + +if [[ "$WITH_EXPLORE" == true && "$MODE" == "mapping" ]]; then + start_bg "C_explore" ros2 launch explore_lite explore.launch.py use_sim_time:="$USE_SIM_TIME" + last_index=$(( ${#PIDS[@]} - 1 )) + EXPLORE_PID="${PIDS[$last_index]}" + EXPLORE_LOG="${LOG_DIR}/C_explore.log" +fi + +echo +if [[ "$WITH_VISION" == true ]]; then + start_bg "D_vision_yolo" ros2 run vision_yolo detector + last_index=$(( ${#PIDS[@]} - 1 )) + VISION_PID="${PIDS[$last_index]}" + VISION_LOG="${LOG_DIR}/D_vision_yolo.log" +fi + +echo "[run_all] started. Mode: ${MODE}" +echo "[run_all] logs: ${LOG_DIR}" +echo + +if [[ "$MODE" == "mapping" ]]; then + if [[ "$WITH_GZWEB" == true ]]; then + if [[ "$NO_GZWEB_VITE" == true ]]; then + echo "[run_all] UI: foundation /opt/gzweb on port 8000 (no office Vite)." + elif wait_for_port 5173 25; then + echo "[run_all] office gzweb-demo ready: http://localhost:5173" + else + echo "[run_all] WARN: office gzweb frontend not ready yet, check ${LOG_DIR}/E_gzweb_frontend.log" + fi + fi + + if [[ "${WITH_EXPLORE}" == true && -n "${EXPLORE_PID:-}" ]]; then + echo "[run_all] exploration is running (pid=${EXPLORE_PID}), waiting for stop signal (timeout=${EXPLORE_WAIT_TIMEOUT}s)..." + if wait_for_explore_stop_log_with_timer "${EXPLORE_LOG}" "${EXPLORE_WAIT_TIMEOUT}"; then + echo "[run_all] detected exploration stop from log, finalizing explore process..." + else + echo "[run_all] WARN: explore stop not detected within timeout; forcing stop and saving current map." + fi + kill "${EXPLORE_PID}" 2>/dev/null || true + wait "${EXPLORE_PID}" 2>/dev/null || true + echo "[run_all] explore_lite stopped, now saving map." + fi + + if [[ "${WITH_EXPLORE}" == true ]]; then + echo "[run_all] auto-saving map to ${MAP_BASENAME}.yaml/.pgm ..." + ros2 run nav2_map_server map_saver_cli -f "$MAP_BASENAME" --ros-args -p use_sim_time:="$USE_SIM_TIME" + echo "[run_all] map saved successfully:" + echo " - ${MAP_BASENAME}.yaml" + echo " - ${MAP_BASENAME}.pgm" + echo "[run_all] mapping workflow completed, exiting now." + exit 0 + fi + + cat < + + + vision_yolo + 0.0.0 + TODO: Package description + devuser + TODO: License declaration + + rclpy + sensor_msgs + vision_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/vision_yolo/resource/vision_yolo b/src/vision_yolo/resource/vision_yolo new file mode 100644 index 0000000..e69de29 diff --git a/src/vision_yolo/setup.cfg b/src/vision_yolo/setup.cfg new file mode 100644 index 0000000..210487a --- /dev/null +++ b/src/vision_yolo/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/vision_yolo +[install] +install_scripts=$base/lib/vision_yolo diff --git a/src/vision_yolo/setup.py b/src/vision_yolo/setup.py new file mode 100644 index 0000000..0a3e814 --- /dev/null +++ b/src/vision_yolo/setup.py @@ -0,0 +1,30 @@ +from setuptools import find_packages, setup + +package_name = 'vision_yolo' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='devuser', + maintainer_email='devuser@todo.todo', + description='YOLOv8 vision detector for TurtleBot3', + license='MIT', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'detector = vision_yolo.detector:main', + ], + }, +) diff --git a/src/vision_yolo/test/test_copyright.py b/src/vision_yolo/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/vision_yolo/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/vision_yolo/test/test_flake8.py b/src/vision_yolo/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/vision_yolo/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/vision_yolo/test/test_pep257.py b/src/vision_yolo/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/vision_yolo/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/vision_yolo/vision_yolo/__init__.py b/src/vision_yolo/vision_yolo/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/vision_yolo/vision_yolo/detector.py b/src/vision_yolo/vision_yolo/detector.py new file mode 100644 index 0000000..35a79ad --- /dev/null +++ b/src/vision_yolo/vision_yolo/detector.py @@ -0,0 +1,83 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from vision_msgs.msg import Detection2DArray, Detection2D, ObjectHypothesisWithPose +from cv_bridge import CvBridge +from ultralytics import YOLO +import cv2 + +class YoloDetector(Node): + def __init__(self): + super().__init__('yolo_detector') + self.bridge = CvBridge() + self.get_logger().info('Loading YOLOv8n model...') + self.model = YOLO('yolov8n.pt') + self.get_logger().info('YOLOv8n model loaded') + + self.sub = self.create_subscription( + Image, '/camera/image_raw', self.on_image, 10) + self.pub = self.create_publisher( + Detection2DArray, '/vision/detections', 10) + self.pub_vis = self.create_publisher( + Image, '/vision/image_annotated', 10) + + self.get_logger().info('YOLO detector node started, subscribing to /camera/image_raw') + + def on_image(self, msg): + try: + cv_img = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + # CPU 降采样加速 + cv_img = cv2.resize(cv_img, (320, 240)) + except Exception as e: + self.get_logger().error(f'cv_bridge failed: {e}') + return + + results = self.model(cv_img, verbose=False, imgsz=320) + dets = Detection2DArray() + dets.header = msg.header + + for r in results: + for box in r.boxes: + cls_id = int(box.cls[0]) + conf = float(box.conf[0]) + label = self.model.names[cls_id] + + x1, y1, x2, y2 = map(float, box.xyxy[0]) + + det = Detection2D() + det.bbox.center.position.x = (x1 + x2) / 2.0 + det.bbox.center.position.y = (y1 + y2) / 2.0 + det.bbox.center.theta = 0.0 + det.bbox.size_x = x2 - x1 + det.bbox.size_y = y2 - y1 + + hyp = ObjectHypothesisWithPose() + hyp.hypothesis.class_id = label + hyp.hypothesis.score = conf + det.results.append(hyp) + dets.detections.append(det) + + cv2.rectangle(cv_img, (int(x1), int(y1)), (int(x2), int(y2)), (0, 255, 0), 2) + cv2.putText(cv_img, f'{label} {conf:.2f}', (int(x1), int(y1) - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) + + self.pub.publish(dets) + if dets.detections: + self.get_logger().info(f'Detected: {[d.results[0].hypothesis.class_id for d in dets.detections]}') + + vis_msg = self.bridge.cv2_to_imgmsg(cv_img, 'bgr8') + vis_msg.header = msg.header + self.pub_vis.publish(vis_msg) + +def main(args=None): + rclpy.init(args=args) + node = YoloDetector() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office_gz.launch.py b/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office_gz.launch.py index 20c92a4..ffda714 100644 --- a/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office_gz.launch.py +++ b/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office_gz.launch.py @@ -113,12 +113,16 @@ def _launch_setup(context, *args, **kwargs): f'{joint_gz}@sensor_msgs/msg/JointState[gz.msgs.Model', f'{tf_topic}@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V', '/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan', + '/camera@sensor_msgs/msg/Image@gz.msgs.Image', + '/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo', ], remappings=[ (joint_gz, '/joint_states'), (tf_topic, '/tf'), (cmd_topic, '/cmd_vel'), (odom_topic, '/odom'), + ('/camera', '/camera/image_raw'), + ('/camera_info', '/camera/camera_info'), ], parameters=[{ f'qos_overrides.{cmd_topic}.subscriber.reliability': 'reliable', diff --git a/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_waffle/model_gz.sdf b/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_waffle/model_gz.sdf index 486743b..b5bfa00 100755 --- a/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_waffle/model_gz.sdf +++ b/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_waffle/model_gz.sdf @@ -432,10 +432,6 @@ - - base_link - realsense_link - wheel_left_joint @@ -458,6 +454,37 @@ + + 0.064 -0.065 0.094 0 0 0 + + 0.01 + + 0.0010.0010.001 + + + + + 1.047 + + 640 + 480 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + + + base_link + camera_link + diff --git a/update_requirements.sh b/update_requirements.sh new file mode 100755 index 0000000..94f62fd --- /dev/null +++ b/update_requirements.sh @@ -0,0 +1,15 @@ +#!/usr/bin/env bash +# 安装 vision_yolo 依赖到 /workspace/.site-packages +# 容器重启后系统盘重置，但 /workspace 是 EFS 持久化存储 +set -euo pipefail + +TARGET="/workspace/.site-packages" +mkdir -p "$TARGET" + +pip install ultralytics opencv-python "numpy<2" \ + -t "$TARGET" \ + -i https://pypi.tuna.tsinghua.edu.cn/simple \ + --no-cache-dir + +echo "Dependencies installed to $TARGET" +echo "PYTHONPATH is already set in run_all.sh"