diff --git a/README.md b/README.md index a1dedc1..b7edde6 100644 --- a/README.md +++ b/README.md @@ -1,18 +1,93 @@ -# 开始拉取代码 +* 语义推理与导航 +智能语义推理框架包含两个核心模块： +语义目标标注：利用 VLM识别物体 +语义导航：机器人借助已保存的物体记忆，通过语义路标完成导航。 + +语义标注模块将 Faster R-CNN 与 CLIP 结合，实现零样本目标检测。 + +给定一个物体列表（例如：["Refrigerator", "water dispenser", "sofa", "white toilet", "office chair with wheels"]），由 Faster R-CNN 从 RGB 图像帧中生成候选区域。 + +随后，使用 CLIP 将每个候选区域进行特征编码，并与文本标签做余弦相似度对比，为区域分配最可能的类别标签。 + +点击 RViz2 中Nav2 Goal，在地图上选择目标位置，调整箭头设置机器人到达后的朝向，确认后机器人将自主导航，在探索过程中，系统会结合 RGB-D 数据 记录物体的中心坐标，并将置信度分数与位置信息存入 JSON 文件。 +只有当检测到更高置信度的结果时，才会更新原有记录。 + +## 1. 环境要求 +- Ubuntu 22.04 +- ROS2 Humble +- Gazebo Sim Harmonic --- -``` -git clone http://git-test.databall.tech:3000/hq/ros2_office_vlm.git -cd ros2_office_vlm +### 编译 -# 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/ros2_office_vlm.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. 启动仿真， Nav2导航栈， AMCL节点，rviz + +```bash +./gzsim_run.sh + +ros2 launch nav2_rrtstar_planner bringup_localization_with_initial_pose.launch.py \ + use_sim_time:=true \ + map:=$NAV2_MAP_PATH + +ros2 launch nav2_bringup navigation_launch.py \ + use_sim_time:=True \ + params_file:=/workspace/src/TurtleBot-RRT-Star/nav2_params.yaml \ + 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 + +``` +2. 新开终端，执行环境生效后启动vlm_perception_pkg的语义检测节点，开始目标语义标注并构建语义记忆库，在机器人导航过程中，节点实时检测环境中的目标并生成语义标注结果，保存JSON 格式的语义记忆库到指定路径。 +```bash +ros2 run vlm_perception_pkg vlm_detection +# ros2 run vlm_perception_pkg vlm_detection --ros-args -p use_sim_time:=true -p reset_records_on_startup:=true # 重置检测记录 +``` +3. 加载生成的语义记忆库文件（in /workspace/src/vlm_nav_pkg/config），通过 CLIP 将语义标签编码为shared embedding space，当用户输入自然语言查询时，系统匹配最接近的已知目标，提取其坐标并发送至 Nav2，实现基于自然语言的语义导航。以下是验证步骤： +4. 重新启动仿真， Nav2导航栈， AMCL节点，rviz +```bash + +./gzsim_run.sh + +ros2 launch nav2_rrtstar_planner bringup_localization_with_initial_pose.launch.py \ + use_sim_time:=true \ + map:=$NAV2_MAP_PATH + +ros2 launch nav2_bringup navigation_launch.py \ + use_sim_time:=True \ + params_file:=/workspace/src/TurtleBot-RRT-Star/nav2_params.yaml \ + 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 + +pip3 install "numpy<2" +ros2 run vlm_nav_pkg semantic_nav +``` + +节点启动后，在终端输入自然语言语义目标（如 “water dispenser”“sofa”），系统将自动完成：语义查询与shared embedding space匹配，提取目标对应的地图坐标并向 Nav2 发送导航目标位姿，然后机器人自主规划路径并导航至语义目标位置，RViz2 中实时可视化规划路径与导航过程 diff --git a/assets/picture07.png b/assets/picture07.png new file mode 100644 index 0000000..ed383af Binary files /dev/null and b/assets/picture07.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/turtlebot3_simulations/.DS_Store b/src/turtlebot3_simulations/.DS_Store new file mode 100644 index 0000000..91ce61e Binary files /dev/null and b/src/turtlebot3_simulations/.DS_Store differ diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/.DS_Store b/src/turtlebot3_simulations/turtlebot3_gazebo/.DS_Store new file mode 100644 index 0000000..9ee7680 Binary files /dev/null and b/src/turtlebot3_simulations/turtlebot3_gazebo/.DS_Store differ 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 +