├── .colcon ├── build │ └── .gitkeep ├── log │ └── .gitkeep └── install │ └── .gitkeep ├── tb_autonomy ├── python │ └── tb_behaviors │ │ ├── __init__.py │ │ ├── vision.py │ │ └── navigation.py ├── package.xml ├── bt_xml │ ├── nav_tree_naive.xml │ ├── nav_tree_queue.xml │ ├── tree_queue.xml │ └── tree_naive.xml ├── include │ ├── vision_behaviors.h │ └── navigation_behaviors.h ├── CMakeLists.txt ├── scripts │ ├── test_move_base.py │ ├── test_vision.py │ └── autonomy_node.py ├── launch │ ├── tb_demo_behavior_py.launch.py │ └── tb_demo_behavior_cpp.launch.py └── src │ ├── vision_behaviors.cpp │ ├── autonomy_node.cpp │ └── navigation_behaviors.cpp ├── .gitignore ├── media ├── demo_screenshot_cpp.png └── demo_screenshot_python.png ├── tb_worlds ├── maps │ ├── sim_house_map.pgm │ ├── sim_house_map.yaml │ └── sim_house_locations.yaml ├── models │ ├── red_block │ │ ├── model.config │ │ └── model.sdf │ ├── sim_house │ │ ├── model.config │ │ └── model.sdf │ ├── blue_block │ │ ├── model.config │ │ └── model.sdf │ └── green_block │ │ ├── model.config │ │ └── model.sdf ├── CMakeLists.txt ├── package.xml ├── configs │ ├── turtlebot3_bridge.yaml │ ├── turtlebot4_bridge.yaml │ └── nav2_params.yaml ├── launch │ ├── block_spawner.launch.py │ ├── turtlebot_spawner.launch.py │ ├── tb_demo_world.launch.py │ └── tb_world.launch.py ├── urdf │ └── gz_waffle.sdf.xacro └── worlds │ └── sim_house.sdf.xacro ├── dependencies.repos ├── .env ├── docker ├── entrypoint.sh ├── run_docker.sh └── Dockerfile ├── .github └── workflows │ └── docker.yml ├── LICENSE ├── .pre-commit-config.yaml ├── docker-compose.yaml └── README.md /.colcon/build/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.colcon/log/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.colcon/install/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /tb_autonomy/python/tb_behaviors/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .colcon/ 2 | .vscode/ 3 | **/__pycache__/ 4 | -------------------------------------------------------------------------------- /media/demo_screenshot_cpp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/turtlebot3_behavior_demos/HEAD/media/demo_screenshot_cpp.png -------------------------------------------------------------------------------- /media/demo_screenshot_python.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/turtlebot3_behavior_demos/HEAD/media/demo_screenshot_python.png -------------------------------------------------------------------------------- /tb_worlds/maps/sim_house_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/turtlebot3_behavior_demos/HEAD/tb_worlds/maps/sim_house_map.pgm -------------------------------------------------------------------------------- /tb_worlds/maps/sim_house_map.yaml: -------------------------------------------------------------------------------- 1 | image: sim_house_map.pgm 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-2.07, -1.64, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 8 | -------------------------------------------------------------------------------- /tb_worlds/maps/sim_house_locations.yaml: -------------------------------------------------------------------------------- 1 | # Defines object locations as [x, y, theta] 2 | location1: [-1.0, -0.5, -2.25] 3 | location2: [0.5, 4.0, 0.785] 4 | location3: [4.0, 0.5, 1.571] 5 | location4: [2.75, 2.5, -1.571] 6 | -------------------------------------------------------------------------------- /dependencies.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | # Simulation and Nav2 support for TurtleBot robots 3 | nav2_minimal_turtlebot_simulation: 4 | type: git 5 | url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git 6 | version: main 7 | -------------------------------------------------------------------------------- /tb_worlds/models/red_block/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | red_block 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /tb_worlds/models/sim_house/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | sim_house 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /tb_worlds/models/blue_block/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | blue_block 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /tb_worlds/models/green_block/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | green_block 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /.env: -------------------------------------------------------------------------------- 1 | # Default environment variables for Docker Compose file 2 | 3 | # The ROS distribution to use (tested on Jazzy and Rolling) 4 | ROS_DISTRO=jazzy 5 | 6 | # TurtleBot model (3 or 4) 7 | TURTLEBOT_MODEL=3 8 | 9 | # Behavior tree type: Can be naive or queue. 10 | BT_TYPE=queue 11 | 12 | # Set to true to use vision, else false to only do navigation behaviors. 13 | ENABLE_VISION=true 14 | 15 | # Target color for vision: Can be red, green, or blue. 16 | TARGET_COLOR=blue 17 | -------------------------------------------------------------------------------- /tb_worlds/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(tb_worlds) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # Find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclpy REQUIRED) 11 | find_package(geometry_msgs REQUIRED) 12 | find_package(nav2_minimal_tb3_sim REQUIRED) 13 | find_package(nav2_minimal_tb4_sim REQUIRED) 14 | 15 | # Install directories 16 | install(DIRECTORY 17 | launch maps models worlds urdf configs 18 | DESTINATION share/${PROJECT_NAME} 19 | ) 20 | 21 | ament_package() 22 | -------------------------------------------------------------------------------- /docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Basic entrypoint for ROS Docker containers 3 | 4 | # Source ROS 2 5 | source /opt/ros/${ROS_DISTRO}/setup.bash 6 | echo "Sourced ROS 2 ${ROS_DISTRO}" 7 | 8 | # Source the base workspace, if built 9 | if [ -f /turtlebot_ws/install/setup.bash ] 10 | then 11 | source /turtlebot_ws/install/setup.bash 12 | echo "Sourced TurtleBot base workspace" 13 | fi 14 | 15 | # Source the overlay workspace, if built 16 | if [ -f /overlay_ws/install/setup.bash ] 17 | then 18 | source /overlay_ws/install/setup.bash 19 | echo "Sourced autonomy overlay workspace" 20 | fi 21 | 22 | # Execute the command passed into this entrypoint 23 | exec "$@" 24 | -------------------------------------------------------------------------------- /tb_worlds/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tb_worlds 4 | 0.0.0 5 | Simple world examples for TurtleBot simulation. 6 | 7 | Sebastian Castro 8 | MIT 9 | 10 | ament_cmake 11 | 12 | rclpy 13 | geometry_msgs 14 | nav2_bringup 15 | nav2_minimal_tb3_sim 16 | nav2_minimal_tb4_sim 17 | rviz2 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /docker/run_docker.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Sample script to run a command in a Docker container 3 | # 4 | # Usage Example: 5 | # ./run_docker.sh turtlebot_behavior:overlay "ros2 launch tb_worlds tb_demo_world.launch.py" 6 | 7 | # Define Docker volumes and environment variables 8 | DOCKER_VOLUMES=" 9 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 10 | --volume="${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority" \ 11 | " 12 | DOCKER_ENV_VARS=" 13 | --env="TURTLEBOT_MODEL=3" \ 14 | --env="DISPLAY" \ 15 | --env="QT_X11_NO_MITSHM=1" \ 16 | --env="NVIDIA_DRIVER_CAPABILITIES=all" \ 17 | " 18 | DOCKER_ARGS=${DOCKER_VOLUMES}" "${DOCKER_ENV_VARS} 19 | 20 | # Run the command 21 | docker run -it --net=host --ipc=host --privileged ${DOCKER_ARGS} "$1" bash -c "$2" 22 | -------------------------------------------------------------------------------- /.github/workflows/docker.yml: -------------------------------------------------------------------------------- 1 | name: build_docker 2 | 3 | on: 4 | # Run action on certain pull request events 5 | pull_request: 6 | types: [opened, synchronize, reopened, ready_for_review] 7 | 8 | # Nightly job on default (main) branch 9 | schedule: 10 | - cron: '0 0 * * *' 11 | 12 | # Whenever certain branches are pushed 13 | push: 14 | branches: [main, humble] 15 | 16 | # On demand from github.com for testing 17 | workflow_dispatch: 18 | 19 | jobs: 20 | # Build docker images 21 | docker: 22 | runs-on: ubuntu-latest 23 | steps: 24 | - name: Check out repository 25 | uses: actions/checkout@v4 26 | - name: Set up Docker Buildx 27 | uses: docker/setup-buildx-action@v3 28 | - name: Build and push 29 | uses: docker/build-push-action@v5 30 | with: 31 | file: docker/Dockerfile 32 | push: false 33 | -------------------------------------------------------------------------------- /tb_autonomy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tb_autonomy 4 | 0.0.0 5 | Simple autonomy examples for TurtleBot 6 | 7 | Sebastian Castro 8 | MIT 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | rclpy 14 | rclcpp 15 | rclcpp_action 16 | ament_index_cpp 17 | behaviortree_cpp 18 | nav2_msgs 19 | py_trees_ros 20 | py_trees_ros_viewer 21 | sensor_msgs 22 | tb_worlds 23 | tf2 24 | tf2_ros 25 | tf2_geometry_msgs 26 | yaml-cpp 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /tb_worlds/models/red_block/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0.05 0 0 0 5 | true 6 | 7 | 8 | 1.0 9 | 10 | 0.01 11 | 0.0 12 | 0.0 13 | 0.01 14 | 0.0 15 | 0.01 16 | 17 | 18 | 19 | 20 | 21 | 0.1 0.1 0.1 22 | 23 | 24 | 25 | 26 | 27 | 28 | 0.1 0.1 0.1 29 | 30 | 31 | 32 | 0.8 0.0 0.1 1 33 | 0.8 0.0 0.2 1 34 | 0 0 0 0 35 | 0 0 0 1 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021-2024 Sebastian Castro 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /tb_worlds/models/blue_block/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0.05 0 0 0 5 | true 6 | 7 | 8 | 1.0 9 | 10 | 0.01 11 | 0.0 12 | 0.0 13 | 0.01 14 | 0.0 15 | 0.01 16 | 17 | 18 | 19 | 20 | 21 | 0.1 0.1 0.1 22 | 23 | 24 | 25 | 26 | 27 | 28 | 0.1 0.1 0.1 29 | 30 | 31 | 32 | 0.0 0.0 0.8 1 33 | 0.0 0.0 0.8 1 34 | 0 0 0 0 35 | 0 0 0 1 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /tb_worlds/models/green_block/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0.05 0 0 0 5 | true 6 | 7 | 8 | 1.0 9 | 10 | 0.01 11 | 0.0 12 | 0.0 13 | 0.01 14 | 0.0 15 | 0.01 16 | 17 | 18 | 19 | 20 | 21 | 0.1 0.1 0.1 22 | 23 | 24 | 25 | 26 | 27 | 28 | 0.1 0.1 0.1 29 | 30 | 31 | 32 | 0.0 0.7 0.0 1 33 | 0.0 0.7 0.0 1 34 | 0 0 0 0 35 | 0 0 0 1 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /tb_autonomy/bt_xml/nav_tree_naive.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /tb_autonomy/include/vision_behaviors.h: -------------------------------------------------------------------------------- 1 | // Vision behaviors for TurtleBot 2 | 3 | #include 4 | 5 | #include "rclcpp/rclcpp.hpp" 6 | #include "image_transport/image_transport.hpp" 7 | #include "sensor_msgs/msg/image.hpp" 8 | #include "behaviortree_cpp/behavior_tree.h" 9 | 10 | // HSV Thresholding parameters 11 | // The convention is {H_MIN, H_MAX, S_MIN, S_MAX, V_MIN, V_MAX} 12 | typedef std::map> ColorThresholdMap; 13 | const ColorThresholdMap hsv_threshold_dict = { 14 | {"red", {160, 180, 220, 255, 0, 255}}, 15 | {"green", {40, 90, 220, 255, 0, 255}}, 16 | {"blue", {100, 150, 220, 255, 0, 255}}, 17 | }; 18 | 19 | // Look for an object of a particular color 20 | class LookForObject : public BT::StatefulActionNode 21 | { 22 | public: 23 | 24 | rclcpp::Node::SharedPtr node_ptr_; 25 | bool received_image_; 26 | sensor_msgs::msg::Image::ConstSharedPtr latest_image_ptr_; 27 | image_transport::Subscriber image_sub_; 28 | 29 | // Method overrides 30 | LookForObject(const std::string& name, const BT::NodeConfig& config, 31 | rclcpp::Node::SharedPtr node_ptr); 32 | BT::NodeStatus onStart() override; 33 | BT::NodeStatus onRunning() override; 34 | void onHalted() override; 35 | static BT::PortsList providedPorts() { return {}; }; 36 | 37 | // Image subscriber callback 38 | void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg); 39 | }; 40 | -------------------------------------------------------------------------------- /tb_autonomy/bt_xml/nav_tree_queue.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /tb_autonomy/bt_xml/tree_queue.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /tb_worlds/configs/turtlebot3_bridge.yaml: -------------------------------------------------------------------------------- 1 | # This file has been modified from 2 | # https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation/blob/091b5ff12436890a54de6325df3573ae110e912b/nav2_minimal_tb3_sim/configs/turtlebot3_waffle_bridge.yaml 3 | # Modification: 4 | # - Added camera bridge 5 | 6 | # replace clock_bridge 7 | - topic_name: "/clock" 8 | ros_type_name: "rosgraph_msgs/msg/Clock" 9 | gz_type_name: "gz.msgs.Clock" 10 | direction: GZ_TO_ROS 11 | 12 | # no equivalent in TB3 - add 13 | - topic_name: "joint_states" 14 | ros_type_name: "sensor_msgs/msg/JointState" 15 | gz_type_name: "gz.msgs.Model" 16 | direction: GZ_TO_ROS 17 | 18 | # replace odom_bridge - check gz topic name 19 | # gz topic published by DiffDrive plugin 20 | - topic_name: "odom" 21 | ros_type_name: "nav_msgs/msg/Odometry" 22 | gz_type_name: "gz.msgs.Odometry" 23 | direction: GZ_TO_ROS 24 | 25 | # replace odom_tf_bridge - check gz and ros topic names 26 | # gz topic published by DiffDrive plugin 27 | # prefix ros2 topic with gz 28 | - topic_name: "tf" 29 | ros_type_name: "tf2_msgs/msg/TFMessage" 30 | gz_type_name: "gz.msgs.Pose_V" 31 | direction: GZ_TO_ROS 32 | 33 | # replace imu_bridge - check gz topic name 34 | - topic_name: "imu" 35 | ros_type_name: "sensor_msgs/msg/Imu" 36 | gz_type_name: "gz.msgs.IMU" 37 | direction: GZ_TO_ROS 38 | 39 | # replace lidar_bridge 40 | - topic_name: "scan" 41 | ros_type_name: "sensor_msgs/msg/LaserScan" 42 | gz_type_name: "gz.msgs.LaserScan" 43 | direction: GZ_TO_ROS 44 | 45 | # replace cmd_vel_bridge 46 | - topic_name: "cmd_vel" 47 | ros_type_name: "geometry_msgs/msg/TwistStamped" 48 | gz_type_name: "gz.msgs.Twist" 49 | direction: ROS_TO_GZ 50 | 51 | # replace image_bridge 52 | - ros_topic_name: "/camera/image_raw" 53 | gz_topic_name: "/realsense_camera" 54 | ros_type_name: "sensor_msgs/msg/Image" 55 | gz_type_name: "gz.msgs.Image" 56 | direction: GZ_TO_ROS 57 | -------------------------------------------------------------------------------- /tb_autonomy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(tb_autonomy) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # Find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_python REQUIRED) 11 | find_package(ament_index_cpp REQUIRED) 12 | find_package(rclpy REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(rclcpp_action REQUIRED) 15 | find_package(nav2_msgs REQUIRED) 16 | find_package(sensor_msgs REQUIRED) 17 | find_package(behaviortree_cpp REQUIRED) 18 | find_package(cv_bridge REQUIRED) 19 | find_package(OpenCV REQUIRED) 20 | find_package(image_transport REQUIRED) 21 | find_package(tf2 REQUIRED) 22 | find_package(tf2_ros REQUIRED) 23 | find_package(tf2_geometry_msgs REQUIRED) 24 | find_package(yaml-cpp REQUIRED) 25 | 26 | # Install directories 27 | install(DIRECTORY 28 | bt_xml launch 29 | DESTINATION share/${PROJECT_NAME} 30 | ) 31 | 32 | # Install Python package 33 | ament_python_install_package( 34 | tb_behaviors 35 | PACKAGE_DIR python/tb_behaviors) 36 | 37 | # Install Python scripts 38 | install(PROGRAMS 39 | scripts/autonomy_node.py 40 | scripts/test_move_base.py 41 | scripts/test_vision.py 42 | DESTINATION lib/${PROJECT_NAME} 43 | ) 44 | 45 | # Install C++ nodes 46 | set(AUTONOMY_SOURCES 47 | src/navigation_behaviors.cpp 48 | src/vision_behaviors.cpp 49 | ) 50 | set(TARGET_DEPENDS 51 | ament_index_cpp rclcpp rclcpp_action 52 | nav2_msgs sensor_msgs cv_bridge image_transport 53 | behaviortree_cpp tf2 tf2_ros tf2_geometry_msgs yaml-cpp 54 | ) 55 | include_directories(include) 56 | add_executable(autonomy_node_cpp src/autonomy_node.cpp ${AUTONOMY_SOURCES}) 57 | install(TARGETS 58 | autonomy_node_cpp 59 | DESTINATION lib/${PROJECT_NAME} 60 | ) 61 | ament_target_dependencies(autonomy_node_cpp ${TARGET_DEPENDS}) 62 | target_link_libraries(autonomy_node_cpp ${OpenCV_LIBS} ${YAML_CPP_LIBRARIES}) 63 | 64 | ament_package() 65 | -------------------------------------------------------------------------------- /tb_autonomy/scripts/test_move_base.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Script that tests the `navigate_to_pose` action client to move the robot base. 5 | 6 | Example usage: 7 | Python: python3 test_move_base.py --x 0 --y 2 --theta 1.57 8 | ros2: ros2 run tb_autonomy test_move_base.py --x 0 --y 2 --theta 1.57 9 | """ 10 | 11 | import argparse 12 | import rclpy 13 | from rclpy.action import ActionClient 14 | from rclpy.node import Node 15 | import transforms3d 16 | from nav2_msgs.action import NavigateToPose 17 | 18 | 19 | class MoveBaseClient(Node): 20 | def __init__(self): 21 | super().__init__("move_base_client") 22 | self.cli = ActionClient(self, NavigateToPose, "/navigate_to_pose") 23 | self.get_logger().info("Move base test node started") 24 | 25 | def send_pose_goal(self, x, y, theta): 26 | self.get_logger().info(f"Going to [x: {x}, y: {y}, theta: {theta}] ...") 27 | goal_msg = NavigateToPose.Goal() 28 | goal_msg.pose.pose.position.x = x 29 | goal_msg.pose.pose.position.y = y 30 | quat = transforms3d.euler.euler2quat(0, 0, theta) 31 | goal_msg.pose.pose.orientation.w = quat[0] 32 | goal_msg.pose.pose.orientation.x = quat[1] 33 | goal_msg.pose.pose.orientation.y = quat[2] 34 | goal_msg.pose.pose.orientation.z = quat[3] 35 | self.cli.wait_for_server() 36 | return self.cli.send_goal_async(goal_msg) 37 | 38 | 39 | if __name__ == "__main__": 40 | # Parse command-line arguments 41 | parser = argparse.ArgumentParser(description="Move base test script") 42 | parser.add_argument("--x", type=float, default=1.0) 43 | parser.add_argument("--y", type=float, default=0.0) 44 | parser.add_argument("--theta", type=float, default=0.0) 45 | args = parser.parse_args() 46 | 47 | # Start ROS node and action client 48 | rclpy.init() 49 | client = MoveBaseClient() 50 | 51 | # Send goal to the move_base action server 52 | future = client.send_pose_goal(args.x, args.y, args.theta) 53 | rclpy.spin_until_future_complete(client, future) 54 | client.get_logger("Pose goal reached.") 55 | -------------------------------------------------------------------------------- /tb_worlds/configs/turtlebot4_bridge.yaml: -------------------------------------------------------------------------------- 1 | # This file has been modified from 2 | # https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation/blob/091b5ff12436890a54de6325df3573ae110e912b/nav2_minimal_tb4_sim/configs/tb4_bridge.yaml 3 | # Modification: 4 | # - Added camera bridge 5 | 6 | # replace clock_bridge 7 | - ros_topic_name: "/clock" 8 | gz_topic_name: "/clock" 9 | ros_type_name: "rosgraph_msgs/msg/Clock" 10 | gz_type_name: "gz.msgs.Clock" 11 | direction: GZ_TO_ROS 12 | 13 | # no equivalent in TB3 - add 14 | - ros_topic_name: "joint_states" 15 | gz_topic_name: "joint_states" 16 | ros_type_name: "sensor_msgs/msg/JointState" 17 | gz_type_name: "gz.msgs.Model" 18 | direction: GZ_TO_ROS 19 | 20 | # replace odom_bridge - check gz topic name 21 | # gz topic published by DiffDrive plugin 22 | - ros_topic_name: "odom" 23 | gz_topic_name: "/odom" 24 | ros_type_name: "nav_msgs/msg/Odometry" 25 | gz_type_name: "gz.msgs.Odometry" 26 | direction: GZ_TO_ROS 27 | 28 | # replace odom_tf_bridge - check gz and ros topic names 29 | # gz topic published by DiffDrive plugin 30 | # prefix ros2 topic with gz 31 | - ros_topic_name: "tf" 32 | gz_topic_name: "/tf" 33 | ros_type_name: "tf2_msgs/msg/TFMessage" 34 | gz_type_name: "gz.msgs.Pose_V" 35 | direction: GZ_TO_ROS 36 | 37 | # replace imu_bridge - check gz topic name 38 | - ros_topic_name: "imu" 39 | gz_topic_name: "/imu" 40 | ros_type_name: "sensor_msgs/msg/Imu" 41 | gz_type_name: "gz.msgs.IMU" 42 | direction: GZ_TO_ROS 43 | 44 | # replace lidar_bridge 45 | - ros_topic_name: "scan" 46 | gz_topic_name: "/scan" 47 | ros_type_name: "sensor_msgs/msg/LaserScan" 48 | gz_type_name: "gz.msgs.LaserScan" 49 | direction: GZ_TO_ROS 50 | 51 | # replace cmd_vel_bridge 52 | - ros_topic_name: "cmd_vel" 53 | gz_topic_name: "/cmd_vel" 54 | ros_type_name: "geometry_msgs/msg/TwistStamped" 55 | gz_type_name: "gz.msgs.Twist" 56 | direction: ROS_TO_GZ 57 | 58 | # replace image_bridge 59 | - ros_topic_name: "/camera/image_raw" 60 | gz_topic_name: "/rgbd_camera/image" 61 | ros_type_name: "sensor_msgs/msg/Image" 62 | gz_type_name: "gz.msgs.Image" 63 | direction: GZ_TO_ROS 64 | -------------------------------------------------------------------------------- /tb_autonomy/bt_xml/tree_naive.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /tb_worlds/launch/block_spawner.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import math 3 | import yaml 4 | import random 5 | from ament_index_python.packages import get_package_share_directory 6 | 7 | from launch import LaunchDescription 8 | from launch.actions import IncludeLaunchDescription 9 | from launch.launch_description_sources import PythonLaunchDescriptionSource 10 | 11 | 12 | def generate_launch_description(): 13 | lds = [] # Launch Descriptions 14 | pkg_dir = get_package_share_directory("tb_worlds") 15 | 16 | # Parse locations YAML file 17 | yaml_config_path = os.path.join(pkg_dir, "maps", "sim_house_locations.yaml") 18 | with open(yaml_config_path, "r") as f: 19 | locations = yaml.load(f, Loader=yaml.FullLoader) 20 | 21 | # Offset between Gazebo world and map frame (can we fix this somehow else?) 22 | block_spawn_offset = [-0.6, -0.6] 23 | 24 | # Distance from navigation waypoint to spawn object 25 | offset_from_robot = 0.6 26 | 27 | # Spawn blocks in random locations 28 | model_names = ["red_block", "green_block", "blue_block"] 29 | sampled_locs = random.sample(list(locations.keys()), len(model_names)) 30 | for mdl_name, loc in zip(model_names, sampled_locs): 31 | x, y, theta = locations[loc] 32 | x += block_spawn_offset[0] + offset_from_robot * math.cos(theta) 33 | y += block_spawn_offset[1] + offset_from_robot * math.sin(theta) 34 | 35 | mdl_sdf = os.path.join(pkg_dir, "models", mdl_name, "model.sdf") 36 | lds.append( 37 | IncludeLaunchDescription( 38 | PythonLaunchDescriptionSource( 39 | os.path.join( 40 | get_package_share_directory("ros_gz_sim"), 41 | "launch", 42 | "gz_spawn_model.launch.py", 43 | ) 44 | ), 45 | launch_arguments={ 46 | "world": "", 47 | "file": mdl_sdf, 48 | "name": mdl_name, 49 | "x": str(x), 50 | "y": str(y), 51 | "z": "0.06", 52 | "Y": str(theta), 53 | }.items(), 54 | ) 55 | ) 56 | 57 | return LaunchDescription(lds) 58 | -------------------------------------------------------------------------------- /tb_autonomy/launch/tb_demo_behavior_py.launch.py: -------------------------------------------------------------------------------- 1 | from os.path import join 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import DeclareLaunchArgument, ExecuteProcess 6 | from launch.substitutions import LaunchConfiguration, TextSubstitution 7 | from launch_ros.actions import Node 8 | 9 | 10 | def generate_launch_description(): 11 | pkg_tb_worlds = get_package_share_directory("tb_worlds") 12 | default_world_dir = join(pkg_tb_worlds, "maps", "sim_house_locations.yaml") 13 | 14 | return LaunchDescription( 15 | [ 16 | # Arguments 17 | DeclareLaunchArgument( 18 | "location_file", 19 | default_value=TextSubstitution(text=default_world_dir), 20 | description="YAML file name containing map locations in the world.", 21 | ), 22 | DeclareLaunchArgument( 23 | "target_color", 24 | default_value=TextSubstitution(text="blue"), 25 | description="Target object color (red, green, or blue)", 26 | ), 27 | DeclareLaunchArgument( 28 | "tree_type", 29 | default_value=TextSubstitution(text="queue"), 30 | description="Behavior tree type (naive or queue)", 31 | ), 32 | DeclareLaunchArgument( 33 | "enable_vision", 34 | default_value=TextSubstitution(text="True"), 35 | description="Enable vision behaviors. If false, do navigation only.", 36 | ), 37 | # Main autonomy node 38 | Node( 39 | package="tb_autonomy", 40 | executable="autonomy_node.py", 41 | name="autonomy_node_python", 42 | output="screen", 43 | emulate_tty=True, 44 | parameters=[ 45 | { 46 | "location_file": LaunchConfiguration("location_file"), 47 | "target_color": LaunchConfiguration("target_color"), 48 | "tree_type": LaunchConfiguration("tree_type"), 49 | "enable_vision": LaunchConfiguration("enable_vision"), 50 | } 51 | ], 52 | ), 53 | # Behavior tree visualization 54 | ExecuteProcess(cmd=["py-trees-tree-viewer", "--no-sandbox"]), 55 | ] 56 | ) 57 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # To use: 2 | # 3 | # pre-commit run -a 4 | # 5 | # Or: 6 | # 7 | # pre-commit install # (runs every time you commit in git) 8 | # 9 | # To update this file: 10 | # 11 | # pre-commit autoupdate 12 | # 13 | # See https://github.com/pre-commit/pre-commit 14 | repos: 15 | # Standard hooks 16 | - repo: https://github.com/pre-commit/pre-commit-hooks 17 | rev: v6.0.0 18 | hooks: 19 | - id: check-ast 20 | - id: check-case-conflict 21 | - id: check-docstring-first 22 | - id: check-merge-conflict 23 | - id: check-symlinks 24 | - id: check-yaml 25 | args: ["--unsafe"] # Fixes errors parsing custom YAML constructors like ur_description's !degrees 26 | - id: debug-statements 27 | - id: end-of-file-fixer 28 | - id: mixed-line-ending 29 | - id: fix-byte-order-marker 30 | 31 | - repo: https://github.com/psf/black-pre-commit-mirror 32 | rev: 25.12.0 33 | hooks: 34 | - id: black 35 | 36 | - repo: https://github.com/codespell-project/codespell 37 | rev: v2.4.1 38 | hooks: 39 | - id: codespell 40 | args: ["--write-changes", "-L", "atleast,inout,ether"] # Provide a comma-separated list of misspelled words that codespell should ignore (for example: '-L', 'word1,word2,word3'). 41 | exclude: \.(svg|pyc|stl|dae|lock)$ 42 | 43 | - repo: https://github.com/pre-commit/mirrors-clang-format 44 | rev: v21.1.8 45 | hooks: 46 | - id: clang-format 47 | files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|m|proto|vert)$ 48 | # -i arg is included by default by the hook 49 | args: ["-fallback-style=none"] 50 | 51 | - repo: https://github.com/adrienverge/yamllint 52 | rev: v1.37.1 53 | hooks: 54 | - id: yamllint 55 | args: 56 | [ 57 | "--no-warnings", 58 | "--config-data", 59 | "{extends: default, rules: {line-length: disable, braces: {max-spaces-inside: 1}}}", 60 | ] 61 | types: [text] 62 | files: \.(yml|yaml)$ 63 | 64 | - repo: https://github.com/tcort/markdown-link-check 65 | rev: v3.14.2 66 | hooks: 67 | - id: markdown-link-check 68 | 69 | ci: 70 | autofix_commit_msg: | 71 | [pre-commit.ci] auto fixes from pre-commit.com hooks 72 | for more information, see https://pre-commit.ci 73 | autofix_prs: true 74 | autoupdate_branch: '' 75 | autoupdate_commit_msg: '[pre-commit.ci] pre-commit autoupdate' 76 | autoupdate_schedule: monthly 77 | skip: ['markdown-link-check'] 78 | submodules: false 79 | -------------------------------------------------------------------------------- /tb_autonomy/include/navigation_behaviors.h: -------------------------------------------------------------------------------- 1 | // Navigation behaviors for TurtleBot 2 | 3 | #include "rclcpp/rclcpp.hpp" 4 | #include "rclcpp_action/rclcpp_action.hpp" 5 | #include "nav2_msgs/action/navigate_to_pose.hpp" 6 | #include "behaviortree_cpp/behavior_tree.h" 7 | #include "yaml-cpp/yaml.h" 8 | 9 | // Struct to keep location pose data 10 | struct Pose{ 11 | double x; 12 | double y; 13 | double theta; 14 | }; 15 | 16 | // YAML parsing template specialization for the Pose type, which is used to parse locations directly 17 | namespace YAML { 18 | template<> 19 | struct convert { 20 | static bool decode(const Node& node, Pose& pose) { 21 | if (!node.IsSequence() || node.size() != 3) { 22 | return false; 23 | } 24 | pose.x = node[0].as(); 25 | pose.y = node[1].as(); 26 | pose.theta = node[2].as(); 27 | return true; 28 | } 29 | }; 30 | } 31 | 32 | // Sets number of locations from list. 33 | class SetLocations : public BT::SyncActionNode 34 | { 35 | public: 36 | SetLocations(const std::string& name, const BT::NodeConfig& config); 37 | BT::NodeStatus tick() override; 38 | static BT::PortsList providedPorts(); 39 | }; 40 | 41 | // Gets location from a queue of locations read from a list. 42 | class GetLocationFromQueue : public BT::SyncActionNode 43 | { 44 | public: 45 | GetLocationFromQueue(const std::string& name, const BT::NodeConfig& config); 46 | BT::NodeStatus tick() override; 47 | static BT::PortsList providedPorts(); 48 | 49 | private: 50 | std::deque location_queue_; 51 | }; 52 | 53 | // Go to a target location (wraps around `navigate_to_pose` action). 54 | class GoToPose : public BT::StatefulActionNode 55 | { 56 | public: 57 | using NavigateToPose = nav2_msgs::action::NavigateToPose; 58 | using GoalHandleNav = rclcpp_action::ClientGoalHandle; 59 | 60 | bool done_flag_; 61 | rclcpp_action::ResultCode nav_result_; 62 | rclcpp::Node::SharedPtr node_ptr_; 63 | rclcpp_action::Client::SharedPtr client_ptr_; 64 | 65 | // Method overrides 66 | GoToPose(const std::string& name, const BT::NodeConfig& config, 67 | rclcpp::Node::SharedPtr node_ptr); 68 | BT::NodeStatus onStart() override; 69 | BT::NodeStatus onRunning() override; 70 | void onHalted() override {}; 71 | static BT::PortsList providedPorts(); 72 | 73 | // Action client callbacks 74 | void result_callback(const GoalHandleNav::WrappedResult& result); 75 | }; 76 | -------------------------------------------------------------------------------- /docker-compose.yaml: -------------------------------------------------------------------------------- 1 | # Docker Compose file for TurtleBot Behavior Examples 2 | # 3 | # Usage: 4 | # 5 | # To build the images: 6 | # docker compose build 7 | # 8 | # To start up a specific service by name: 9 | # docker compose up 10 | # 11 | # To open an interactive shell to a running container: 12 | # docker exec -it bash 13 | 14 | services: 15 | # Base image containing dependencies. 16 | base: 17 | image: turtlebot_behavior:base 18 | build: 19 | context: . 20 | dockerfile: docker/Dockerfile 21 | args: 22 | ROS_DISTRO: ${ROS_DISTRO:?} 23 | target: base 24 | # Interactive shell 25 | stdin_open: true 26 | tty: true 27 | # Networking and IPC for ROS 2 28 | network_mode: host 29 | ipc: host 30 | # Needed to display graphical applications 31 | privileged: True 32 | gpus: all 33 | environment: 34 | # Needed to define a TurtleBot model type (3 or 4) 35 | - TURTLEBOT_MODEL=${TURTLEBOT_MODEL:-3} 36 | # Allows graphical programs in the container. 37 | - DISPLAY=${DISPLAY} 38 | - QT_X11_NO_MITSHM=1 39 | - NVIDIA_DRIVER_CAPABILITIES=all 40 | volumes: 41 | # Allows graphical programs in the container. 42 | - /tmp/.X11-unix:/tmp/.X11-unix:rw 43 | - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority 44 | 45 | # Overlay image containing the example source code. 46 | overlay: 47 | extends: base 48 | image: turtlebot_behavior:overlay 49 | build: 50 | context: . 51 | dockerfile: docker/Dockerfile 52 | target: overlay 53 | 54 | # Developer container 55 | dev: 56 | extends: overlay 57 | image: turtlebot_behavior:dev 58 | build: 59 | context: . 60 | dockerfile: docker/Dockerfile 61 | target: dev 62 | args: 63 | - UID=${UID:-1000} 64 | - GID=${UID:-1000} 65 | - USERNAME=${USERNAME:-devuser} 66 | volumes: 67 | # Mount the source code 68 | - ./tb_autonomy:/overlay_ws/src/tb_autonomy:rw 69 | - ./tb_worlds:/overlay_ws/src/tb_worlds:rw 70 | # Mount colcon build artifacts for faster rebuilds 71 | - ./.colcon/build/:/overlay_ws/build/:rw 72 | - ./.colcon/install/:/overlay_ws/install/:rw 73 | - ./.colcon/log/:/overlay_ws/log/:rw 74 | user: ${USERNAME:-devuser} 75 | command: sleep infinity 76 | 77 | # Demo simulation world 78 | demo-world: 79 | extends: overlay 80 | command: ros2 launch tb_worlds tb_demo_world.launch.py 81 | 82 | # Behavior demo using Python and py_trees 83 | demo-behavior-py: 84 | extends: overlay 85 | command: > 86 | ros2 launch tb_autonomy tb_demo_behavior_py.launch.py 87 | tree_type:=${BT_TYPE:?} 88 | enable_vision:=${ENABLE_VISION:?} 89 | target_color:=${TARGET_COLOR:?} 90 | 91 | # Behavior demo using C++ and BehaviorTree.CPP 92 | demo-behavior-cpp: 93 | extends: overlay 94 | command: > 95 | ros2 launch tb_autonomy tb_demo_behavior_cpp.launch.py 96 | tree_type:=${BT_TYPE:?} 97 | enable_vision:=${ENABLE_VISION:?} 98 | target_color:=${TARGET_COLOR:?} 99 | -------------------------------------------------------------------------------- /tb_autonomy/scripts/test_vision.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Script that tests a simple color thresholding vision approach. 5 | 6 | Example usage: 7 | Python: python3 test_vision.py --min_h 0 --max_h 30 --min_s 220 --max_s 255 --min_v 0 --max_v 255 8 | ros2: ros2 run tb_autonomy test_vision.py --min_h 0 --max_h 30 --min_s 220 --max_s 255 --min_v 0 --max_v 255 9 | 10 | For reference, these are some approximate thresholds on the H channel. 11 | Red: 160 - 180 12 | Green: 40 - 90 13 | Blue: 100 - 150 14 | 15 | Since the blocks are highly saturated, the default S channel thresholds are tight (220-255) 16 | """ 17 | 18 | import argparse 19 | import cv_bridge 20 | import cv2 21 | import rclpy 22 | from rclpy.node import Node 23 | from sensor_msgs.msg import Image 24 | 25 | 26 | class ColorThresholdTester(Node): 27 | def __init__(self, args): 28 | # Define ROS subscriber 29 | super().__init__("test_vision") 30 | self.sub = self.create_subscription( 31 | Image, "/camera/image_raw", self.img_callback, 10 32 | ) 33 | 34 | # Define vision related variables 35 | self.bridge = cv_bridge.CvBridge() 36 | cv2.namedWindow("Image with Detections", cv2.WINDOW_NORMAL) 37 | params = cv2.SimpleBlobDetector_Params() 38 | params.minArea = 100 39 | params.maxArea = 100000 40 | params.filterByArea = True 41 | params.filterByColor = False 42 | params.filterByInertia = False 43 | params.filterByConvexity = False 44 | params.thresholdStep = 60 45 | self.detector = cv2.SimpleBlobDetector_create(params) 46 | self.min_bounds = (args.min_h, args.min_s, args.min_v) 47 | self.max_bounds = (args.max_h, args.max_s, args.max_v) 48 | self.get_logger().info( 49 | f"Using limits:\n" 50 | f"H: [{args.min_h} {args.max_h}] " 51 | f"S: [{args.min_s} {args.max_s}] " 52 | f"V: [{args.min_v} {args.max_v}] " 53 | ) 54 | 55 | def img_callback(self, msg): 56 | """Image topic subscriber callback""" 57 | img = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") 58 | hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) 59 | mask = cv2.inRange(hsv, self.min_bounds, self.max_bounds) 60 | keypoints = self.detector.detect(mask) 61 | labeled_img = cv2.drawKeypoints( 62 | img, 63 | keypoints, 64 | None, 65 | (255, 0, 0), 66 | cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS, 67 | ) 68 | cv2.imshow("Image with Detections", labeled_img) 69 | cv2.waitKey(10) 70 | 71 | 72 | if __name__ == "__main__": 73 | # Parse command-line arguments 74 | parser = argparse.ArgumentParser(description="HSV Color Thresholding test script") 75 | parser.add_argument("--min_h", type=int, default=0) 76 | parser.add_argument("--max_h", type=int, default=255) 77 | parser.add_argument("--min_s", type=int, default=220) 78 | parser.add_argument("--max_s", type=int, default=255) 79 | parser.add_argument("--min_v", type=int, default=0) 80 | parser.add_argument("--max_v", type=int, default=255) 81 | args = parser.parse_args() 82 | 83 | # Start ROS node and subscriber 84 | rclpy.init() 85 | subscriber = ColorThresholdTester(args) 86 | rclpy.spin(subscriber) 87 | subscriber.destroy_node() 88 | rclpy.shutdown() 89 | -------------------------------------------------------------------------------- /tb_autonomy/launch/tb_demo_behavior_cpp.launch.py: -------------------------------------------------------------------------------- 1 | from ament_index_python.packages import get_package_share_directory 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction 4 | from launch.substitutions import LaunchConfiguration, TextSubstitution 5 | from launch_ros.actions import Node 6 | from os import environ 7 | from os.path import join 8 | 9 | 10 | # Change this to your Groot2 executable path. 11 | # This currently defaults to a Groot2 AppImage file in your home directory. 12 | groot2_executable = join(environ.get("HOME", "/"), "Groot2.AppImage") 13 | 14 | 15 | def get_autonomy_and_visualization_nodes(context, *args, **kwargs): 16 | # Unpack arguments 17 | tree_type = LaunchConfiguration("tree_type").perform(context) 18 | enable_vision = ( 19 | LaunchConfiguration("enable_vision").perform(context).lower() == "true" 20 | ) 21 | 22 | prefix = "nav_" if not enable_vision else "" 23 | xml_file_name = f"{prefix}tree_{tree_type}.xml" 24 | print(f"\nUsing Behavior tree file: {xml_file_name}\n") 25 | 26 | pkg_tb_autonomy = get_package_share_directory("tb_autonomy") 27 | xml_file_path = join(pkg_tb_autonomy, "bt_xml", xml_file_name) 28 | 29 | return [ 30 | # Main autonomy node. 31 | Node( 32 | package="tb_autonomy", 33 | executable="autonomy_node_cpp", 34 | name="autonomy_node_cpp", 35 | output="screen", 36 | emulate_tty=True, 37 | parameters=[ 38 | { 39 | "location_file": LaunchConfiguration("location_file"), 40 | "target_color": ( 41 | LaunchConfiguration("target_color") if enable_vision else "" 42 | ), 43 | "tree_xml_file": xml_file_path, 44 | } 45 | ], 46 | ), 47 | # Behavior tree visualization with Groot2. 48 | ExecuteProcess( 49 | cmd=[groot2_executable, "--nosplash", "true", "--file", xml_file_path] 50 | ), 51 | ] 52 | 53 | 54 | def generate_launch_description(): 55 | pkg_tb_worlds = get_package_share_directory("tb_worlds") 56 | default_world_dir = join(pkg_tb_worlds, "maps", "sim_house_locations.yaml") 57 | 58 | return LaunchDescription( 59 | [ 60 | # Arguments 61 | DeclareLaunchArgument( 62 | "location_file", 63 | default_value=TextSubstitution(text=default_world_dir), 64 | description="YAML file name containing map locations in the world.", 65 | ), 66 | DeclareLaunchArgument( 67 | "target_color", 68 | default_value=TextSubstitution(text="blue"), 69 | description="Target object color (red, green, or blue)", 70 | ), 71 | DeclareLaunchArgument( 72 | "tree_type", 73 | default_value=TextSubstitution(text="queue"), 74 | description="Behavior tree type (naive or queue)", 75 | ), 76 | DeclareLaunchArgument( 77 | "enable_vision", 78 | default_value=TextSubstitution(text="True"), 79 | description="Enable vision behaviors. If false, do navigation only.", 80 | ), 81 | # Autonomy node and behavior tree visualization nodes 82 | OpaqueFunction(function=get_autonomy_and_visualization_nodes), 83 | ] 84 | ) 85 | -------------------------------------------------------------------------------- /tb_autonomy/src/vision_behaviors.cpp: -------------------------------------------------------------------------------- 1 | // Vision related behaviors 2 | 3 | #include "vision_behaviors.h" 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | using std::placeholders::_1; 13 | 14 | // LOOKFOROBJECT 15 | // Looks for an object of a certain color, specified by a parameter 16 | LookForObject::LookForObject(const std::string& name, 17 | const BT::NodeConfig& config, 18 | rclcpp::Node::SharedPtr node_ptr) : 19 | BT::StatefulActionNode(name, config), node_ptr_{node_ptr} 20 | { 21 | std::cout << "[" << this->name() << "] Initialized" << std::endl; 22 | } 23 | 24 | BT::NodeStatus LookForObject::onStart() { 25 | received_image_ = false; 26 | image_sub_ = image_transport::create_subscription( 27 | node_ptr_.get(), "/camera/image_raw", 28 | std::bind(&LookForObject::image_callback, this, _1), 29 | "raw", rmw_qos_profile_sensor_data); 30 | return BT::NodeStatus::RUNNING; 31 | } 32 | 33 | BT::NodeStatus LookForObject::onRunning() 34 | { 35 | // std::string target_color = "blue"; 36 | std::string target_color = 37 | node_ptr_->get_parameter("target_color").as_string(); 38 | std::cout << "[" << this->name() << "] Looking for " << target_color << " object" << std::endl; 39 | 40 | // Wait to receive an image 41 | // TODO Add timeout? 42 | if (!received_image_) { 43 | // std::cout << "[" << this->name() << "] Waiting for image" << std::endl; 44 | return BT::NodeStatus::RUNNING; 45 | } 46 | 47 | // Convert to HSV and threshold 48 | std::vector th = hsv_threshold_dict.at(target_color); 49 | cv::Mat img, img_hsv, img_threshold, img_keypoints; 50 | img = cv_bridge::toCvShare(latest_image_ptr_, "bgr8")->image; 51 | cv::cvtColor(img, img_hsv, cv::COLOR_BGR2HSV); 52 | cv::inRange(img_hsv, 53 | cv::Scalar(th[0], th[2], th[4]), 54 | cv::Scalar(th[1], th[3], th[5]), img_threshold); 55 | 56 | // Do blob detection 57 | cv::SimpleBlobDetector::Params params; 58 | params.minArea = 100; 59 | params.maxArea = 100000; 60 | params.filterByArea = true; 61 | params.filterByColor = false; 62 | params.filterByInertia = false; 63 | params.filterByConvexity = false; 64 | params.thresholdStep = 50; 65 | cv::Ptr detector = cv::SimpleBlobDetector::create(params); 66 | std::vector keypoints; 67 | detector->detect(img_threshold, keypoints); 68 | cv::drawKeypoints(img, keypoints, img_keypoints, 69 | cv::Scalar(255,0,0), 70 | cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS); 71 | 72 | // Display the image 73 | cv::namedWindow("Image"); 74 | cv::imshow("Image", img_keypoints); 75 | cv::waitKey(2000); 76 | 77 | if (keypoints.size() > 0) { 78 | std::cout << "[" << this->name() << "] Found object" << std::endl; 79 | return BT::NodeStatus::SUCCESS; 80 | } else { 81 | std::cout << "[" << this->name() << "] No object detected" << std::endl; 82 | return BT::NodeStatus::FAILURE; 83 | } 84 | } 85 | 86 | void LookForObject::onHalted() { 87 | image_sub_.shutdown(); 88 | received_image_ = false; 89 | } 90 | 91 | void LookForObject::image_callback( 92 | const sensor_msgs::msg::Image::ConstSharedPtr& msg) { 93 | latest_image_ptr_ = msg; 94 | received_image_ = true; 95 | } 96 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | # ROS distribution to use 2 | ARG ROS_DISTRO=jazzy 3 | 4 | ####################################### 5 | # Base Image for TurtleBot Simulation # 6 | ####################################### 7 | FROM ros:${ROS_DISTRO} AS base 8 | ENV ROS_DISTRO=${ROS_DISTRO} 9 | SHELL ["/bin/bash", "-c"] 10 | 11 | # Install basic apt packages 12 | RUN apt-get update && apt-get install -y --no-install-recommends \ 13 | curl git libcanberra-gtk-module libcanberra-gtk3-module fuse3 libfuse2 libqt5svg5-dev \ 14 | python3-pip python3-opencv python3-tk python3-pyqt5.qtwebengine 15 | 16 | # Install additional Python modules 17 | RUN pip3 install --break-system-packages matplotlib transforms3d 18 | 19 | # Use Cyclone DDS as middleware 20 | RUN apt-get update && apt-get install -y --no-install-recommends \ 21 | ros-${ROS_DISTRO}-rmw-cyclonedds-cpp 22 | ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 23 | 24 | # Create Colcon workspace with external dependencies 25 | RUN mkdir -p /turtlebot_ws/src 26 | WORKDIR /turtlebot_ws/src 27 | COPY dependencies.repos . 28 | RUN vcs import < dependencies.repos 29 | 30 | # Build the base Colcon workspace, installing dependencies first. 31 | WORKDIR /turtlebot_ws 32 | RUN source /opt/ros/${ROS_DISTRO}/setup.bash \ 33 | && apt-get update -y \ 34 | && rosdep install --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -y \ 35 | && colcon build --symlink-install 36 | ENV TURTLEBOT_MODEL=3 37 | 38 | # Download Groot2 AppImage and place it in the home folder. 39 | WORKDIR /root/ 40 | RUN curl -o Groot2.AppImage https://s3.us-west-1.amazonaws.com/download.behaviortree.dev/groot2_linux_installer/Groot2-v1.6.1-x86_64.AppImage \ 41 | && chmod a+x Groot2.AppImage 42 | 43 | # Remove display warnings 44 | RUN mkdir /tmp/runtime-root 45 | ENV XDG_RUNTIME_DIR "/tmp/runtime-root" 46 | RUN chmod -R 0700 /tmp/runtime-root 47 | ENV NO_AT_BRIDGE 1 48 | 49 | # Set up the entrypoint 50 | WORKDIR /turtlebot_ws 51 | COPY ./docker/entrypoint.sh / 52 | ENTRYPOINT [ "/entrypoint.sh" ] 53 | 54 | ########################################## 55 | # Overlay Image for TurtleBot Simulation # 56 | ########################################## 57 | FROM base AS overlay 58 | 59 | # Create an overlay Colcon workspace 60 | RUN mkdir -p /overlay_ws/src 61 | WORKDIR /overlay_ws 62 | COPY ./tb_autonomy/ ./src/tb_autonomy/ 63 | COPY ./tb_worlds/ ./src/tb_worlds/ 64 | RUN source /turtlebot_ws/install/setup.bash \ 65 | && rosdep install --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -y \ 66 | && colcon build --symlink-install 67 | 68 | # Set up the entrypoint 69 | COPY ./docker/entrypoint.sh / 70 | ENTRYPOINT [ "/entrypoint.sh" ] 71 | 72 | ##################### 73 | # Development Image # 74 | ##################### 75 | FROM overlay AS dev 76 | 77 | # Dev container arguments 78 | ARG USERNAME=devuser 79 | ARG UID=1000 80 | ARG GID=${UID} 81 | 82 | # Install extra tools for development 83 | RUN apt-get update && apt-get install -y --no-install-recommends \ 84 | gdb gdbserver nano 85 | 86 | # In Ubuntu 24.04, there is already a user named "ubuntu" with UID 1000. 87 | # Delete this in the (common) event that the user on the host also has this UID. 88 | RUN touch /var/mail/ubuntu \ 89 | && chown ubuntu /var/mail/ubuntu \ 90 | && userdel -r ubuntu 91 | 92 | # Create new user and home directory 93 | RUN groupadd --gid $GID $USERNAME \ 94 | && useradd --uid ${GID} --gid ${UID} --create-home ${USERNAME} \ 95 | && echo ${USERNAME} ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/${USERNAME} \ 96 | && chmod 0440 /etc/sudoers.d/${USERNAME} \ 97 | && mkdir -p /home/${USERNAME} \ 98 | && chown -R ${UID}:${GID} /home/${USERNAME} 99 | 100 | # Set the ownership of the overlay workspace to the new user 101 | RUN chown -R ${UID}:${GID} /overlay_ws/ 102 | 103 | # Move Groot2 to new user's home directory and ensure it can be run 104 | RUN groupadd fuse \ 105 | && usermod -aG fuse ${USERNAME} 106 | RUN mv /root/Groot2.AppImage /home/${USERNAME} \ 107 | && chown ${UID}:${GID} /home/${USERNAME}/Groot2.AppImage 108 | 109 | # Set up the entrypoint, including it in the .bashrc for interactive shells 110 | USER ${USERNAME} 111 | RUN echo "source /entrypoint.sh" >> /home/${USERNAME}/.bashrc 112 | -------------------------------------------------------------------------------- /tb_autonomy/python/tb_behaviors/vision.py: -------------------------------------------------------------------------------- 1 | """ 2 | Vision behaviors for TurtleBot. 3 | """ 4 | 5 | import cv2 6 | import cv_bridge 7 | import rclpy 8 | from rclpy.duration import Duration 9 | import py_trees 10 | from sensor_msgs.msg import Image 11 | 12 | import matplotlib 13 | 14 | matplotlib.use("Agg") 15 | import matplotlib.pyplot as plt 16 | 17 | # Define HSV color space thresholds 18 | hsv_threshold_dict = { 19 | "red": ((160, 220, 0), (180, 255, 255)), 20 | "green": ((40, 220, 0), (90, 255, 255)), 21 | "blue": ((100, 220, 0), (150, 255, 255)), 22 | } 23 | 24 | 25 | class LookForObject(py_trees.behaviour.Behaviour): 26 | """ 27 | Gets images from the robot and looks for object using 28 | simple HSV color space thresholding and blob detection. 29 | """ 30 | 31 | def __init__(self, name, color, node, img_timeout=3.0, visualize=True): 32 | super(LookForObject, self).__init__(name) 33 | self.color = color 34 | self.node = node 35 | self.hsv_min = hsv_threshold_dict[color][0] 36 | self.hsv_max = hsv_threshold_dict[color][1] 37 | self.img_timeout = Duration(nanoseconds=img_timeout * 1e9) 38 | self.viz_window_name = "Image with Detections" 39 | self.visualize = visualize 40 | if self.visualize: 41 | plt.figure(1) 42 | plt.axis("off") 43 | plt.title(self.viz_window_name) 44 | plt.ion() 45 | 46 | def initialise(self): 47 | """Starts all the vision related objects""" 48 | self.bridge = cv_bridge.CvBridge() 49 | params = cv2.SimpleBlobDetector_Params() 50 | params.minArea = 100 51 | params.maxArea = 100000 52 | params.filterByArea = True 53 | params.filterByColor = False 54 | params.filterByInertia = False 55 | params.filterByConvexity = False 56 | params.thresholdStep = 50 57 | self.detector = cv2.SimpleBlobDetector_create(params) 58 | 59 | self.start_time = self.node.get_clock().now() 60 | self.latest_img_msg = None 61 | self.img_sub = self.node.create_subscription( 62 | Image, "/camera/image_raw", self.img_callback, 10 63 | ) 64 | 65 | def update(self): 66 | """Looks for at least one object detection using HSV thresholding""" 67 | # Wait for an image message and handle failure case 68 | now = self.node.get_clock().now() 69 | if self.latest_img_msg is None: 70 | if now - self.start_time < self.img_timeout: 71 | return py_trees.common.Status.RUNNING 72 | else: 73 | self.logger.info("Image timeout exceeded") 74 | return py_trees.common.Status.FAILURE 75 | 76 | # Process the image 77 | img = self.bridge.imgmsg_to_cv2(self.latest_img_msg, desired_encoding="bgr8") 78 | hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) 79 | mask = cv2.inRange(hsv, self.hsv_min, self.hsv_max) 80 | keypoints = self.detector.detect(mask) 81 | 82 | # Visualize, if enabled 83 | if self.visualize: 84 | labeled_img = cv2.drawKeypoints( 85 | img, 86 | keypoints, 87 | None, 88 | (255, 0, 0), 89 | cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS, 90 | ) 91 | # OpenCV visualization 92 | cv2.destroyAllWindows() 93 | cv2.imshow(self.viz_window_name, labeled_img) 94 | cv2.waitKey(100) 95 | 96 | # Matplotlib visualization 97 | # plt.imshow(labeled_img[:,:,::-1]) 98 | # plt.pause(0.1) 99 | 100 | # If there were no failures along the way, the behavior was successful 101 | if len(keypoints) == 0: 102 | self.logger.info("No objects detected") 103 | return py_trees.common.Status.FAILURE 104 | for k in keypoints: 105 | self.logger.info(f"Detected object at [{k.pt[0]}, {k.pt[1]}]") 106 | return py_trees.common.Status.SUCCESS 107 | 108 | def terminate(self, new_status): 109 | self.logger.info(f"Terminated with status {new_status}") 110 | self.img_sub = None 111 | self.latest_img_msg = None 112 | 113 | def img_callback(self, msg): 114 | # self.logger.info("Image received") 115 | self.latest_img_msg = msg 116 | -------------------------------------------------------------------------------- /tb_autonomy/python/tb_behaviors/navigation.py: -------------------------------------------------------------------------------- 1 | """ 2 | Navigation behaviors for TurtleBot. 3 | """ 4 | 5 | import py_trees 6 | import transforms3d 7 | 8 | from action_msgs.msg import GoalStatus 9 | from rclpy.action import ActionClient 10 | from nav2_msgs.action import NavigateToPose 11 | 12 | 13 | class GetLocationFromQueue(py_trees.behaviour.Behaviour): 14 | """Gets a location name from the queue""" 15 | 16 | def __init__(self, name, location_dict): 17 | super(GetLocationFromQueue, self).__init__(name) 18 | self.location_dict = location_dict 19 | self.bb = py_trees.blackboard.Blackboard() 20 | 21 | def update(self): 22 | """Checks for the status of the navigation action""" 23 | loc_list = self.bb.get("loc_list") 24 | if len(loc_list) == 0: 25 | self.logger.info("No locations available") 26 | return py_trees.common.Status.FAILURE 27 | else: 28 | target_location = loc_list.pop() 29 | self.logger.info(f"Selected location {target_location}") 30 | target_pose = self.location_dict[target_location] 31 | self.bb.set("target_pose", target_pose) 32 | return py_trees.common.Status.SUCCESS 33 | 34 | def terminate(self, new_status): 35 | self.logger.info(f"Terminated with status {new_status}") 36 | 37 | 38 | class GoToPose(py_trees.behaviour.Behaviour): 39 | """Wrapper behavior around the `move_base` action client""" 40 | 41 | def __init__(self, name, pose, node): 42 | super(GoToPose, self).__init__(name) 43 | self.pose = pose 44 | self.client = None 45 | self.node = node 46 | self.bb = py_trees.blackboard.Blackboard() 47 | 48 | def initialise(self): 49 | """Sends the initial navigation action goal""" 50 | # Check if there is a pose available in the blackboard 51 | try: 52 | target_pose = self.bb.get("target_pose") 53 | if target_pose is not None: 54 | self.pose = target_pose 55 | except: 56 | pass 57 | 58 | self.client = ActionClient(self.node, NavigateToPose, "/navigate_to_pose") 59 | self.client.wait_for_server() 60 | 61 | self.goal_status = None 62 | x, y, theta = self.pose 63 | self.logger.info(f"Going to [x: {x}, y: {y}, theta: {theta}] ...") 64 | self.goal = self.create_move_base_goal(x, y, theta) 65 | self.send_goal_future = self.client.send_goal_async(self.goal) 66 | self.send_goal_future.add_done_callback(self.goal_callback) 67 | 68 | def goal_callback(self, future): 69 | res = future.result() 70 | if res is None or not res.accepted: 71 | return 72 | future = res.get_result_async() 73 | future.add_done_callback(self.goal_result_callback) 74 | 75 | def goal_result_callback(self, future): 76 | # If there is a result, we consider navigation completed and save the 77 | # result code to be checked in the `update()` method. 78 | self.goal_status = future.result().status 79 | 80 | def update(self): 81 | """Checks for the status of the navigation action""" 82 | # If there is a result, we can check the status of the action directly. 83 | # Otherwise, the action is still running. 84 | if self.goal_status is not None: 85 | if self.goal_status == GoalStatus.STATUS_SUCCEEDED: 86 | return py_trees.common.Status.SUCCESS 87 | else: 88 | return py_trees.common.Status.FAILURE 89 | return py_trees.common.Status.RUNNING 90 | 91 | def terminate(self, new_status): 92 | self.logger.info(f"Terminated with status {new_status}") 93 | self.client = None 94 | self.bb.set("target_pose", None) 95 | 96 | def create_move_base_goal(self, x, y, theta): 97 | """Creates a MoveBaseGoal message from a 2D navigation pose""" 98 | goal = NavigateToPose.Goal() 99 | goal.pose.header.frame_id = "map" 100 | goal.pose.header.stamp = self.node.get_clock().now().to_msg() 101 | goal.pose.pose.position.x = x 102 | goal.pose.pose.position.y = y 103 | quat = transforms3d.euler.euler2quat(0, 0, theta) 104 | goal.pose.pose.orientation.w = quat[0] 105 | goal.pose.pose.orientation.x = quat[1] 106 | goal.pose.pose.orientation.y = quat[2] 107 | goal.pose.pose.orientation.z = quat[3] 108 | return goal 109 | -------------------------------------------------------------------------------- /tb_autonomy/src/autonomy_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Main behavior node for TurtleBot. 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "rclcpp/rclcpp.hpp" 10 | #include "ament_index_cpp/get_package_share_directory.hpp" 11 | #include "behaviortree_cpp/bt_factory.h" 12 | #include "behaviortree_cpp/xml_parsing.h" 13 | #include "behaviortree_cpp/loggers/groot2_publisher.h" 14 | #include "yaml-cpp/yaml.h" 15 | 16 | #include "navigation_behaviors.h" 17 | #include "vision_behaviors.h" 18 | 19 | using namespace std::chrono_literals; 20 | 21 | const std::string default_bt_xml_file = 22 | ament_index_cpp::get_package_share_directory("tb_autonomy") + "/bt_xml/tree_naive.xml"; 23 | const std::string tb_worlds_share_dir = 24 | ament_index_cpp::get_package_share_directory("tb_worlds"); 25 | const std::string default_location_file = 26 | tb_worlds_share_dir + "/maps/sim_house_locations.yaml"; 27 | 28 | 29 | class AutonomyNode : public rclcpp::Node { 30 | public: 31 | AutonomyNode() : Node("autonomy_node") { 32 | // Read the location file and shuffle it 33 | this->declare_parameter( 34 | "location_file", default_location_file); 35 | location_file_ = 36 | this->get_parameter("location_file").as_string(); 37 | RCLCPP_INFO(this->get_logger(), 38 | "Using location file %s", location_file_.c_str()); 39 | 40 | // Declare and get the other node parameters. 41 | this->declare_parameter("tree_xml_file", default_bt_xml_file); 42 | tree_xml_file_ = this->get_parameter("tree_xml_file").as_string(); 43 | this->declare_parameter("target_color", ""); 44 | target_color_ = this->get_parameter("target_color").as_string(); 45 | if (target_color_ != "") { 46 | RCLCPP_INFO(this->get_logger(), "Searching for target color %s...", 47 | target_color_.c_str()); 48 | } 49 | } 50 | 51 | void execute() { 52 | // Build and initialize the behavior tree based on parameters. 53 | create_behavior_tree(); 54 | 55 | // Create a timer to tick the behavior tree. 56 | const auto timer_period = 500ms; 57 | timer_ = this->create_wall_timer( 58 | timer_period, 59 | std::bind(&AutonomyNode::update_behavior_tree, this)); 60 | 61 | rclcpp::spin(shared_from_this()); 62 | rclcpp::shutdown(); 63 | } 64 | 65 | void create_behavior_tree() { 66 | // Build a behavior tree from XML and set it up for logging 67 | BT::BehaviorTreeFactory factory; 68 | factory.registerNodeType("SetLocations"); 69 | factory.registerNodeType("GetLocationFromQueue"); 70 | factory.registerNodeType("GoToPose", shared_from_this()); 71 | factory.registerNodeType("LookForObject", shared_from_this()); 72 | 73 | auto blackboard = BT::Blackboard::create(); 74 | blackboard->set("location_file", location_file_); 75 | tree_ = factory.createTreeFromFile(tree_xml_file_, blackboard); 76 | 77 | // Set up tree logging to monitor the tree in Groot2. 78 | // Default ports (1666/1667) are used by the Nav2 behavior tree, so we use another port. 79 | // NOTE: You must have the PRO version of Groot2 to view live tree updates. 80 | publisher_ptr_ = std::make_unique(tree_, 1668); 81 | } 82 | 83 | void update_behavior_tree() { 84 | // Tick the behavior tree. 85 | BT::NodeStatus tree_status = tree_.tickOnce(); 86 | if (tree_status == BT::NodeStatus::RUNNING) { 87 | return; 88 | } 89 | // Cancel the timer if we hit a terminal state. 90 | if (tree_status == BT::NodeStatus::SUCCESS) { 91 | RCLCPP_INFO(this->get_logger(), "Finished with status SUCCESS"); 92 | timer_->cancel(); 93 | } else if (tree_status == BT::NodeStatus::FAILURE) { 94 | RCLCPP_INFO(this->get_logger(), "Finished with status FAILURE"); 95 | timer_->cancel(); 96 | } 97 | } 98 | 99 | // Configuration parameters. 100 | std::string tree_xml_file_; 101 | std::string location_file_; 102 | std::string target_color_; 103 | 104 | // ROS and BehaviorTree.CPP variables. 105 | rclcpp::TimerBase::SharedPtr timer_; 106 | BT::Tree tree_; 107 | std::unique_ptr publisher_ptr_; 108 | }; 109 | 110 | 111 | int main(int argc, char **argv) 112 | { 113 | rclcpp::init(argc, argv); 114 | auto node = std::make_shared(); 115 | node->execute(); 116 | return 0; 117 | } 118 | -------------------------------------------------------------------------------- /tb_worlds/launch/turtlebot_spawner.launch.py: -------------------------------------------------------------------------------- 1 | # This file has been modified from 2 | # https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation/blob/091b5ff12436890a54de6325df3573ae110e912b/nav2_minimal_tb3_sim/launch/spawn_tb3.launch.py 3 | # Modification: 4 | # - Add gz_bridge_config argument for specifying bridge yaml file 5 | # - Spawn Robot: Given the environment variable ("TURTLEBOT_MODEL"), choose either: 6 | # - Spawn turtlebot3 using xacro executable 7 | # - Spawn turtlebot4 using robot_description topic 8 | 9 | import os 10 | from pathlib import Path 11 | from ament_index_python.packages import get_package_share_directory 12 | 13 | from launch import LaunchDescription 14 | from launch.actions import AppendEnvironmentVariable 15 | from launch.actions import DeclareLaunchArgument 16 | from launch.substitutions import ( 17 | LaunchConfiguration, 18 | PythonExpression, 19 | EnvironmentVariable, 20 | ) 21 | from launch.substitutions.command import Command 22 | from launch.substitutions.find_executable import FindExecutable 23 | 24 | from launch_ros.actions import Node 25 | from launch.conditions import IfCondition 26 | 27 | 28 | def generate_launch_description(): 29 | gz_mdl_dir = get_package_share_directory("nav2_minimal_tb3_sim") 30 | bringup_dir = get_package_share_directory("tb_worlds") 31 | 32 | namespace = LaunchConfiguration("namespace") 33 | robot_name = LaunchConfiguration("robot_name") 34 | robot_sdf = LaunchConfiguration("robot_sdf") 35 | turtlebot_model = LaunchConfiguration("turtlebot_model") 36 | gz_bridge_config = LaunchConfiguration("gz_bridge_config") 37 | pose = { 38 | "x": LaunchConfiguration("x_pose", default="0.0"), 39 | "y": LaunchConfiguration("y_pose", default="0.0"), 40 | "z": LaunchConfiguration("z_pose", default="0.01"), 41 | "R": LaunchConfiguration("roll", default="0.00"), 42 | "P": LaunchConfiguration("pitch", default="0.00"), 43 | "Y": LaunchConfiguration("yaw", default="0.00"), 44 | } 45 | 46 | # Declare the launch arguments 47 | declare_namespace_cmd = DeclareLaunchArgument( 48 | "namespace", default_value="", description="Top-level namespace" 49 | ) 50 | 51 | declare_robot_name_cmd = DeclareLaunchArgument( 52 | "robot_name", default_value="turtlebot3", description="name of the robot" 53 | ) 54 | 55 | declare_robot_sdf_cmd = DeclareLaunchArgument( 56 | "robot_sdf", 57 | default_value=os.path.join(bringup_dir, "urdf", "gz_waffle.sdf.xacro"), 58 | description="Full path to robot sdf file to spawn the robot in gazebo", 59 | ) 60 | 61 | declare_turtlebot_model_cmd = DeclareLaunchArgument( 62 | "turtlebot_model", 63 | default_value=EnvironmentVariable("TURTLEBOT_MODEL", default_value="3"), 64 | ) 65 | 66 | declare_gz_bridge_cmd = DeclareLaunchArgument( 67 | "gz_bridge_config", 68 | default_value=os.path.join(bringup_dir, "configs", "turtlebot3_bridge.yaml"), 69 | description="Full path to robot bridge configuration file", 70 | ) 71 | 72 | bridge = Node( 73 | package="ros_gz_bridge", 74 | executable="parameter_bridge", 75 | namespace=namespace, 76 | parameters=[ 77 | { 78 | "config_file": gz_bridge_config, 79 | "expand_gz_topic_names": True, 80 | "use_sim_time": True, 81 | } 82 | ], 83 | output="screen", 84 | ) 85 | 86 | spawn_tb3_model = Node( 87 | condition=IfCondition(PythonExpression([turtlebot_model, " == 3"])), 88 | package="ros_gz_sim", 89 | executable="create", 90 | output="screen", 91 | namespace=namespace, 92 | arguments=[ 93 | "-name", 94 | robot_name, 95 | "-string", 96 | Command( 97 | [ 98 | FindExecutable(name="xacro"), 99 | " ", 100 | "namespace:=", 101 | LaunchConfiguration("namespace"), 102 | " ", 103 | robot_sdf, 104 | ] 105 | ), 106 | "-x", 107 | pose["x"], 108 | "-y", 109 | pose["y"], 110 | "-z", 111 | pose["z"], 112 | "-R", 113 | pose["R"], 114 | "-P", 115 | pose["P"], 116 | "-Y", 117 | pose["Y"], 118 | ], 119 | ) 120 | 121 | spawn_tb4_model = Node( 122 | condition=IfCondition(PythonExpression([turtlebot_model, " == 4"])), 123 | package="ros_gz_sim", 124 | executable="create", 125 | namespace=namespace, 126 | output="screen", 127 | arguments=[ 128 | "-name", 129 | robot_name, 130 | "-topic", 131 | "robot_description", 132 | "-x", 133 | pose["x"], 134 | "-y", 135 | pose["y"], 136 | "-z", 137 | pose["z"], 138 | "-R", 139 | pose["R"], 140 | "-P", 141 | pose["P"], 142 | "-Y", 143 | pose["Y"], 144 | ], 145 | parameters=[{"use_sim_time": True}], 146 | ) 147 | 148 | set_env_vars_resources = AppendEnvironmentVariable( 149 | "GZ_SIM_RESOURCE_PATH", os.path.join(gz_mdl_dir, "models") 150 | ) 151 | set_env_vars_resources2 = AppendEnvironmentVariable( 152 | "GZ_SIM_RESOURCE_PATH", str(Path(os.path.join(gz_mdl_dir)).parent.resolve()) 153 | ) 154 | 155 | # Create the launch description and populate 156 | ld = LaunchDescription() 157 | ld.add_action(declare_namespace_cmd) 158 | ld.add_action(declare_robot_name_cmd) 159 | ld.add_action(declare_robot_sdf_cmd) 160 | ld.add_action(declare_turtlebot_model_cmd) 161 | ld.add_action(declare_gz_bridge_cmd) 162 | ld.add_action(set_env_vars_resources) 163 | ld.add_action(set_env_vars_resources2) 164 | 165 | ld.add_action(bridge) 166 | ld.add_action(spawn_tb3_model) 167 | ld.add_action(spawn_tb4_model) 168 | return ld 169 | -------------------------------------------------------------------------------- /tb_autonomy/scripts/autonomy_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Autonomy node for the TurtleBot. 5 | 6 | This script relies on a YAML file of potential navigation locations, 7 | which is listed as a `location_file` ROS parameter. 8 | 9 | Example usage: 10 | ros2 run tb_autonomy autonomy_node.py 11 | ros2 run tb_autonomy autonomy_node.py --ros-args -p location_file:=/path/to/my/file.yaml 12 | ros2 run tb_autonomy autonomy_node.py --ros-args -p tree_type:=queue -p target_color:=green 13 | """ 14 | 15 | import os 16 | import yaml 17 | import random 18 | import rclpy 19 | from rclpy.node import Node 20 | import time 21 | import py_trees 22 | import py_trees_ros 23 | from py_trees.common import OneShotPolicy 24 | from ament_index_python.packages import get_package_share_directory 25 | 26 | from tb_behaviors.navigation import GoToPose, GetLocationFromQueue 27 | from tb_behaviors.vision import LookForObject 28 | 29 | 30 | default_location_file = os.path.join( 31 | get_package_share_directory("tb_worlds"), "maps", "sim_house_locations.yaml" 32 | ) 33 | 34 | 35 | class AutonomyBehavior(Node): 36 | def __init__(self): 37 | super().__init__("autonomy_node") 38 | self.declare_parameter("location_file", value=default_location_file) 39 | self.declare_parameter("tree_type", value="queue") 40 | self.declare_parameter("enable_vision", value=True) 41 | self.declare_parameter("target_color", value="blue") 42 | 43 | # Parse locations YAML file and shuffle the location list. 44 | location_file = self.get_parameter("location_file").value 45 | with open(location_file, "r") as f: 46 | self.locations = yaml.load(f, Loader=yaml.FullLoader) 47 | self.loc_list = list(self.locations.keys()) 48 | random.shuffle(self.loc_list) 49 | 50 | # Create and setup the behavior tree 51 | self.tree_type = self.get_parameter("tree_type").value 52 | self.enable_vision = self.get_parameter("enable_vision").value 53 | self.target_color = self.get_parameter("target_color").value 54 | self.create_behavior_tree(self.tree_type) 55 | 56 | self.tree.node.get_logger().info(f"Using location file: {location_file}") 57 | self.tree.node.get_logger().info(f"Looking for color {self.target_color}...") 58 | 59 | def create_behavior_tree(self, tree_type): 60 | if tree_type == "naive": 61 | self.tree = self.create_naive_tree() 62 | elif tree_type == "queue": 63 | self.tree = self.create_queue_tree() 64 | else: 65 | self.get_logger().info(f"Invalid behavior tree type {tree_type}.") 66 | 67 | def create_naive_tree(self): 68 | """Create behavior tree with explicit nodes for each location.""" 69 | if self.enable_vision: 70 | selector = py_trees.composites.Selector(name="navigation", memory=True) 71 | root = py_trees.decorators.OneShot( 72 | name="root", 73 | child=selector, 74 | policy=OneShotPolicy.ON_SUCCESSFUL_COMPLETION, 75 | ) 76 | tree = py_trees_ros.trees.BehaviourTree(root, unicode_tree_debug=False) 77 | tree.setup(timeout=15.0, node=self) 78 | 79 | for loc in self.loc_list: 80 | pose = self.locations[loc] 81 | selector.add_child( 82 | py_trees.decorators.OneShot( 83 | name=f"try_{loc}", 84 | child=py_trees.composites.Sequence( 85 | name=f"search_{loc}", 86 | children=[ 87 | GoToPose(f"go_to_{loc}", pose, tree.node), 88 | LookForObject( 89 | f"find_{self.target_color}_{loc}", 90 | self.target_color, 91 | tree.node, 92 | ), 93 | ], 94 | memory=True, 95 | ), 96 | policy=OneShotPolicy.ON_COMPLETION, 97 | ) 98 | ) 99 | 100 | else: 101 | seq = py_trees.composites.Sequence(name="navigation", memory=True) 102 | root = py_trees.decorators.OneShot( 103 | name="root", child=seq, policy=OneShotPolicy.ON_SUCCESSFUL_COMPLETION 104 | ) 105 | tree = py_trees_ros.trees.BehaviourTree(root, unicode_tree_debug=False) 106 | tree.setup(timeout=15.0, node=self) 107 | 108 | for loc in self.loc_list: 109 | pose = self.locations[loc] 110 | seq.add_child(GoToPose(f"go_to_{loc}", pose, self)) 111 | 112 | return tree 113 | 114 | def create_queue_tree(self): 115 | """Create behavior tree by picking a next location from a queue""" 116 | bb = py_trees.blackboard.Blackboard() 117 | bb.set("loc_list", self.loc_list) 118 | 119 | seq = py_trees.composites.Sequence(name="search", memory=True) 120 | root = py_trees.decorators.OneShot( 121 | name="root", child=seq, policy=OneShotPolicy.ON_SUCCESSFUL_COMPLETION 122 | ) 123 | tree = py_trees_ros.trees.BehaviourTree(root, unicode_tree_debug=False) 124 | tree.setup(timeout=15.0, node=self) 125 | 126 | seq.add_children( 127 | [ 128 | GetLocationFromQueue("get_next_location", self.locations), 129 | GoToPose("go_to_location", None, tree.node), 130 | ] 131 | ) 132 | if self.enable_vision: 133 | seq.add_child( 134 | LookForObject(f"find_{self.target_color}", self.target_color, tree.node) 135 | ) 136 | return tree 137 | 138 | def execute(self, period=0.5): 139 | """Executes the behavior tree at the specified period.""" 140 | self.tree.tick_tock(period_ms=period * 1000.0) 141 | rclpy.spin(self.tree.node) 142 | rclpy.shutdown() 143 | 144 | 145 | if __name__ == "__main__": 146 | rclpy.init() 147 | behavior = AutonomyBehavior() 148 | behavior.execute() 149 | -------------------------------------------------------------------------------- /tb_worlds/launch/tb_demo_world.launch.py: -------------------------------------------------------------------------------- 1 | # This file has been modified from 2 | # https://github.com/ros-navigation/navigation2/blob/9d60bc0a3ca4e201250254c866c4eedc1441ed4e/nav2_bringup/launch/tb3_simulation_launch.py 3 | # Modification: 4 | # - Separate nav2 bringup from robot simulation 5 | 6 | import os 7 | from ament_index_python.packages import get_package_share_directory 8 | 9 | from launch import LaunchDescription 10 | from launch.actions import ( 11 | DeclareLaunchArgument, 12 | IncludeLaunchDescription, 13 | ) 14 | from launch.conditions import IfCondition 15 | from launch.launch_description_sources import PythonLaunchDescriptionSource 16 | from launch.substitutions import LaunchConfiguration 17 | 18 | from launch_ros.actions import Node 19 | 20 | 21 | def generate_launch_description(): 22 | # Get the launch directory 23 | bringup_dir = get_package_share_directory("tb_worlds") 24 | nav2_bringup_dir = get_package_share_directory("nav2_bringup") 25 | 26 | # Create the launch configuration variables 27 | slam = LaunchConfiguration("slam") 28 | namespace = LaunchConfiguration("namespace") 29 | map_yaml_file = LaunchConfiguration("map") 30 | use_sim_time = LaunchConfiguration("use_sim_time") 31 | params_file = LaunchConfiguration("params_file") 32 | autostart = LaunchConfiguration("autostart") 33 | use_composition = LaunchConfiguration("use_composition") 34 | use_respawn = LaunchConfiguration("use_respawn") 35 | rviz_config_file = LaunchConfiguration("rviz_config_file") 36 | use_rviz = LaunchConfiguration("use_rviz") 37 | 38 | # Declare the launch arguments 39 | declare_namespace_cmd = DeclareLaunchArgument( 40 | "namespace", default_value="", description="Top-level namespace" 41 | ) 42 | 43 | declare_slam_cmd = DeclareLaunchArgument( 44 | "slam", default_value="False", description="Whether run a SLAM" 45 | ) 46 | 47 | declare_map_yaml_cmd = DeclareLaunchArgument( 48 | "map", 49 | default_value=os.path.join(bringup_dir, "maps", "sim_house_map.yaml"), 50 | ) 51 | 52 | declare_use_sim_time_cmd = DeclareLaunchArgument( 53 | "use_sim_time", 54 | default_value="true", 55 | description="Use simulation (Gazebo) clock if true", 56 | ) 57 | 58 | declare_params_file_cmd = DeclareLaunchArgument( 59 | "params_file", 60 | default_value=os.path.join(bringup_dir, "configs", "nav2_params.yaml"), 61 | description="Full path to the ROS2 parameters file to use for all launched nodes", 62 | ) 63 | 64 | declare_autostart_cmd = DeclareLaunchArgument( 65 | "autostart", 66 | default_value="true", 67 | description="Automatically startup the nav2 stack", 68 | ) 69 | 70 | declare_use_composition_cmd = DeclareLaunchArgument( 71 | "use_composition", 72 | default_value="True", 73 | description="Whether to use composed bringup", 74 | ) 75 | 76 | declare_use_respawn_cmd = DeclareLaunchArgument( 77 | "use_respawn", 78 | default_value="False", 79 | description="Whether to respawn if a node crashes. Applied when composition is disabled.", 80 | ) 81 | 82 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 83 | "rviz_config_file", 84 | default_value=os.path.join(nav2_bringup_dir, "rviz", "nav2_default_view.rviz"), 85 | description="Full path to the RVIZ config file to use", 86 | ) 87 | 88 | declare_use_rviz_cmd = DeclareLaunchArgument( 89 | "use_rviz", default_value="True", description="Whether to start RVIZ" 90 | ) 91 | 92 | declare_robot_name_cmd = DeclareLaunchArgument( 93 | "robot_name", default_value="turtlebot", description="name of the robot" 94 | ) 95 | 96 | sim_cmd = IncludeLaunchDescription( 97 | PythonLaunchDescriptionSource( 98 | os.path.join(bringup_dir, "launch", "tb_world.launch.py") 99 | ), 100 | launch_arguments={ 101 | "namespace": namespace, 102 | "use_sim_time": use_sim_time, 103 | }.items(), 104 | ) 105 | 106 | # Block Spawner 107 | block_spawner_cmd = IncludeLaunchDescription( 108 | PythonLaunchDescriptionSource( 109 | os.path.join(bringup_dir, "launch", "block_spawner.launch.py") 110 | ), 111 | ) 112 | 113 | rviz_cmd = IncludeLaunchDescription( 114 | PythonLaunchDescriptionSource( 115 | os.path.join(nav2_bringup_dir, "launch", "rviz_launch.py") 116 | ), 117 | condition=IfCondition(use_rviz), 118 | launch_arguments={ 119 | "namespace": namespace, 120 | "use_sim_time": use_sim_time, 121 | "rviz_config": rviz_config_file, 122 | }.items(), 123 | ) 124 | 125 | bringup_cmd = IncludeLaunchDescription( 126 | PythonLaunchDescriptionSource( 127 | os.path.join(nav2_bringup_dir, "launch", "bringup_launch.py") 128 | ), 129 | launch_arguments={ 130 | "namespace": namespace, 131 | "slam": slam, 132 | "map": map_yaml_file, 133 | "use_sim_time": use_sim_time, 134 | "params_file": params_file, 135 | "autostart": autostart, 136 | "use_composition": use_composition, 137 | "use_respawn": use_respawn, 138 | }.items(), 139 | ) 140 | 141 | # Create the launch description and populate 142 | ld = LaunchDescription() 143 | 144 | # Declare the launch options 145 | ld.add_action(declare_namespace_cmd) 146 | ld.add_action(declare_slam_cmd) 147 | ld.add_action(declare_map_yaml_cmd) 148 | ld.add_action(declare_use_sim_time_cmd) 149 | ld.add_action(declare_params_file_cmd) 150 | ld.add_action(declare_autostart_cmd) 151 | ld.add_action(declare_use_composition_cmd) 152 | 153 | ld.add_action(declare_rviz_config_file_cmd) 154 | ld.add_action(declare_use_rviz_cmd) 155 | ld.add_action(declare_robot_name_cmd) 156 | ld.add_action(declare_use_respawn_cmd) 157 | 158 | # Add the actions to launch all of the navigation nodes 159 | ld.add_action(sim_cmd) 160 | ld.add_action(rviz_cmd) 161 | ld.add_action(bringup_cmd) 162 | ld.add_action(block_spawner_cmd) 163 | 164 | return ld 165 | -------------------------------------------------------------------------------- /tb_worlds/launch/tb_world.launch.py: -------------------------------------------------------------------------------- 1 | # This file has been modified from 2 | # https://github.com/ros-navigation/navigation2/blob/9d60bc0a3ca4e201250254c866c4eedc1441ed4e/nav2_bringup/launch/tb3_simulation_launch.py 3 | # Modification: 4 | # - Minimal setup for simulation, launching: 5 | # - robot_state_publisher 6 | # - gazebo with (worlds/sim_house.sdf.xacro) world 7 | 8 | import os 9 | import tempfile 10 | 11 | from ament_index_python.packages import get_package_share_directory 12 | 13 | from launch import LaunchDescription 14 | from launch.actions import ( 15 | DeclareLaunchArgument, 16 | ExecuteProcess, 17 | IncludeLaunchDescription, 18 | OpaqueFunction, 19 | RegisterEventHandler, 20 | ) 21 | from launch.event_handlers import OnShutdown 22 | from launch.launch_description_sources import PythonLaunchDescriptionSource 23 | from launch.substitutions import LaunchConfiguration, Command 24 | from launch_ros.actions import Node 25 | 26 | 27 | def generate_launch_description(): 28 | # Get the launch directory 29 | bringup_dir = get_package_share_directory("tb_worlds") 30 | 31 | # Create the launch configuration variables 32 | namespace = LaunchConfiguration("namespace") 33 | use_sim_time = LaunchConfiguration("use_sim_time") 34 | 35 | # Launch configuration variables specific to simulation 36 | world = LaunchConfiguration("world") 37 | pose = { 38 | "x": LaunchConfiguration("x_pose", default="0.0"), 39 | "y": LaunchConfiguration("y_pose", default="0.0"), 40 | "z": LaunchConfiguration("z_pose", default="0.01"), 41 | "R": LaunchConfiguration("roll", default="0.00"), 42 | "P": LaunchConfiguration("pitch", default="0.00"), 43 | "Y": LaunchConfiguration("yaw", default="0.00"), 44 | } 45 | robot_name = LaunchConfiguration("robot_name") 46 | robot_sdf = LaunchConfiguration("robot_sdf") 47 | 48 | remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] 49 | 50 | # Declare the launch arguments 51 | declare_namespace_cmd = DeclareLaunchArgument( 52 | "namespace", default_value="", description="Top-level namespace" 53 | ) 54 | 55 | declare_use_sim_time_cmd = DeclareLaunchArgument( 56 | "use_sim_time", 57 | default_value="true", 58 | description="Use simulation (Gazebo) clock if true", 59 | ) 60 | 61 | declare_world_cmd = DeclareLaunchArgument( 62 | "world", 63 | default_value=os.path.join(bringup_dir, "worlds", "sim_house.sdf.xacro"), 64 | description="Full path to world model file to load", 65 | ) 66 | 67 | declare_robot_name_cmd = DeclareLaunchArgument( 68 | "robot_name", default_value="turtlebot", description="name of the robot" 69 | ) 70 | 71 | declare_robot_sdf_cmd = DeclareLaunchArgument( 72 | "robot_sdf", 73 | default_value=os.path.join(bringup_dir, "urdf", "gz_waffle.sdf.xacro"), 74 | description="Full path to robot sdf file to spawn the robot in gazebo", 75 | ) 76 | 77 | turtlebot_model_os_value = os.getenv("TURTLEBOT_MODEL", "3") 78 | 79 | if turtlebot_model_os_value == "3": 80 | gz_bridge_config = os.path.join( 81 | bringup_dir, "configs", "turtlebot3_bridge.yaml" 82 | ) 83 | urdf = os.path.join( 84 | get_package_share_directory("nav2_minimal_tb3_sim"), 85 | "urdf", 86 | "turtlebot3_waffle.urdf", 87 | ) 88 | with open(urdf, "r") as infp: 89 | robot_description = infp.read() 90 | 91 | else: 92 | # TurtleBot 4 model 93 | gz_bridge_config = os.path.join( 94 | bringup_dir, "configs", "turtlebot4_bridge.yaml" 95 | ) 96 | robot_description = Command( 97 | [ 98 | "xacro", 99 | " ", 100 | os.path.join( 101 | get_package_share_directory("nav2_minimal_tb4_description"), 102 | "urdf", 103 | "standard", 104 | "turtlebot4.urdf.xacro", 105 | ), 106 | ] 107 | ) 108 | 109 | robot_state_publisher_cmd = Node( 110 | package="robot_state_publisher", 111 | executable="robot_state_publisher", 112 | name="robot_state_publisher", 113 | namespace=namespace, 114 | output="screen", 115 | parameters=[ 116 | {"use_sim_time": use_sim_time, "robot_description": robot_description} 117 | ], 118 | remappings=remappings, 119 | ) 120 | 121 | # The SDF file for the world is a xacro file because we wanted to 122 | # conditionally load the SceneBroadcaster plugin based on whether we're 123 | # running in headless mode. But currently, the Gazebo command line doesn't 124 | # take SDF strings for worlds, so the output of xacro needs to be saved into 125 | # a temporary file and passed to Gazebo. 126 | world_sdf = tempfile.mktemp(prefix="tb_", suffix=".sdf") 127 | world_sdf_xacro = ExecuteProcess(cmd=["xacro", "-o", world_sdf, world]) 128 | gazebo = ExecuteProcess( 129 | cmd=["gz", "sim", "-r", world_sdf], 130 | output="screen", 131 | ) 132 | 133 | remove_temp_sdf_file = RegisterEventHandler( 134 | event_handler=OnShutdown( 135 | on_shutdown=[OpaqueFunction(function=lambda _: os.remove(world_sdf))] 136 | ) 137 | ) 138 | 139 | gz_tb_spawner = IncludeLaunchDescription( 140 | PythonLaunchDescriptionSource( 141 | os.path.join(bringup_dir, "launch", "turtlebot_spawner.launch.py") 142 | ), 143 | launch_arguments={ 144 | "namespace": namespace, 145 | "use_sim_time": use_sim_time, 146 | "robot_name": robot_name, 147 | "robot_sdf": robot_sdf, 148 | "gz_bridge_config": gz_bridge_config, 149 | "x_pose": pose["x"], 150 | "y_pose": pose["y"], 151 | "z_pose": pose["z"], 152 | "roll": pose["R"], 153 | "pitch": pose["P"], 154 | "yaw": pose["Y"], 155 | }.items(), 156 | ) 157 | 158 | # Create the launch description 159 | return LaunchDescription( 160 | [ 161 | declare_namespace_cmd, 162 | declare_use_sim_time_cmd, 163 | declare_world_cmd, 164 | declare_robot_name_cmd, 165 | declare_robot_sdf_cmd, 166 | world_sdf_xacro, 167 | remove_temp_sdf_file, 168 | gz_tb_spawner, 169 | gazebo, 170 | robot_state_publisher_cmd, 171 | ] 172 | ) 173 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # TurtleBot Behavior Demos 2 | In this repository, we demonstrate autonomous behavior with a simulated [ROBOTIS TurtleBot3](https://emanual.robotis.com/docs/en/platform/turtlebot3/overview/#overview) or [Clearpath TurtleBot 4](https://clearpathrobotics.com/turtlebot-4/) using Ubuntu 24.04 and ROS 2 Jazzy. 3 | 4 | The autonomy in these examples are designed using **behavior trees**. 5 | For more information, refer to [this blog post](https://roboticseabass.com/2021/05/08/introduction-to-behavior-trees/) or the [Behavior Trees in Robotics and AI textbook](https://arxiv.org/abs/1709.00084). 6 | 7 | This also serves as an example for Docker workflows in ROS based projects. 8 | For more information, refer to [this blog post](https://roboticseabass.com/2023/07/09/updated-guide-docker-and-ros2/). 9 | 10 | For older versions: 11 | 12 | * If you want to use ROS 1, check out the [`noetic`](https://github.com/sea-bass/turtlebot3_behavior_demos/tree/noetic) branch of this repository. 13 | * If you want to use ROS 2 Humble, check out the [`humble`](https://github.com/sea-bass/turtlebot3_behavior_demos/tree/noetic) branch of this repository. 14 | 15 | By Sebastian Castro, 2021-2025 16 | 17 | Other key contributors: 18 | * [Kemal Bektaş](https://github.com/bektaskemal) -- Upgrade to BehaviorTree.CPP v4. 19 | * [ElSayed ElSheikh](https://github.com/elsayedelsheikh) -- Upgrade to new Gazebo. 20 | 21 | --- 22 | 23 | ## Setup 24 | 25 | ### Docker Setup (Recommended) 26 | First, install Docker and Docker Compose using [the official install guide](https://docs.docker.com/engine/install/ubuntu/). 27 | 28 | To run Docker containers with NVIDIA GPU support, you can optionally install the [NVIDIA Container Toolkit](https://github.com/NVIDIA/nvidia-docker). 29 | 30 | 31 | First, clone this repository and go into the top-level folder: 32 | 33 | ``` 34 | git clone https://github.com/sea-bass/turtlebot3_behavior_demos.git 35 | cd turtlebot3_behavior_demos 36 | ``` 37 | 38 | Build the Docker images. 39 | This will take a while and requires approximately 5 GB of disk space. 40 | 41 | ``` 42 | docker compose build 43 | ``` 44 | 45 | ### Local Setup 46 | 47 | If you do not want to use Docker, you can directly clone this package to a ROS 2 workspace and build it provided you have the necessary dependencies. 48 | As long as you can run the examples in the [Nav2 Minimal TurtleBot Simulation repo](https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation), you should be in good shape. 49 | 50 | First, make a ROS 2 workspace and clone this repo there: 51 | 52 | ``` 53 | mkdir -p turtlebot_ws/src 54 | cd turtlebot_ws/src 55 | git clone https://github.com/sea-bass/turtlebot3_behavior_demos.git 56 | ``` 57 | 58 | Clone the external dependencies: 59 | 60 | ``` 61 | sudo apt-get install python3-vcstool 62 | vcs import < turtlebot3_behavior_demos/dependencies.repos 63 | ``` 64 | 65 | Set up any additional dependencies using rosdep: 66 | 67 | ``` 68 | sudo apt update && rosdep install -r --from-paths . --ignore-src --rosdistro $ROS_DISTRO -y 69 | ``` 70 | 71 | Ensure you have the necessary Python packages for these examples: 72 | 73 | ``` 74 | pip3 install matplotlib transforms3d 75 | ``` 76 | 77 | Then, build the workspace. 78 | 79 | ``` 80 | cd turtlebot_ws 81 | colcon build 82 | ``` 83 | 84 | NOTE: For best results, we recommend that you change your ROS Middleware (RMW) implementation to Cyclone DDS by following [these instructions](https://docs.ros.org/en/jazzy/Installation/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html). 85 | 86 | --- 87 | 88 | ## Basic Usage 89 | 90 | We use [Docker Compose](https://docs.docker.com/compose/) to automate building, as shown above, but also for various useful entry points into the Docker container once it has been built. 91 | **All `docker compose` commands below should be run from your host machine, and not from inside the container**. 92 | 93 | To enter a Terminal in the overlay container: 94 | 95 | ``` 96 | docker compose run overlay bash 97 | ``` 98 | 99 | Once inside the container, you can verify that display in Docker works by starting a Gazebo simulation with Nav2 support: 100 | 101 | ``` 102 | ros2 launch tb_worlds tb_demo_world.launch.py 103 | ``` 104 | 105 | Alternatively, you can use the pre-existing `sim` service to do this in a single line: 106 | 107 | ``` 108 | docker compose up demo-world 109 | ``` 110 | 111 | If you want to develop using Docker, you can also launch a dev container using: 112 | 113 | ``` 114 | # Start the dev container 115 | docker compose up dev 116 | 117 | # Open as many interactive shells as you want to the container 118 | docker compose exec -it dev bash 119 | ``` 120 | 121 | --- 122 | 123 | ## Behavior Trees Demo 124 | 125 | In this example, the robot navigates around known locations with the goal of finding a block of a specified color (red, green, or blue). 126 | Object detection is done using simple thresholding in the [HSV color space](https://en.wikipedia.org/wiki/HSL_and_HSV) with calibrated values. 127 | 128 | To start the demo world, run the following command: 129 | 130 | ``` 131 | docker compose up demo-world 132 | ``` 133 | 134 | ### Behavior Trees in Python 135 | 136 | To start the Python based demo, which uses [`py_trees`](https://py-trees.readthedocs.io/en/devel/): 137 | 138 | ``` 139 | docker compose up demo-behavior-py 140 | ``` 141 | 142 | You can also change the following environment variables to set arguments for the launch files, or by modifying the defaults in the `.env` file: 143 | 144 | ``` 145 | TARGET_COLOR=green BT_TYPE=queue ENABLE_VISION=true TURTLEBOT_MODEL=4 docker compose up demo-behavior-py 146 | ``` 147 | 148 | Note that the behavior tree viewer ([`py_trees_ros_viewer`](https://github.com/splintered-reality/py_trees_ros_viewer)) should automatically discover the ROS node containing the behavior tree and visualize it. 149 | 150 | After starting the commands above (plus doing some waiting and window rearranging), you should see the following. 151 | The labeled images will appear once the robot reaches a target location. 152 | 153 | ![Example demo screenshot](./media/demo_screenshot_python.png) 154 | 155 | ### Behavior Trees in C++ 156 | 157 | If you want to use BehaviorTree.CPP and Groot2 for visualization, [download Groot2 from the website](https://www.behaviortree.dev/groot/). 158 | To be consistent with the repository, download the AppImage and save it to your `$HOME` folder. 159 | 160 | To start the C++ demo, which uses [`BehaviorTree.CPP`](https://www.behaviortree.dev/): 161 | 162 | ``` 163 | docker compose up demo-behavior-cpp 164 | ``` 165 | 166 | You can also change the following environment variables to set arguments for the launch files, or by modifying the defaults in the `.env` file: 167 | 168 | ``` 169 | TARGET_COLOR=green BT_TYPE=queue ENABLE_VISION=true TURTLEBOT_MODEL=4 docker compose up demo-behavior-cpp 170 | ``` 171 | 172 | This example uses the behavior tree viewer ([`Groot2`](https://github.com/BehaviorTree/Groot2)). 173 | 174 | After starting the commands above (plus doing some waiting and window rearranging), you should see the following. 175 | The labeled images will appear once the robot reaches a target location. 176 | 177 | NOTE: You will need the PRO version of Groot2 to view live behavior tree updates. 178 | If you are a student or involved in academic work, you can get a free license to try this out. 179 | Refer to [the Groot2 website](https://www.behaviortree.dev/groot/) for more information. 180 | 181 | ![Example demo screenshot](./media/demo_screenshot_cpp.png) 182 | -------------------------------------------------------------------------------- /tb_autonomy/src/navigation_behaviors.cpp: -------------------------------------------------------------------------------- 1 | // Navigation Related Behaviors 2 | 3 | #include "navigation_behaviors.h" 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | // SETLOCATIONS 10 | // Gets a list of locations from a YAML file and ensures they are not empty 11 | SetLocations::SetLocations(const std::string& name, const BT::NodeConfig& config) : 12 | BT::SyncActionNode(name, config) 13 | { 14 | std::cout << "[" << this->name() << "] Initialized" << std::endl; 15 | } 16 | 17 | BT::NodeStatus SetLocations::tick() 18 | { 19 | std::string location_file; 20 | const auto result = config().blackboard->get("location_file", location_file); 21 | if (!result) { 22 | std::cerr << "[" << this->name() << "] Could not read locations file from blackboard." << std::endl; 23 | return BT::NodeStatus::FAILURE; 24 | } 25 | 26 | try { 27 | YAML::Node locations = YAML::LoadFile(location_file); 28 | int num_locs = locations.size(); 29 | if (num_locs == 0) { 30 | std::cerr << "[" << this->name() << "] No locations found." << std::endl; 31 | return BT::NodeStatus::FAILURE; 32 | } 33 | setOutput("num_locs", num_locs); 34 | std::cout << "[" << this->name() << "] Found " << num_locs << " locations." << std::endl; 35 | 36 | std::deque location_names{}; 37 | std::map location_poses{}; 38 | for (YAML::const_iterator it=locations.begin(); it!=locations.end(); ++it) { 39 | const auto name = it->first.as(); 40 | location_names.push_back(name); 41 | const Pose pose = it->second.as(); 42 | location_poses.emplace(name, pose); 43 | } 44 | // Shuffle location names to get random order in each run 45 | std::random_device rd; 46 | std::mt19937 rng(rd()); 47 | std::shuffle(location_names.begin(), location_names.end(), rng); 48 | setOutput("loc_names", location_names); 49 | setOutput("loc_poses", location_poses); 50 | 51 | } catch (YAML::Exception const& e) { 52 | std::cerr << "Couldn't load locations file: " << location_file << ". Error: " << e.what() << std::endl; 53 | return BT::NodeStatus::FAILURE; 54 | } 55 | 56 | return BT::NodeStatus::SUCCESS; 57 | } 58 | 59 | BT::PortsList SetLocations::providedPorts() 60 | { 61 | return { BT::OutputPort("num_locs"), 62 | BT::OutputPort>("loc_names"), 63 | BT::OutputPort>("loc_poses") 64 | }; 65 | } 66 | 67 | 68 | // GETLOCATIONFROMQUEUE 69 | // Gets a location name from a queue of locations to visit. 70 | // If the queue is empty, this behavior fails. 71 | GetLocationFromQueue::GetLocationFromQueue(const std::string& name, 72 | const BT::NodeConfig& config) : 73 | BT::SyncActionNode(name, config) 74 | { 75 | std::cout << "[" << this->name() << "] Initialized" << std::endl; 76 | } 77 | 78 | BT::NodeStatus GetLocationFromQueue::tick() 79 | { 80 | // Get the locations from the port and select first one as the next target 81 | auto location_queue_ = getInput>("loc_names"); 82 | if (!location_queue_) { 83 | std::cerr << "Couldn't get loc_names!" << std::endl; 84 | } 85 | if (location_queue_.value().empty()) { 86 | std::cout << "[" << this->name() << "] No more locations!" << std::endl; 87 | return BT::NodeStatus::FAILURE; 88 | } else { 89 | std::string tgt_loc = location_queue_.value().front(); 90 | setOutput("target_location", tgt_loc); 91 | location_queue_.value().pop_front(); 92 | std::cout << "[" << this->name() << "] Targeting location: " << tgt_loc << std::endl; 93 | setOutput("loc_names", location_queue_.value()); 94 | return BT::NodeStatus::SUCCESS; 95 | } 96 | } 97 | 98 | BT::PortsList GetLocationFromQueue::providedPorts() 99 | { 100 | return { BT::OutputPort("target_location"), 101 | BT::BidirectionalPort>("loc_names") }; 102 | } 103 | 104 | // GOTOPOSE 105 | // Wrapper behavior around the `navigate_to_pose` action client, 106 | // whose status reflects the status of the ROS action. 107 | GoToPose::GoToPose(const std::string& name, const BT::NodeConfig& config, 108 | rclcpp::Node::SharedPtr node_ptr) : 109 | BT::StatefulActionNode(name, config), node_ptr_{node_ptr} {} 110 | 111 | BT::NodeStatus GoToPose::onStart() { 112 | // Validate that a node exists 113 | if (!node_ptr_) { 114 | std::cout << "ROS2 node not registered via init() method" << std::endl; 115 | return BT::NodeStatus::FAILURE; 116 | } 117 | 118 | // Read the YAML file 119 | BT::Expected loc = getInput("loc"); 120 | 121 | auto location_poses = getInput>("loc_poses"); 122 | if (!location_poses){ 123 | std::cerr << "Couldn't get loc_poses!" << std::endl; 124 | return BT::NodeStatus::FAILURE; 125 | } 126 | auto target_loc = getInput("loc"); 127 | if (!target_loc) { 128 | std::cerr << "Couldn't get target loc!" << std::endl; 129 | return BT::NodeStatus::FAILURE; 130 | } 131 | auto target_pose = location_poses.value().at(target_loc.value()); 132 | 133 | // Set up the action client 134 | using namespace std::placeholders; 135 | auto send_goal_options = 136 | rclcpp_action::Client::SendGoalOptions(); 137 | send_goal_options.result_callback = 138 | std::bind(&GoToPose::result_callback, this, _1); 139 | client_ptr_ = rclcpp_action::create_client( 140 | node_ptr_, "/navigate_to_pose"); 141 | 142 | // Package up the the goal 143 | auto goal_msg = NavigateToPose::Goal(); 144 | goal_msg.pose.header.frame_id = "map"; 145 | goal_msg.pose.pose.position.x = target_pose.x; 146 | goal_msg.pose.pose.position.y = target_pose.y; 147 | tf2::Quaternion q; 148 | q.setRPY(0, 0, target_pose.theta); 149 | q.normalize(); 150 | goal_msg.pose.pose.orientation = tf2::toMsg(q); 151 | 152 | // Send the navigation action goal. 153 | done_flag_ = false; 154 | client_ptr_->async_send_goal(goal_msg, send_goal_options); 155 | std::cout << "[" << this->name() << "] Sent goal message" << std::endl; 156 | return BT::NodeStatus::RUNNING; 157 | } 158 | 159 | BT::NodeStatus GoToPose::onRunning() { 160 | // If there is a result, we can check the status of the action directly. 161 | // Otherwise, the action is still running. 162 | if (done_flag_) { 163 | if (nav_result_ == rclcpp_action::ResultCode::SUCCEEDED) { 164 | std::cout << "[" << this->name() << "] Goal reached" << std::endl; 165 | return BT::NodeStatus::SUCCESS; 166 | } else { 167 | std::cout << "[" << this->name() << "] Failed to reach goal" << std::endl; 168 | return BT::NodeStatus::FAILURE; 169 | } 170 | } else { 171 | return BT::NodeStatus::RUNNING; 172 | } 173 | } 174 | 175 | BT::PortsList GoToPose::providedPorts() { 176 | return { BT::InputPort("loc"), 177 | BT::InputPort>("loc_poses") }; 178 | } 179 | 180 | void GoToPose::result_callback(const GoalHandleNav::WrappedResult& result) { 181 | // If there is a result, we consider navigation completed and save the 182 | // result code to be checked in the `onRunning()` method. 183 | if (result.result) { 184 | done_flag_ = true; 185 | nav_result_ = result.code; 186 | } 187 | } 188 | -------------------------------------------------------------------------------- /tb_worlds/configs/nav2_params.yaml: -------------------------------------------------------------------------------- 1 | # This file has been modified from 2 | # https://github.com/ros-navigation/navigation2/blob/3ee33f73a211f7e75c6f147644ec6ff275b94c88/nav2_bringup/params/nav2_params.yaml 3 | # Modification: 4 | # - Set initial_pose for AMCL 5 | # - Use stamped_cmd_vel 6 | 7 | /**: 8 | ros__parameters: 9 | enable_stamped_cmd_vel: true 10 | 11 | amcl: 12 | ros__parameters: 13 | alpha1: 0.2 14 | alpha2: 0.2 15 | alpha3: 0.2 16 | alpha4: 0.2 17 | alpha5: 0.2 18 | base_frame_id: "base_footprint" 19 | beam_skip_distance: 0.5 20 | beam_skip_error_threshold: 0.9 21 | beam_skip_threshold: 0.3 22 | do_beamskip: false 23 | global_frame_id: "map" 24 | lambda_short: 0.1 25 | laser_likelihood_max_dist: 2.0 26 | laser_max_range: 100.0 27 | laser_min_range: -1.0 28 | laser_model_type: "likelihood_field" 29 | max_beams: 60 30 | max_particles: 2000 31 | min_particles: 500 32 | odom_frame_id: "odom" 33 | pf_err: 0.05 34 | pf_z: 0.99 35 | recovery_alpha_fast: 0.0 36 | recovery_alpha_slow: 0.0 37 | resample_interval: 1 38 | robot_model_type: "nav2_amcl::DifferentialMotionModel" 39 | save_pose_rate: 0.5 40 | sigma_hit: 0.2 41 | tf_broadcast: true 42 | transform_tolerance: 1.0 43 | update_min_a: 0.2 44 | update_min_d: 0.25 45 | z_hit: 0.5 46 | z_max: 0.05 47 | z_rand: 0.5 48 | z_short: 0.05 49 | scan_topic: scan 50 | set_initial_pose: true 51 | always_reset_initial_pose: false 52 | initial_pose: 53 | x: 0.6 54 | y: 0.6 55 | z: 0.0 56 | yaw: 0.0 57 | 58 | bt_navigator: 59 | ros__parameters: 60 | global_frame: map 61 | robot_base_frame: base_link 62 | odom_topic: /odom 63 | bt_loop_duration: 10 64 | default_server_timeout: 20 65 | wait_for_service_timeout: 1000 66 | action_server_result_timeout: 900.0 67 | navigators: ["navigate_to_pose", "navigate_through_poses"] 68 | navigate_to_pose: 69 | plugin: "nav2_bt_navigator::NavigateToPoseNavigator" 70 | navigate_through_poses: 71 | plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" 72 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 73 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 74 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 75 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 76 | 77 | # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). 78 | # Built-in plugins are added automatically 79 | # plugin_lib_names: [] 80 | 81 | error_code_names: 82 | - compute_path_error_code 83 | - follow_path_error_code 84 | 85 | controller_server: 86 | ros__parameters: 87 | controller_frequency: 20.0 88 | costmap_update_timeout: 0.30 89 | min_x_velocity_threshold: 0.001 90 | min_y_velocity_threshold: 0.5 91 | min_theta_velocity_threshold: 0.001 92 | failure_tolerance: 0.3 93 | progress_checker_plugins: ["progress_checker"] 94 | goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" 95 | controller_plugins: ["FollowPath"] 96 | use_realtime_priority: false 97 | 98 | # Progress checker parameters 99 | progress_checker: 100 | plugin: "nav2_controller::SimpleProgressChecker" 101 | required_movement_radius: 0.5 102 | movement_time_allowance: 10.0 103 | # Goal checker parameters 104 | #precise_goal_checker: 105 | # plugin: "nav2_controller::SimpleGoalChecker" 106 | # xy_goal_tolerance: 0.25 107 | # yaw_goal_tolerance: 0.25 108 | # stateful: True 109 | general_goal_checker: 110 | stateful: True 111 | plugin: "nav2_controller::SimpleGoalChecker" 112 | xy_goal_tolerance: 0.25 113 | yaw_goal_tolerance: 0.25 114 | FollowPath: 115 | plugin: "nav2_mppi_controller::MPPIController" 116 | time_steps: 56 117 | model_dt: 0.05 118 | batch_size: 2000 119 | ax_max: 3.0 120 | ax_min: -3.0 121 | ay_max: 3.0 122 | ay_min: -3.0 123 | az_max: 3.5 124 | vx_std: 0.2 125 | vy_std: 0.2 126 | wz_std: 0.4 127 | vx_max: 0.5 128 | vx_min: -0.35 129 | vy_max: 0.5 130 | wz_max: 1.9 131 | iteration_count: 1 132 | prune_distance: 1.7 133 | transform_tolerance: 0.1 134 | temperature: 0.3 135 | gamma: 0.015 136 | motion_model: "DiffDrive" 137 | visualize: true 138 | regenerate_noises: true 139 | TrajectoryVisualizer: 140 | trajectory_step: 5 141 | time_step: 3 142 | AckermannConstraints: 143 | min_turning_r: 0.2 144 | critics: [ 145 | "ConstraintCritic", "CostCritic", "GoalCritic", 146 | "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", 147 | "PathAngleCritic", "PreferForwardCritic"] 148 | ConstraintCritic: 149 | enabled: true 150 | cost_power: 1 151 | cost_weight: 4.0 152 | GoalCritic: 153 | enabled: true 154 | cost_power: 1 155 | cost_weight: 5.0 156 | threshold_to_consider: 1.4 157 | GoalAngleCritic: 158 | enabled: true 159 | cost_power: 1 160 | cost_weight: 3.0 161 | threshold_to_consider: 0.5 162 | PreferForwardCritic: 163 | enabled: true 164 | cost_power: 1 165 | cost_weight: 5.0 166 | threshold_to_consider: 0.5 167 | CostCritic: 168 | enabled: true 169 | cost_power: 1 170 | cost_weight: 3.81 171 | near_collision_cost: 253 172 | critical_cost: 300.0 173 | consider_footprint: false 174 | collision_cost: 1000000.0 175 | near_goal_distance: 1.0 176 | trajectory_point_step: 2 177 | PathAlignCritic: 178 | enabled: true 179 | cost_power: 1 180 | cost_weight: 14.0 181 | max_path_occupancy_ratio: 0.05 182 | trajectory_point_step: 4 183 | threshold_to_consider: 0.5 184 | offset_from_furthest: 20 185 | use_path_orientations: false 186 | PathFollowCritic: 187 | enabled: true 188 | cost_power: 1 189 | cost_weight: 5.0 190 | offset_from_furthest: 5 191 | threshold_to_consider: 1.4 192 | PathAngleCritic: 193 | enabled: true 194 | cost_power: 1 195 | cost_weight: 2.0 196 | offset_from_furthest: 4 197 | threshold_to_consider: 0.5 198 | max_angle_to_furthest: 1.0 199 | mode: 0 200 | # TwirlingCritic: 201 | # enabled: true 202 | # twirling_cost_power: 1 203 | # twirling_cost_weight: 10.0 204 | 205 | local_costmap: 206 | local_costmap: 207 | ros__parameters: 208 | update_frequency: 5.0 209 | publish_frequency: 2.0 210 | global_frame: odom 211 | robot_base_frame: base_link 212 | rolling_window: true 213 | width: 3 214 | height: 3 215 | resolution: 0.05 216 | robot_radius: 0.22 217 | plugins: ["voxel_layer", "inflation_layer"] 218 | inflation_layer: 219 | plugin: "nav2_costmap_2d::InflationLayer" 220 | cost_scaling_factor: 3.0 221 | inflation_radius: 0.70 222 | voxel_layer: 223 | plugin: "nav2_costmap_2d::VoxelLayer" 224 | enabled: True 225 | publish_voxel_map: True 226 | origin_z: 0.0 227 | z_resolution: 0.05 228 | z_voxels: 16 229 | max_obstacle_height: 2.0 230 | mark_threshold: 0 231 | observation_sources: scan 232 | scan: 233 | topic: /scan 234 | max_obstacle_height: 2.0 235 | clearing: True 236 | marking: True 237 | data_type: "LaserScan" 238 | raytrace_max_range: 3.0 239 | raytrace_min_range: 0.0 240 | obstacle_max_range: 2.5 241 | obstacle_min_range: 0.0 242 | static_layer: 243 | plugin: "nav2_costmap_2d::StaticLayer" 244 | map_subscribe_transient_local: True 245 | always_send_full_costmap: True 246 | 247 | global_costmap: 248 | global_costmap: 249 | ros__parameters: 250 | update_frequency: 1.0 251 | publish_frequency: 1.0 252 | global_frame: map 253 | robot_base_frame: base_link 254 | robot_radius: 0.22 255 | resolution: 0.05 256 | track_unknown_space: true 257 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 258 | obstacle_layer: 259 | plugin: "nav2_costmap_2d::ObstacleLayer" 260 | enabled: True 261 | observation_sources: scan 262 | scan: 263 | topic: /scan 264 | max_obstacle_height: 2.0 265 | clearing: True 266 | marking: True 267 | data_type: "LaserScan" 268 | raytrace_max_range: 3.0 269 | raytrace_min_range: 0.0 270 | obstacle_max_range: 2.5 271 | obstacle_min_range: 0.0 272 | static_layer: 273 | plugin: "nav2_costmap_2d::StaticLayer" 274 | map_subscribe_transient_local: True 275 | inflation_layer: 276 | plugin: "nav2_costmap_2d::InflationLayer" 277 | cost_scaling_factor: 3.0 278 | inflation_radius: 0.7 279 | always_send_full_costmap: True 280 | 281 | # The yaml_filename does not need to be specified since it going to be set by defaults in launch. 282 | # If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py 283 | # file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. 284 | # map_server: 285 | # ros__parameters: 286 | # yaml_filename: "" 287 | 288 | map_saver: 289 | ros__parameters: 290 | save_map_timeout: 5.0 291 | free_thresh_default: 0.25 292 | occupied_thresh_default: 0.65 293 | map_subscribe_transient_local: True 294 | 295 | planner_server: 296 | ros__parameters: 297 | expected_planner_frequency: 20.0 298 | planner_plugins: ["GridBased"] 299 | costmap_update_timeout: 1.0 300 | GridBased: 301 | plugin: "nav2_navfn_planner::NavfnPlanner" 302 | tolerance: 0.5 303 | use_astar: false 304 | allow_unknown: true 305 | 306 | smoother_server: 307 | ros__parameters: 308 | smoother_plugins: ["simple_smoother"] 309 | simple_smoother: 310 | plugin: "nav2_smoother::SimpleSmoother" 311 | tolerance: 1.0e-10 312 | max_its: 1000 313 | do_refinement: True 314 | 315 | behavior_server: 316 | ros__parameters: 317 | local_costmap_topic: local_costmap/costmap_raw 318 | global_costmap_topic: global_costmap/costmap_raw 319 | local_footprint_topic: local_costmap/published_footprint 320 | global_footprint_topic: global_costmap/published_footprint 321 | cycle_frequency: 10.0 322 | behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] 323 | spin: 324 | plugin: "nav2_behaviors::Spin" 325 | backup: 326 | plugin: "nav2_behaviors::BackUp" 327 | drive_on_heading: 328 | plugin: "nav2_behaviors::DriveOnHeading" 329 | wait: 330 | plugin: "nav2_behaviors::Wait" 331 | assisted_teleop: 332 | plugin: "nav2_behaviors::AssistedTeleop" 333 | local_frame: odom 334 | global_frame: map 335 | robot_base_frame: base_link 336 | transform_tolerance: 0.1 337 | simulate_ahead_time: 2.0 338 | max_rotational_vel: 1.0 339 | min_rotational_vel: 0.4 340 | rotational_acc_lim: 3.2 341 | 342 | waypoint_follower: 343 | ros__parameters: 344 | loop_rate: 20 345 | stop_on_failure: false 346 | action_server_result_timeout: 900.0 347 | waypoint_task_executor_plugin: "wait_at_waypoint" 348 | wait_at_waypoint: 349 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 350 | enabled: True 351 | waypoint_pause_duration: 200 352 | 353 | velocity_smoother: 354 | ros__parameters: 355 | smoothing_frequency: 20.0 356 | scale_velocities: False 357 | feedback: "OPEN_LOOP" 358 | max_velocity: [0.5, 0.0, 2.0] 359 | min_velocity: [-0.5, 0.0, -2.0] 360 | max_accel: [2.5, 0.0, 3.2] 361 | max_decel: [-2.5, 0.0, -3.2] 362 | odom_topic: "odom" 363 | odom_duration: 0.1 364 | deadband_velocity: [0.0, 0.0, 0.0] 365 | velocity_timeout: 1.0 366 | 367 | collision_monitor: 368 | ros__parameters: 369 | base_frame_id: "base_footprint" 370 | odom_frame_id: "odom" 371 | cmd_vel_in_topic: "cmd_vel_smoothed" 372 | cmd_vel_out_topic: "cmd_vel" 373 | state_topic: "collision_monitor_state" 374 | transform_tolerance: 0.2 375 | source_timeout: 1.0 376 | base_shift_correction: True 377 | stop_pub_timeout: 2.0 378 | # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, 379 | # and robot footprint for "approach" action type. 380 | polygons: ["FootprintApproach"] 381 | FootprintApproach: 382 | type: "polygon" 383 | action_type: "approach" 384 | footprint_topic: "/local_costmap/published_footprint" 385 | time_before_collision: 1.2 386 | simulation_time_step: 0.1 387 | min_points: 6 388 | visualize: False 389 | enabled: True 390 | observation_sources: ["scan"] 391 | scan: 392 | type: "scan" 393 | topic: "scan" 394 | min_height: 0.15 395 | max_height: 2.0 396 | enabled: True 397 | 398 | docking_server: 399 | ros__parameters: 400 | controller_frequency: 50.0 401 | initial_perception_timeout: 5.0 402 | wait_charge_timeout: 5.0 403 | dock_approach_timeout: 30.0 404 | undock_linear_tolerance: 0.05 405 | undock_angular_tolerance: 0.1 406 | max_retries: 3 407 | base_frame: "base_link" 408 | fixed_frame: "odom" 409 | dock_backwards: false 410 | dock_prestaging_tolerance: 0.5 411 | 412 | # Types of docks 413 | dock_plugins: ['simple_charging_dock'] 414 | simple_charging_dock: 415 | plugin: 'opennav_docking::SimpleChargingDock' 416 | docking_threshold: 0.05 417 | staging_x_offset: -0.7 418 | use_external_detection_pose: true 419 | use_battery_status: false # true 420 | use_stall_detection: false # true 421 | 422 | external_detection_timeout: 1.0 423 | external_detection_translation_x: -0.18 424 | external_detection_translation_y: 0.0 425 | external_detection_rotation_roll: -1.57 426 | external_detection_rotation_pitch: -1.57 427 | external_detection_rotation_yaw: 0.0 428 | filter_coef: 0.1 429 | 430 | # Dock instances 431 | # The following example illustrates configuring dock instances. 432 | # docks: ['home_dock'] # Input your docks here 433 | # home_dock: 434 | # type: 'simple_charging_dock' 435 | # frame: map 436 | # pose: [0.0, 0.0, 0.0] 437 | 438 | controller: 439 | k_phi: 3.0 440 | k_delta: 2.0 441 | v_linear_min: 0.15 442 | v_linear_max: 0.15 443 | use_collision_detection: true 444 | costmap_topic: "local_costmap/costmap_raw" 445 | footprint_topic: "local_costmap/published_footprint" 446 | transform_tolerance: 0.1 447 | projection_time: 5.0 448 | simulation_step: 0.1 449 | dock_collision_threshold: 0.3 450 | 451 | loopback_simulator: 452 | ros__parameters: 453 | base_frame_id: "base_footprint" 454 | odom_frame_id: "odom" 455 | map_frame_id: "map" 456 | scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' 457 | update_duration: 0.02 458 | scan_range_min: 0.05 459 | scan_range_max: 30.0 460 | scan_angle_min: -3.1415 461 | scan_angle_max: 3.1415 462 | scan_angle_increment: 0.02617 463 | scan_use_inf: true 464 | -------------------------------------------------------------------------------- /tb_worlds/urdf/gz_waffle.sdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 9 | 10 | 11 | 12 | 0.0 0.0 0.0 0.0 0.0 0.0 13 | 14 | 15 | 16 | 17 | 18 | 19 | -0.064 0 0.048 0 0 0 20 | 21 | 0.001 22 | 0.000 23 | 0.000 24 | 0.001 25 | 0.000 26 | 0.001 27 | 28 | 1.0 29 | 30 | 31 | 32 | -0.064 0 0.048 0 0 0 33 | 34 | 35 | 0.265 0.265 0.089 36 | 37 | 38 | 39 | 40 | 41 | -0.064 0 0 0 0 0 42 | 43 | 44 | package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/waffle_base.dae 45 | 0.001 0.001 0.001 46 | 47 | 48 | 49 | ${3/255} ${3/255} ${3/255} 1 50 | 0 0 0 1 51 | 0 0 0 1 52 | 53 | 54 | 55 | 56 | 57 | 58 | true 59 | 200 60 | $(arg namespace)/imu 61 | imu_link 62 | 63 | 64 | 65 | 66 | 0.0 67 | 2e-4 68 | 69 | 70 | 71 | 72 | 0.0 73 | 2e-4 74 | 75 | 76 | 77 | 78 | 0.0 79 | 2e-4 80 | 81 | 82 | 83 | 84 | 85 | 86 | 0.0 87 | 1.7e-2 88 | 89 | 90 | 91 | 92 | 0.0 93 | 1.7e-2 94 | 95 | 96 | 97 | 98 | 0.0 99 | 1.7e-2 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | -0.064 0 0.121 0 0 0 110 | 111 | 0.001 112 | 0.000 113 | 0.000 114 | 0.001 115 | 0.000 116 | 0.001 117 | 118 | 0.125 119 | 120 | 121 | 122 | -0.052 0 0.111 0 0 0 123 | 124 | 125 | 0.0508 126 | 0.055 127 | 128 | 129 | 130 | 131 | 132 | -0.064 0 0.121 0 0 0 133 | 134 | 135 | package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/lds.dae 136 | 0.001 0.001 0.001 137 | 138 | 139 | 140 | 1 1 1 1 141 | ${80/255} ${80/255} ${80/255} 1 142 | 0 0 0 1 143 | 144 | 145 | 146 | 147 | true 148 | true 149 | -0.064 0 0.15 0 0 0 150 | 5 151 | $(arg namespace)/scan 152 | base_scan 153 | 154 | 155 | 156 | 360 157 | 1.000000 158 | 0.000000 159 | 6.280000 160 | 161 | 162 | 163 | 0.00001 164 | 20.0 165 | 0.015000 166 | 167 | 168 | gaussian 169 | 0.0 170 | 0.01 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 0.0 0.144 0.023 -1.57 0 0 180 | 181 | 0.001 182 | 0.000 183 | 0.000 184 | 0.001 185 | 0.000 186 | 0.001 187 | 188 | 0.1 189 | 190 | 191 | 192 | 0.0 0.144 0.023 -1.57 0 0 193 | 194 | 195 | 0.033 196 | 0.018 197 | 198 | 199 | 200 | 201 | 202 | 1 203 | 1 204 | 0.035 205 | 0 206 | 0 0 1 207 | 208 | 209 | 210 | 211 | 0 212 | 0.2 213 | 1e+5 214 | 1 215 | 0.01 216 | 0.001 217 | 218 | 219 | 220 | 221 | 222 | 223 | 0.0 0.144 0.023 0 0 0 224 | 225 | 226 | package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/tire.dae 227 | 0.001 0.001 0.001 228 | 229 | 230 | 231 | 0 0 0 1 232 | ${80/255} ${80/255} ${80/255} 1 233 | 0 0 0 1 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 0.0 -0.144 0.023 -1.57 0 0 242 | 243 | 0.001 244 | 0.000 245 | 0.000 246 | 0.001 247 | 0.000 248 | 0.001 249 | 250 | 0.1 251 | 252 | 253 | 254 | 0.0 -0.144 0.023 -1.57 0 0 255 | 256 | 257 | 0.033 258 | 0.018 259 | 260 | 261 | 262 | 263 | 264 | 1 265 | 1 266 | 0.035 267 | 0 268 | 0 0 1 269 | 270 | 271 | 272 | 273 | 0 274 | 0.2 275 | 1e+5 276 | 1 277 | 0.01 278 | 0.001 279 | 280 | 281 | 282 | 283 | 284 | 285 | 0.0 -0.144 0.023 0 0 0 286 | 287 | 288 | package://nav2_minimal_tb3_sim/models/turtlebot3_model/meshes/tire.dae 289 | 0.001 0.001 0.001 290 | 291 | 292 | 293 | 0 0 0 1 294 | ${80/255} ${80/255} ${80/255} 1 295 | 0 0 0 1 296 | 297 | 298 | 299 | 300 | 301 | -0.177 -0.064 -0.004 0 0 0 302 | 303 | 0.001 304 | 305 | 0.00001 306 | 0.000 307 | 0.000 308 | 0.00001 309 | 0.000 310 | 0.00001 311 | 312 | 313 | 314 | 315 | 316 | 0.005000 317 | 318 | 319 | 320 | 321 | 322 | 0 323 | 0.2 324 | 1e+5 325 | 1 326 | 0.01 327 | 0.001 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | -0.177 0.064 -0.004 0 0 0 336 | 337 | 0.001 338 | 339 | 0.00001 340 | 0.000 341 | 0.000 342 | 0.00001 343 | 0.000 344 | 0.00001 345 | 346 | 347 | 348 | 349 | 350 | 0.005000 351 | 352 | 353 | 354 | 355 | 356 | 0 357 | 0.2 358 | 1e+5 359 | 1 360 | 0.01 361 | 0.001 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 0.069 -0.047 0.107 0 0 0 371 | 372 | 0.001 373 | 0.000 374 | 0.000 375 | 0.001 376 | 0.000 377 | 0.001 378 | 379 | 0.035 380 | 381 | 382 | 0 0.047 -0.005 0 0 0 383 | 384 | 385 | 0.008 0.130 0.022 386 | 387 | 388 | 389 | 390 | 0.069 -0.047 0.107 0 0 0 391 | 392 | 393 | 1 394 | 0.064 -0.047 0.107 0 0 0 395 | camera_depth_frame 396 | 397 | 1.25 398 | 399 | 320 400 | 240 401 | 402 | 403 | 0.001 404 | 100 405 | 406 | 407 | 20.0 408 | realsense_camera 409 | 410 | 411 | 412 | 413 | base_footprint 414 | base_link 415 | 0.0 0.0 0.010 0 0 0 416 | 417 | 418 | 419 | base_link 420 | wheel_left_link 421 | 0.0 0.144 0.023 -1.57 0 0 422 | 423 | 0 0 1 424 | 425 | 426 | 427 | 428 | base_link 429 | wheel_right_link 430 | 0.0 -0.144 0.023 -1.57 0 0 431 | 432 | 0 0 1 433 | 434 | 435 | 436 | 437 | base_link 438 | caster_back_right_link 439 | 440 | 441 | 442 | base_link 443 | caster_back_left_link 444 | 445 | 446 | 447 | base_link 448 | base_scan 449 | -0.064 0 0.121 0 0 0 450 | 451 | 0 0 1 452 | 453 | 454 | 455 | 456 | base_link 457 | camera_link 458 | 0.064 -0.065 0.094 0 0 0 459 | 460 | 0 0 1 461 | 462 | 463 | 464 | 465 | 468 | wheel_left_joint 469 | wheel_right_joint 470 | 0.287 471 | 0.033 472 | 1 473 | -1 474 | 2 475 | -2 476 | 0.46 477 | -0.46 478 | 1.9 479 | -1.9 480 | $(arg namespace)/cmd_vel 481 | $(arg namespace)/odom 482 | $(arg namespace)/tf 483 | odom 484 | base_footprint 485 | 30 486 | 487 | 488 | 491 | wheel_left_joint 492 | wheel_right_joint 493 | $(arg namespace)/joint_states 494 | 30 495 | 496 | 497 | 498 | 499 | -------------------------------------------------------------------------------- /tb_worlds/models/sim_house/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1.21598 -2.10912 0 0 -0 0 5 | 6 | 7 | 8 | 9 | 2.5 0.15 1.0 10 | 11 | 12 | 0 0 0.5 0 -0 0 13 | 14 | 15 | 0 0 0.5 0 -0 0 16 | 17 | 18 | 2.5 0.15 1.0 19 | 20 | 21 | 22 | 26 | 1 1 1 1 27 | 28 | 29 | 0 30 | 31 | 32 | -2.70098 -3.18088 0 0 -0 0 33 | 34 | 35 | 36 | 37 | 38 | 0.75 0.15 1.0 39 | 40 | 41 | 0 0 0.5 0 -0 0 42 | 43 | 44 | 0 0 0.5 0 -0 0 45 | 46 | 47 | 0.75 0.15 1.0 48 | 49 | 50 | 51 | 55 | 1 1 1 1 56 | 57 | 58 | 0 59 | 60 | 61 | -1.52598 -2.88088 0 0 -0 1.5708 62 | 63 | 64 | 65 | 66 | 67 | 3.5 0.15 1.0 68 | 69 | 70 | 0 0 0.5 0 -0 0 71 | 72 | 73 | 0 0 0.5 0 -0 0 74 | 75 | 76 | 3.5 0.15 1.0 77 | 78 | 79 | 80 | 84 | 1 1 1 1 85 | 86 | 87 | 0 88 | 89 | 90 | -2.79863 1.99647 0 0 -0 -2.35619 91 | 92 | 93 | 94 | 95 | 96 | 4.153 0.15 1.0 97 | 98 | 99 | 0 0 0.5 0 -0 0 100 | 101 | 102 | 0 0 0.5 0 -0 0 103 | 104 | 105 | 4.153 0.15 1.0 106 | 107 | 108 | 109 | 113 | 1 1 1 1 114 | 115 | 116 | 0 117 | 118 | 119 | -3.98298 -1.18938 0 0 -0 -1.5708 120 | 121 | 122 | 123 | 124 | 125 | 0.75 0.15 1.0 126 | 127 | 128 | 0 0 0.5 0 -0 0 129 | 130 | 131 | 0 0 0.5 0 -0 0 132 | 133 | 134 | 0.75 0.15 1.0 135 | 136 | 137 | 138 | 142 | 1 1 1 1 143 | 144 | 145 | 0 146 | 147 | 148 | -3.6653 0.087099 0 0 -0 0 149 | 150 | 151 | 152 | 153 | 154 | 1.76432 0.15 1.0 155 | 156 | 157 | 0 0 0.5 0 -0 0 158 | 159 | 160 | 0 0 0.5 0 -0 0 161 | 162 | 163 | 1.76432 0.15 1.0 164 | 165 | 166 | 167 | 171 | 1 1 1 1 172 | 173 | 174 | 0 175 | 176 | 177 | -3.3653 -0.720058 0 0 -0 -1.5708 178 | 179 | 180 | 181 | 182 | 183 | 0.75 0.15 1.0 184 | 185 | 186 | 0 0 0.5 0 -0 0 187 | 188 | 189 | 0 0 0.5 0 -0 0 190 | 191 | 192 | 0.75 0.15 1.0 193 | 194 | 195 | 196 | 200 | 1 1 1 1 201 | 202 | 203 | 0 204 | 205 | 206 | -3.6653 -1.5071 0 0 -0 3.14159 207 | 208 | 209 | 210 | 211 | 212 | 3.25 0.15 1.0 213 | 214 | 215 | 0 0 0.5 0 -0 0 216 | 217 | 218 | 0 0 0.5 0 -0 0 219 | 220 | 221 | 3.25 0.15 1.0 222 | 223 | 224 | 225 | 229 | 1 1 1 1 230 | 231 | 232 | 0 233 | 234 | 235 | -0.665001 1.54843 0 0 -0 -1.5708 236 | 237 | 238 | 239 | 240 | 241 | 1 0.15 1.0 242 | 243 | 244 | 0 0 0.5 0 -0 0 245 | 246 | 247 | 0 0 0.5 0 -0 0 248 | 249 | 250 | 1 0.15 1.0 251 | 252 | 253 | 254 | 258 | 1 1 1 1 259 | 260 | 261 | 0 262 | 263 | 264 | -1.09 -0.001568 0 0 -0 3.14159 265 | 266 | 267 | 268 | 269 | 270 | 0.75 0.15 1.0 271 | 272 | 273 | 0 0 0.5 0 -0 0 274 | 275 | 276 | 0 0 0.5 0 -0 0 277 | 278 | 279 | 0.75 0.15 1.0 280 | 281 | 282 | 283 | 287 | 1 1 1 1 288 | 289 | 290 | 0 291 | 292 | 293 | -1.515 -0.301568 0 0 -0 -1.5708 294 | 295 | 296 | 297 | 298 | 299 | 1.25 0.15 1.0 300 | 301 | 302 | 0 0 0.5 0 -0 0 303 | 304 | 305 | 0 0 0.5 0 -0 0 306 | 307 | 308 | 1.25 0.15 1.0 309 | 310 | 311 | 312 | 316 | 1 1 1 1 317 | 318 | 319 | 0 320 | 321 | 322 | -0.975982 -2.58088 0 0 -0 0 323 | 324 | 325 | 326 | 327 | 328 | 1 0.15 1.0 329 | 330 | 331 | 0 0 0.5 0 -0 0 332 | 333 | 334 | 0 0 0.5 0 -0 0 335 | 336 | 337 | 1 0.15 1.0 338 | 339 | 340 | 341 | 345 | 1 1 1 1 346 | 347 | 348 | 0 349 | 350 | 351 | -1.09 -0.601568 0 0 -0 0 352 | 353 | 354 | 355 | 356 | 357 | 1 0.15 1.0 358 | 359 | 360 | 0 0 0.5 0 -0 0 361 | 362 | 363 | 0 0 0.5 0 -0 0 364 | 365 | 366 | 1 0.15 1.0 367 | 368 | 369 | 370 | 374 | 1 1 1 1 375 | 376 | 377 | 0 378 | 379 | 380 | -0.665001 -1.02657 0 0 -0 -1.5708 381 | 382 | 383 | 384 | 385 | 386 | 1.25 0.15 1.0 387 | 388 | 389 | 0 0 0.5 0 -0 0 390 | 391 | 392 | 0 0 0.5 0 -0 0 393 | 394 | 395 | 1.25 0.15 1.0 396 | 397 | 398 | 399 | 403 | 1 1 1 1 404 | 405 | 406 | 0 407 | 408 | 409 | -0.115001 -1.45157 0 0 -0 0 410 | 411 | 412 | 413 | 414 | 415 | 0.75 0.15 1.0 416 | 417 | 418 | 0 0 0.5 0 -0 0 419 | 420 | 421 | 0 0 0.5 0 -0 0 422 | 423 | 424 | 0.75 0.15 1.0 425 | 426 | 427 | 428 | 432 | 1 1 1 1 433 | 434 | 435 | 0 436 | 437 | 438 | 0.215018 -0.740877 0 0 -0 3.14159 439 | 440 | 441 | 442 | 443 | 444 | 3.5 0.15 1.0 445 | 446 | 447 | 0 0 0.5 0 -0 0 448 | 449 | 450 | 0 0 0.5 0 -0 0 451 | 452 | 453 | 3.5 0.15 1.0 454 | 455 | 456 | 457 | 461 | 1 1 1 1 462 | 463 | 464 | 0 465 | 466 | 467 | -0.084982 0.934123 0 0 -0 1.5708 468 | 469 | 470 | 471 | 472 | 473 | 1.5 0.15 1.0 474 | 475 | 476 | 0 0 0.5 0 -0 0 477 | 478 | 479 | 0 0 0.5 0 -0 0 480 | 481 | 482 | 1.5 0.15 1.0 483 | 484 | 485 | 486 | 490 | 1 1 1 1 491 | 492 | 493 | 0 494 | 495 | 496 | 0.992315 -0.26358 0 0 -0 0.785398 497 | 498 | 499 | 500 | 501 | 502 | 0.75 0.15 1.0 503 | 504 | 505 | 0 0 0.5 0 -0 0 506 | 507 | 508 | 0 0 0.5 0 -0 0 509 | 510 | 511 | 0.75 0.15 1.0 512 | 513 | 514 | 515 | 519 | 1 1 1 1 520 | 521 | 522 | 0 523 | 524 | 525 | -0.425982 -2.88088 0 0 -0 -1.5708 526 | 527 | 528 | 529 | 530 | 531 | 1.5 0.15 1.0 532 | 533 | 534 | 0 0 0.5 0 -0 0 535 | 536 | 537 | 0 0 0.5 0 -0 0 538 | 539 | 540 | 1.5 0.15 1.0 541 | 542 | 543 | 544 | 548 | 1 1 1 1 549 | 550 | 551 | 0 552 | 553 | 554 | 2.14461 0.213717 0 0 -0 0 555 | 556 | 557 | 558 | 559 | 560 | 0.5 0.15 1.0 561 | 562 | 563 | 0 0 0.5 0 -0 0 564 | 565 | 566 | 0 0 0.5 0 -0 0 567 | 568 | 569 | 0.5 0.15 1.0 570 | 571 | 572 | 573 | 577 | 1 1 1 1 578 | 579 | 580 | 0 581 | 582 | 583 | 2.81961 0.038717 0 0 -0 -1.5708 584 | 585 | 586 | 587 | 588 | 589 | 1.25 0.15 1.0 590 | 591 | 592 | 0 0 0.5 0 -0 0 593 | 594 | 595 | 0 0 0.5 0 -0 0 596 | 597 | 598 | 1.25 0.15 1.0 599 | 600 | 601 | 602 | 606 | 1 1 1 1 607 | 608 | 609 | 0 610 | 611 | 612 | 2.26961 -0.136283 0 0 -0 3.14159 613 | 614 | 615 | 616 | 617 | 618 | 2 0.15 1.0 619 | 620 | 621 | 0 0 0.5 0 -0 0 622 | 623 | 624 | 0 0 0.5 0 -0 0 625 | 626 | 627 | 2 0.15 1.0 628 | 629 | 630 | 631 | 635 | 1 1 1 1 636 | 637 | 638 | 0 639 | 640 | 641 | 1.06554 -0.790357 0 0 -0 -2.35619 642 | 643 | 644 | 645 | 646 | 647 | 0.75 0.15 1.0 648 | 649 | 650 | 0 0 0.5 0 -0 0 651 | 652 | 653 | 0 0 0.5 0 -0 0 654 | 655 | 656 | 0.75 0.15 1.0 657 | 658 | 659 | 660 | 664 | 1 1 1 1 665 | 666 | 667 | 0 668 | 669 | 670 | 0.215018 2.60912 0 0 -0 0 671 | 672 | 673 | 674 | 675 | 676 | 0.75 0.15 1.0 677 | 678 | 679 | 0 0 0.5 0 -0 0 680 | 681 | 682 | 0 0 0.5 0 -0 0 683 | 684 | 685 | 0.75 0.15 1.0 686 | 687 | 688 | 689 | 693 | 1 1 1 1 694 | 695 | 696 | 0 697 | 698 | 699 | 0.515018 2.90912 0 0 -0 1.5708 700 | 701 | 702 | 703 | 704 | 705 | 2.89732 0.15 1.0 706 | 707 | 708 | 0 0 0.5 0 -0 0 709 | 710 | 711 | 0 0 0.5 0 -0 0 712 | 713 | 714 | 2.89732 0.15 1.0 715 | 716 | 717 | 718 | 722 | 1 1 1 1 723 | 724 | 725 | 0 726 | 727 | 728 | 0.947676 -3.18088 0 0 -0 0 729 | 730 | 731 | 732 | 733 | 734 | 2.5 0.15 1.0 735 | 736 | 737 | 0 0 0.5 0 -0 0 738 | 739 | 740 | 0 0 0.5 0 -0 0 741 | 742 | 743 | 2.5 0.15 1.0 744 | 745 | 746 | 747 | 751 | 1 1 1 1 752 | 753 | 754 | 0 755 | 756 | 757 | 3.15218 -2.35003 0 0 -0 0.785398 758 | 759 | 760 | 761 | 762 | 763 | 4 0.15 1.0 764 | 765 | 766 | 0 0 0.5 0 -0 0 767 | 768 | 769 | 0 0 0.5 0 -0 0 770 | 771 | 772 | 4 0.15 1.0 773 | 774 | 775 | 776 | 780 | 1 1 1 1 781 | 782 | 783 | 0 784 | 785 | 786 | 3.99824 0.405824 0 0 -0 1.5708 787 | 788 | 789 | 790 | 791 | 792 | 2.14732 0.15 1.0 793 | 794 | 795 | 0 0 0.5 0 -0 0 796 | 797 | 798 | 0 0 0.5 0 -0 0 799 | 800 | 801 | 2.14732 0.15 1.0 802 | 803 | 804 | 805 | 809 | 1 1 1 1 810 | 811 | 812 | 0 813 | 814 | 815 | 2.98438 2.33082 0 0 -0 3.14159 816 | 817 | 818 | 819 | 820 | 821 | 1 0.15 1.0 822 | 823 | 824 | 0 0 0.5 0 -0 0 825 | 826 | 827 | 0 0 0.5 0 -0 0 828 | 829 | 830 | 1 0.15 1.0 831 | 832 | 833 | 834 | 838 | 1 1 1 1 839 | 840 | 841 | 0 842 | 843 | 844 | 1.98572 2.75582 0 0 -0 1.5708 845 | 846 | 847 | 848 | 849 | 850 | 3.75 0.15 1.0 851 | 852 | 853 | 0 0 0.5 0 -0 0 854 | 855 | 856 | 0 0 0.5 0 -0 0 857 | 858 | 859 | 3.75 0.15 1.0 860 | 861 | 862 | 863 | 867 | 1 1 1 1 868 | 869 | 870 | 0 871 | 872 | 873 | 0.185719 3.18082 0 0 -0 3.14159 874 | 875 | 1 876 | 877 | 878 | -------------------------------------------------------------------------------- /tb_worlds/worlds/sim_house.sdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 12 | 13 | 16 | 17 | 18 | 21 | 22 | 23 | 26 | ogre2 27 | 28 | 31 | 32 | 33 | 34 | 0 35 | 0 0 10 0 -0 0 36 | 0.8 0.8 0.8 1 37 | 0.2 0.2 0.2 1 38 | 39 | 1000 40 | 0.9 41 | 0.01 42 | 0.001 43 | 44 | -0.5 0.1 -0.9 45 | 46 | 0 47 | 0 48 | 0 49 | 50 | 51 | 52 | 1 53 | 54 | 55 | 56 | 57 | 0 0 1 58 | 100 100 59 | 60 | 61 | 62 | 63 | 65535 64 | 65 | 66 | 67 | 68 | 100 69 | 50 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 10 78 | 79 | 80 | 0 81 | 82 | 83 | 0 0 1 84 | 100 100 85 | 86 | 87 | 88 | 92 | 93 | 94 | 0 95 | 0 96 | 0 97 | 98 | 99 | 0 0 -9.8 100 | 6e-06 2.3e-05 -4.2e-05 101 | 102 | 103 | 0.003 104 | 1 105 | 106 | 107 | 0.8 0.8 0.8 1 108 | 0.7 0.7 0.7 1 109 | 0 110 | 111 | 112 | 113 | EARTH_WGS84 114 | 0 115 | 0 116 | 0 117 | 0 118 | 119 | 120 | 1.48679 1.14707 0 0 -0 0 121 | 122 | 123 | 124 | 125 | 2.5 0.15 1 126 | 127 | 128 | 0 0 0.5 0 -0 0 129 | 10 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 0 0 0.5 0 -0 0 145 | 146 | 147 | 2.5 0.15 1 148 | 149 | 150 | 151 | 155 | 1 1 1 1 156 | 157 | 158 | 0 159 | 160 | 161 | -2.70098 -3.18088 0 0 -0 0 162 | 0 163 | 0 164 | 0 165 | 166 | 167 | 168 | 169 | 170 | 0.75 0.15 1 171 | 172 | 173 | 0 0 0.5 0 -0 0 174 | 10 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 0 0 0.5 0 -0 0 190 | 191 | 192 | 0.75 0.15 1 193 | 194 | 195 | 196 | 200 | 1 1 1 1 201 | 202 | 203 | 0 204 | 205 | 206 | -1.52598 -2.88088 0 0 -0 1.5708 207 | 0 208 | 0 209 | 0 210 | 211 | 212 | 213 | 214 | 215 | 3.5 0.15 1 216 | 217 | 218 | 0 0 0.5 0 -0 0 219 | 10 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 0 0 0.5 0 -0 0 235 | 236 | 237 | 3.5 0.15 1 238 | 239 | 240 | 241 | 245 | 1 1 1 1 246 | 247 | 248 | 0 249 | 250 | 251 | -2.79863 1.99647 0 0 -0 -2.35619 252 | 0 253 | 0 254 | 0 255 | 256 | 257 | 258 | 259 | 260 | 4.153 0.15 1 261 | 262 | 263 | 0 0 0.5 0 -0 0 264 | 10 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 0 0 0.5 0 -0 0 280 | 281 | 282 | 4.153 0.15 1 283 | 284 | 285 | 286 | 290 | 1 1 1 1 291 | 292 | 293 | 0 294 | 295 | 296 | -3.98298 -1.18938 0 0 -0 -1.5708 297 | 0 298 | 0 299 | 0 300 | 301 | 302 | 303 | 304 | 305 | 0.75 0.15 1 306 | 307 | 308 | 0 0 0.5 0 -0 0 309 | 10 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 0 0 0.5 0 -0 0 325 | 326 | 327 | 0.75 0.15 1 328 | 329 | 330 | 331 | 335 | 1 1 1 1 336 | 337 | 338 | 0 339 | 340 | 341 | -3.6653 0.087099 0 0 -0 0 342 | 0 343 | 0 344 | 0 345 | 346 | 347 | 348 | 349 | 350 | 1.76432 0.15 1 351 | 352 | 353 | 0 0 0.5 0 -0 0 354 | 10 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 0 0 0.5 0 -0 0 370 | 371 | 372 | 1.76432 0.15 1 373 | 374 | 375 | 376 | 380 | 1 1 1 1 381 | 382 | 383 | 0 384 | 385 | 386 | -3.3653 -0.720058 0 0 -0 -1.5708 387 | 0 388 | 0 389 | 0 390 | 391 | 392 | 393 | 394 | 395 | 0.75 0.15 1 396 | 397 | 398 | 0 0 0.5 0 -0 0 399 | 10 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 0 0 0.5 0 -0 0 415 | 416 | 417 | 0.75 0.15 1 418 | 419 | 420 | 421 | 425 | 1 1 1 1 426 | 427 | 428 | 0 429 | 430 | 431 | -3.6653 -1.5071 0 0 -0 3.14159 432 | 0 433 | 0 434 | 0 435 | 436 | 437 | 438 | 439 | 440 | 3.25 0.15 1 441 | 442 | 443 | 0 0 0.5 0 -0 0 444 | 10 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 0 0 0.5 0 -0 0 460 | 461 | 462 | 3.25 0.15 1 463 | 464 | 465 | 466 | 470 | 1 1 1 1 471 | 472 | 473 | 0 474 | 475 | 476 | -0.665001 1.54843 0 0 -0 -1.5708 477 | 0 478 | 0 479 | 0 480 | 481 | 482 | 483 | 484 | 485 | 1 0.15 1 486 | 487 | 488 | 0 0 0.5 0 -0 0 489 | 10 490 | 491 | 492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 0 0 0.5 0 -0 0 505 | 506 | 507 | 1 0.15 1 508 | 509 | 510 | 511 | 515 | 1 1 1 1 516 | 517 | 518 | 0 519 | 520 | 521 | -1.09 -0.001568 0 0 -0 3.14159 522 | 0 523 | 0 524 | 0 525 | 526 | 527 | 528 | 529 | 530 | 0.75 0.15 1 531 | 532 | 533 | 0 0 0.5 0 -0 0 534 | 10 535 | 536 | 537 | 538 | 539 | 540 | 541 | 542 | 543 | 544 | 545 | 546 | 547 | 548 | 549 | 0 0 0.5 0 -0 0 550 | 551 | 552 | 0.75 0.15 1 553 | 554 | 555 | 556 | 560 | 1 1 1 1 561 | 562 | 563 | 0 564 | 565 | 566 | -1.515 -0.301568 0 0 -0 -1.5708 567 | 0 568 | 0 569 | 0 570 | 571 | 572 | 573 | 574 | 575 | 1.25 0.15 1 576 | 577 | 578 | 0 0 0.5 0 -0 0 579 | 10 580 | 581 | 582 | 583 | 584 | 585 | 586 | 587 | 588 | 589 | 590 | 591 | 592 | 593 | 594 | 0 0 0.5 0 -0 0 595 | 596 | 597 | 1.25 0.15 1 598 | 599 | 600 | 601 | 605 | 1 1 1 1 606 | 607 | 608 | 0 609 | 610 | 611 | -0.975982 -2.58088 0 0 -0 0 612 | 0 613 | 0 614 | 0 615 | 616 | 617 | 618 | 619 | 620 | 1 0.15 1 621 | 622 | 623 | 0 0 0.5 0 -0 0 624 | 10 625 | 626 | 627 | 628 | 629 | 630 | 631 | 632 | 633 | 634 | 635 | 636 | 637 | 638 | 639 | 0 0 0.5 0 -0 0 640 | 641 | 642 | 1 0.15 1 643 | 644 | 645 | 646 | 650 | 1 1 1 1 651 | 652 | 653 | 0 654 | 655 | 656 | -1.09 -0.601568 0 0 -0 0 657 | 0 658 | 0 659 | 0 660 | 661 | 662 | 663 | 664 | 665 | 1 0.15 1 666 | 667 | 668 | 0 0 0.5 0 -0 0 669 | 10 670 | 671 | 672 | 673 | 674 | 675 | 676 | 677 | 678 | 679 | 680 | 681 | 682 | 683 | 684 | 0 0 0.5 0 -0 0 685 | 686 | 687 | 1 0.15 1 688 | 689 | 690 | 691 | 695 | 1 1 1 1 696 | 697 | 698 | 0 699 | 700 | 701 | -0.665001 -1.02657 0 0 -0 -1.5708 702 | 0 703 | 0 704 | 0 705 | 706 | 707 | 708 | 709 | 710 | 1.25 0.15 1 711 | 712 | 713 | 0 0 0.5 0 -0 0 714 | 10 715 | 716 | 717 | 718 | 719 | 720 | 721 | 722 | 723 | 724 | 725 | 726 | 727 | 728 | 729 | 0 0 0.5 0 -0 0 730 | 731 | 732 | 1.25 0.15 1 733 | 734 | 735 | 736 | 740 | 1 1 1 1 741 | 742 | 743 | 0 744 | 745 | 746 | -0.115001 -1.45157 0 0 -0 0 747 | 0 748 | 0 749 | 0 750 | 751 | 752 | 753 | 754 | 755 | 0.75 0.15 1 756 | 757 | 758 | 0 0 0.5 0 -0 0 759 | 10 760 | 761 | 762 | 763 | 764 | 765 | 766 | 767 | 768 | 769 | 770 | 771 | 772 | 773 | 774 | 0 0 0.5 0 -0 0 775 | 776 | 777 | 0.75 0.15 1 778 | 779 | 780 | 781 | 785 | 1 1 1 1 786 | 787 | 788 | 0 789 | 790 | 791 | 0.215018 -0.740877 0 0 -0 3.14159 792 | 0 793 | 0 794 | 0 795 | 796 | 797 | 798 | 799 | 800 | 3.5 0.15 1 801 | 802 | 803 | 0 0 0.5 0 -0 0 804 | 10 805 | 806 | 807 | 808 | 809 | 810 | 811 | 812 | 813 | 814 | 815 | 816 | 817 | 818 | 819 | 0 0 0.5 0 -0 0 820 | 821 | 822 | 3.5 0.15 1 823 | 824 | 825 | 826 | 830 | 1 1 1 1 831 | 832 | 833 | 0 834 | 835 | 836 | -0.084982 0.934123 0 0 -0 1.5708 837 | 0 838 | 0 839 | 0 840 | 841 | 842 | 843 | 844 | 845 | 1.5 0.15 1 846 | 847 | 848 | 0 0 0.5 0 -0 0 849 | 10 850 | 851 | 852 | 853 | 854 | 855 | 856 | 857 | 858 | 859 | 860 | 861 | 862 | 863 | 864 | 0 0 0.5 0 -0 0 865 | 866 | 867 | 1.5 0.15 1 868 | 869 | 870 | 871 | 875 | 1 1 1 1 876 | 877 | 878 | 0 879 | 880 | 881 | 0.992315 -0.26358 0 0 -0 0.785398 882 | 0 883 | 0 884 | 0 885 | 886 | 887 | 888 | 889 | 890 | 0.75 0.15 1 891 | 892 | 893 | 0 0 0.5 0 -0 0 894 | 10 895 | 896 | 897 | 898 | 899 | 900 | 901 | 902 | 903 | 904 | 905 | 906 | 907 | 908 | 909 | 0 0 0.5 0 -0 0 910 | 911 | 912 | 0.75 0.15 1 913 | 914 | 915 | 916 | 920 | 1 1 1 1 921 | 922 | 923 | 0 924 | 925 | 926 | -0.425982 -2.88088 0 0 -0 -1.5708 927 | 0 928 | 0 929 | 0 930 | 931 | 932 | 933 | 934 | 935 | 1.5 0.15 1 936 | 937 | 938 | 0 0 0.5 0 -0 0 939 | 10 940 | 941 | 942 | 943 | 944 | 945 | 946 | 947 | 948 | 949 | 950 | 951 | 952 | 953 | 954 | 0 0 0.5 0 -0 0 955 | 956 | 957 | 1.5 0.15 1 958 | 959 | 960 | 961 | 965 | 1 1 1 1 966 | 967 | 968 | 0 969 | 970 | 971 | 2.14461 0.213717 0 0 -0 0 972 | 0 973 | 0 974 | 0 975 | 976 | 977 | 978 | 979 | 980 | 0.5 0.15 1 981 | 982 | 983 | 0 0 0.5 0 -0 0 984 | 10 985 | 986 | 987 | 988 | 989 | 990 | 991 | 992 | 993 | 994 | 995 | 996 | 997 | 998 | 999 | 0 0 0.5 0 -0 0 1000 | 1001 | 1002 | 0.5 0.15 1 1003 | 1004 | 1005 | 1006 | 1010 | 1 1 1 1 1011 | 1012 | 1013 | 0 1014 | 1015 | 1016 | 2.81961 0.038717 0 0 -0 -1.5708 1017 | 0 1018 | 0 1019 | 0 1020 | 1021 | 1022 | 1023 | 1024 | 1025 | 1.25 0.15 1 1026 | 1027 | 1028 | 0 0 0.5 0 -0 0 1029 | 10 1030 | 1031 | 1032 | 1033 | 1034 | 1035 | 1036 | 1037 | 1038 | 1039 | 1040 | 1041 | 1042 | 1043 | 1044 | 0 0 0.5 0 -0 0 1045 | 1046 | 1047 | 1.25 0.15 1 1048 | 1049 | 1050 | 1051 | 1055 | 1 1 1 1 1056 | 1057 | 1058 | 0 1059 | 1060 | 1061 | 2.26961 -0.136283 0 0 -0 3.14159 1062 | 0 1063 | 0 1064 | 0 1065 | 1066 | 1067 | 1068 | 1069 | 1070 | 2 0.15 1 1071 | 1072 | 1073 | 0 0 0.5 0 -0 0 1074 | 10 1075 | 1076 | 1077 | 1078 | 1079 | 1080 | 1081 | 1082 | 1083 | 1084 | 1085 | 1086 | 1087 | 1088 | 1089 | 0 0 0.5 0 -0 0 1090 | 1091 | 1092 | 2 0.15 1 1093 | 1094 | 1095 | 1096 | 1100 | 1 1 1 1 1101 | 1102 | 1103 | 0 1104 | 1105 | 1106 | 1.06554 -0.790357 0 0 -0 -2.35619 1107 | 0 1108 | 0 1109 | 0 1110 | 1111 | 1112 | 1113 | 1114 | 1115 | 0.75 0.15 1 1116 | 1117 | 1118 | 0 0 0.5 0 -0 0 1119 | 10 1120 | 1121 | 1122 | 1123 | 1124 | 1125 | 1126 | 1127 | 1128 | 1129 | 1130 | 1131 | 1132 | 1133 | 1134 | 0 0 0.5 0 -0 0 1135 | 1136 | 1137 | 0.75 0.15 1 1138 | 1139 | 1140 | 1141 | 1145 | 1 1 1 1 1146 | 1147 | 1148 | 0 1149 | 1150 | 1151 | 0.215018 2.60912 0 0 -0 0 1152 | 0 1153 | 0 1154 | 0 1155 | 1156 | 1157 | 1158 | 1159 | 1160 | 0.75 0.15 1 1161 | 1162 | 1163 | 0 0 0.5 0 -0 0 1164 | 10 1165 | 1166 | 1167 | 1168 | 1169 | 1170 | 1171 | 1172 | 1173 | 1174 | 1175 | 1176 | 1177 | 1178 | 1179 | 0 0 0.5 0 -0 0 1180 | 1181 | 1182 | 0.75 0.15 1 1183 | 1184 | 1185 | 1186 | 1190 | 1 1 1 1 1191 | 1192 | 1193 | 0 1194 | 1195 | 1196 | 0.515018 2.90912 0 0 -0 1.5708 1197 | 0 1198 | 0 1199 | 0 1200 | 1201 | 1202 | 1203 | 1204 | 1205 | 2.89732 0.15 1 1206 | 1207 | 1208 | 0 0 0.5 0 -0 0 1209 | 10 1210 | 1211 | 1212 | 1213 | 1214 | 1215 | 1216 | 1217 | 1218 | 1219 | 1220 | 1221 | 1222 | 1223 | 1224 | 0 0 0.5 0 -0 0 1225 | 1226 | 1227 | 2.89732 0.15 1 1228 | 1229 | 1230 | 1231 | 1235 | 1 1 1 1 1236 | 1237 | 1238 | 0 1239 | 1240 | 1241 | 0.947676 -3.18088 0 0 -0 0 1242 | 0 1243 | 0 1244 | 0 1245 | 1246 | 1247 | 1248 | 1249 | 1250 | 2.5 0.15 1 1251 | 1252 | 1253 | 0 0 0.5 0 -0 0 1254 | 10 1255 | 1256 | 1257 | 1258 | 1259 | 1260 | 1261 | 1262 | 1263 | 1264 | 1265 | 1266 | 1267 | 1268 | 1269 | 0 0 0.5 0 -0 0 1270 | 1271 | 1272 | 2.5 0.15 1 1273 | 1274 | 1275 | 1276 | 1280 | 1 1 1 1 1281 | 1282 | 1283 | 0 1284 | 1285 | 1286 | 3.15218 -2.35003 0 0 -0 0.785398 1287 | 0 1288 | 0 1289 | 0 1290 | 1291 | 1292 | 1293 | 1294 | 1295 | 4 0.15 1 1296 | 1297 | 1298 | 0 0 0.5 0 -0 0 1299 | 10 1300 | 1301 | 1302 | 1303 | 1304 | 1305 | 1306 | 1307 | 1308 | 1309 | 1310 | 1311 | 1312 | 1313 | 1314 | 0 0 0.5 0 -0 0 1315 | 1316 | 1317 | 4 0.15 1 1318 | 1319 | 1320 | 1321 | 1325 | 1 1 1 1 1326 | 1327 | 1328 | 0 1329 | 1330 | 1331 | 3.99824 0.405824 0 0 -0 1.5708 1332 | 0 1333 | 0 1334 | 0 1335 | 1336 | 1337 | 1338 | 1339 | 1340 | 2.14732 0.15 1 1341 | 1342 | 1343 | 0 0 0.5 0 -0 0 1344 | 10 1345 | 1346 | 1347 | 1348 | 1349 | 1350 | 1351 | 1352 | 1353 | 1354 | 1355 | 1356 | 1357 | 1358 | 1359 | 0 0 0.5 0 -0 0 1360 | 1361 | 1362 | 2.14732 0.15 1 1363 | 1364 | 1365 | 1366 | 1370 | 1 1 1 1 1371 | 1372 | 1373 | 0 1374 | 1375 | 1376 | 2.98438 2.33082 0 0 -0 3.14159 1377 | 0 1378 | 0 1379 | 0 1380 | 1381 | 1382 | 1383 | 1384 | 1385 | 1 0.15 1 1386 | 1387 | 1388 | 0 0 0.5 0 -0 0 1389 | 10 1390 | 1391 | 1392 | 1393 | 1394 | 1395 | 1396 | 1397 | 1398 | 1399 | 1400 | 1401 | 1402 | 1403 | 1404 | 0 0 0.5 0 -0 0 1405 | 1406 | 1407 | 1 0.15 1 1408 | 1409 | 1410 | 1411 | 1415 | 1 1 1 1 1416 | 1417 | 1418 | 0 1419 | 1420 | 1421 | 1.98572 2.75582 0 0 -0 1.5708 1422 | 0 1423 | 0 1424 | 0 1425 | 1426 | 1427 | 1428 | 1429 | 1430 | 3.75 0.15 1 1431 | 1432 | 1433 | 0 0 0.5 0 -0 0 1434 | 10 1435 | 1436 | 1437 | 1438 | 1439 | 1440 | 1441 | 1442 | 1443 | 1444 | 1445 | 1446 | 1447 | 1448 | 1449 | 0 0 0.5 0 -0 0 1450 | 1451 | 1452 | 3.75 0.15 1 1453 | 1454 | 1455 | 1456 | 1460 | 1 1 1 1 1461 | 1462 | 1463 | 0 1464 | 1465 | 1466 | 0.185719 3.18082 0 0 -0 3.14159 1467 | 0 1468 | 0 1469 | 0 1470 | 1471 | 1 1472 | 1473 | 1474 | 206 459000000 1475 | 79 688300467 1476 | 1618613802 581777859 1477 | 78879 1478 | 1479 | 0 0 0 0 -0 0 1480 | 1 1 1 1481 | 1482 | 0 0 0 0 -0 0 1483 | 0 0 0 0 -0 0 1484 | 0 0 0 0 -0 0 1485 | 0 0 0 0 -0 0 1486 | 1487 | 1488 | 1489 | 2.16464 1.82515 0 0 -0 0 1490 | 1 1 1 1491 | 1492 | -0.536336 -1.35573 0 0 -0 0 1493 | 0 0 0 0 -0 0 1494 | 0 0 0 0 -0 0 1495 | 0 0 0 0 -0 0 1496 | 1497 | 1498 | 0.638668 -1.05573 0 0 -0 1.5708 1499 | 0 0 0 0 -0 0 1500 | 0 0 0 0 -0 0 1501 | 0 0 0 0 -0 0 1502 | 1503 | 1504 | -0.633986 3.82162 0 0 0 -2.35619 1505 | 0 0 0 0 -0 0 1506 | 0 0 0 0 -0 0 1507 | 0 0 0 0 -0 0 1508 | 1509 | 1510 | -1.81834 0.635766 0 0 0 -1.5708 1511 | 0 0 0 0 -0 0 1512 | 0 0 0 0 -0 0 1513 | 0 0 0 0 -0 0 1514 | 1515 | 1516 | -1.50066 1.91224 0 0 -0 0 1517 | 0 0 0 0 -0 0 1518 | 0 0 0 0 -0 0 1519 | 0 0 0 0 -0 0 1520 | 1521 | 1522 | -1.20066 1.10509 0 0 0 -1.5708 1523 | 0 0 0 0 -0 0 1524 | 0 0 0 0 -0 0 1525 | 0 0 0 0 -0 0 1526 | 1527 | 1528 | -1.50066 0.318046 0 0 -0 3.14159 1529 | 0 0 0 0 -0 0 1530 | 0 0 0 0 -0 0 1531 | 0 0 0 0 -0 0 1532 | 1533 | 1534 | 1.49965 3.37358 0 0 0 -1.5708 1535 | 0 0 0 0 -0 0 1536 | 0 0 0 0 -0 0 1537 | 0 0 0 0 -0 0 1538 | 1539 | 1540 | 1.07465 1.82358 0 0 -0 3.14159 1541 | 0 0 0 0 -0 0 1542 | 0 0 0 0 -0 0 1543 | 0 0 0 0 -0 0 1544 | 1545 | 1546 | 0.649648 1.52358 0 0 0 -1.5708 1547 | 0 0 0 0 -0 0 1548 | 0 0 0 0 -0 0 1549 | 0 0 0 0 -0 0 1550 | 1551 | 1552 | 1.18867 -0.755734 0 0 -0 0 1553 | 0 0 0 0 -0 0 1554 | 0 0 0 0 -0 0 1555 | 0 0 0 0 -0 0 1556 | 1557 | 1558 | 1.07465 1.22358 0 0 -0 0 1559 | 0 0 0 0 -0 0 1560 | 0 0 0 0 -0 0 1561 | 0 0 0 0 -0 0 1562 | 1563 | 1564 | 1.49965 0.798576 0 0 0 -1.5708 1565 | 0 0 0 0 -0 0 1566 | 0 0 0 0 -0 0 1567 | 0 0 0 0 -0 0 1568 | 1569 | 1570 | 2.04964 0.373576 0 0 -0 0 1571 | 0 0 0 0 -0 0 1572 | 0 0 0 0 -0 0 1573 | 0 0 0 0 -0 0 1574 | 1575 | 1576 | 2.37966 1.08427 0 0 -0 3.14159 1577 | 0 0 0 0 -0 0 1578 | 0 0 0 0 -0 0 1579 | 0 0 0 0 -0 0 1580 | 1581 | 1582 | 2.07966 2.75927 0 0 -0 1.5708 1583 | 0 0 0 0 -0 0 1584 | 0 0 0 0 -0 0 1585 | 0 0 0 0 -0 0 1586 | 1587 | 1588 | 3.15696 1.56157 0 0 -0 0.785398 1589 | 0 0 0 0 -0 0 1590 | 0 0 0 0 -0 0 1591 | 0 0 0 0 -0 0 1592 | 1593 | 1594 | 1.73867 -1.05573 0 0 0 -1.5708 1595 | 0 0 0 0 -0 0 1596 | 0 0 0 0 -0 0 1597 | 0 0 0 0 -0 0 1598 | 1599 | 1600 | 4.30925 2.03886 0 0 -0 0 1601 | 0 0 0 0 -0 0 1602 | 0 0 0 0 -0 0 1603 | 0 0 0 0 -0 0 1604 | 1605 | 1606 | 4.98425 1.86386 0 0 0 -1.5708 1607 | 0 0 0 0 -0 0 1608 | 0 0 0 0 -0 0 1609 | 0 0 0 0 -0 0 1610 | 1611 | 1612 | 4.43425 1.68886 0 0 -0 3.14159 1613 | 0 0 0 0 -0 0 1614 | 0 0 0 0 -0 0 1615 | 0 0 0 0 -0 0 1616 | 1617 | 1618 | 3.23018 1.03479 0 0 0 -2.35619 1619 | 0 0 0 0 -0 0 1620 | 0 0 0 0 -0 0 1621 | 0 0 0 0 -0 0 1622 | 1623 | 1624 | 2.37966 4.43427 0 0 -0 0 1625 | 0 0 0 0 -0 0 1626 | 0 0 0 0 -0 0 1627 | 0 0 0 0 -0 0 1628 | 1629 | 1630 | 2.67966 4.73427 0 0 -0 1.5708 1631 | 0 0 0 0 -0 0 1632 | 0 0 0 0 -0 0 1633 | 0 0 0 0 -0 0 1634 | 1635 | 1636 | 3.11232 -1.35573 0 0 -0 0 1637 | 0 0 0 0 -0 0 1638 | 0 0 0 0 -0 0 1639 | 0 0 0 0 -0 0 1640 | 1641 | 1642 | 5.31682 -0.524884 0 0 -0 0.785398 1643 | 0 0 0 0 -0 0 1644 | 0 0 0 0 -0 0 1645 | 0 0 0 0 -0 0 1646 | 1647 | 1648 | 6.16288 2.23097 0 0 -0 1.5708 1649 | 0 0 0 0 -0 0 1650 | 0 0 0 0 -0 0 1651 | 0 0 0 0 -0 0 1652 | 1653 | 1654 | 5.14902 4.15597 0 0 -0 3.14159 1655 | 0 0 0 0 -0 0 1656 | 0 0 0 0 -0 0 1657 | 0 0 0 0 -0 0 1658 | 1659 | 1660 | 4.15036 4.58097 0 0 -0 1.5708 1661 | 0 0 0 0 -0 0 1662 | 0 0 0 0 -0 0 1663 | 0 0 0 0 -0 0 1664 | 1665 | 1666 | 2.35036 5.00597 0 0 -0 3.14159 1667 | 0 0 0 0 -0 0 1668 | 0 0 0 0 -0 0 1669 | 0 0 0 0 -0 0 1670 | 1671 | 1672 | 1673 | 0 0 10 0 -0 0 1674 | 1675 | 1676 | 1677 | 1678 | 0.992328 -4.52052 10.8021 0 1.06764 1.5682 1679 | orbit 1680 | perspective 1681 | 1682 | 1683 | 1684 | 1685 | --------------------------------------------------------------------------------