From d96946fc40ddfd29ce3beefc50dfd430527d261a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Wed, 25 Sep 2024 15:16:20 +0200 Subject: [PATCH 01/58] docs: add turtlebot4 tutorial --- examples/turtlebot_demo.py | 91 +++++++++++++++ src/examples/turtlebot4/README.md | 112 +++++++++++++++++++ src/examples/turtlebot4/turtlebot.launch.xml | 36 ++++++ 3 files changed, 239 insertions(+) create mode 100644 examples/turtlebot_demo.py create mode 100644 src/examples/turtlebot4/README.md create mode 100644 src/examples/turtlebot4/turtlebot.launch.xml diff --git a/examples/turtlebot_demo.py b/examples/turtlebot_demo.py new file mode 100644 index 00000000..fd4c38fe --- /dev/null +++ b/examples/turtlebot_demo.py @@ -0,0 +1,91 @@ +# Copyright (C) 2024 Robotec.AI +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + + +import rclpy +import rclpy.callback_groups +import rclpy.executors +import rclpy.qos +import rclpy.subscription +import rclpy.task +from langchain.tools.render import render_text_description_and_args +from langchain_openai import ChatOpenAI + +from rai.agents.state_based import create_state_based_agent +from rai.node import RaiNode +from rai.tools.ros.native import ( + GetCameraImage, + GetMsgFromTopic, + Ros2PubMessageTool, + Ros2ShowMsgInterfaceTool, +) +from rai.tools.ros.native_actions import Ros2RunActionSync +from rai.tools.time import WaitForSecondsTool + + +def main(): + rclpy.init() + llm = ChatOpenAI(model="gpt-4o") + + # TODO(boczekbartek): refactor system prompt + + SYSTEM_PROMPT = "" + + node = RaiNode( + llm=ChatOpenAI( + model="gpt-4o-mini" + ), # smaller model used to describe the environment + system_prompt=SYSTEM_PROMPT, + ) + + tools = [ + WaitForSecondsTool(), + GetMsgFromTopic(node=node), + GetCameraImage(node=node), + Ros2ShowMsgInterfaceTool(), + Ros2PubMessageTool(node=node), + Ros2RunActionSync(node=node), + ] + + state_retriever = node.get_robot_state + + SYSTEM_PROMPT = f"""You are an autonomous robot connected to ros2 environment. Your main goal is to fulfill the user's requests. + Do not make assumptions about the environment you are currently in. + + Here are your tools: + {render_text_description_and_args(tools)} + + You can use ros2 topics, services and actions to operate. """ + + node.get_logger().info(f"{SYSTEM_PROMPT=}") + + node.system_prompt = node.initialize_system_prompt(SYSTEM_PROMPT) + + app = create_state_based_agent( + llm=llm, + tools=tools, + state_retriever=state_retriever, + logger=node.get_logger(), + ) + + node.set_app(app) + + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(node) + executor.spin() + rclpy.shutdown() + + +main() diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md new file mode 100644 index 00000000..3b1c2c33 --- /dev/null +++ b/src/examples/turtlebot4/README.md @@ -0,0 +1,112 @@ +# Turtlebot4 tutorial + +Following tutorial will be presented during [ROSCon 2024](https://roscon.ros.org/2024/) talk about RAI. + +## Recording + +TBD + +## Step by step instructions + +1. O3DE simulation setup + + TBD + +2. Clone and install [rai](https://github.com/RobotecAI/rai) + + 1. Follow setps from [1. Setting up the workspace](https://github.com/RobotecAI/rai?tab=readme-ov-file#1-setting-up-the-workspace) + 2. Then follow steps from [2. Build the project](https://github.com/RobotecAI/rai?tab=readme-ov-file#2-build-the-project) + +3. Setup your [OpenAI API](https://openai.com/index/openai-api/). Make sure you have set `OPENAI_API_KEY` variable. + +4. Configure `rai_whoami_node` (based on ["Your robot identity in RAI"](https://github.com/RobotecAI/rai/blob/development/docs/create_robots_whoami.md) tutorial): + + 1. Create `whoami` package for turtlebot4 in `src/examples/turtlebot4` + + ```bash + poetry run create_rai_ws --name turtlebot4 --destination-directory src/examples + ``` + + 2. Download official turtlebot4 [data sheet](https://bit.ly/3KCp3Du) into + `src/examples/turtlebot4_whoami/description/documentation` + 3. Download [image](https://s3.amazonaws.com/assets.clearpathrobotics.com/wp-content/uploads/2022/03/16113604/Turtlebot-4-20220207.44.png) of turtlebot4 into `src/examples/turtlebot4_whoami/description/images` + 4. Run theย `parse_whoami_package`. This will process the documentation, building + it into a vector database, which is used by RAI agent to reason about its identity. + + > **NOTE**: Vector database is created using the OpenAI API. Parsing bigger documents + > might lead to costs. Embedding model can be configured in + > [config.toml](https://github.com/RobotecAI/rai/blob/development/config.toml#L13) + + ```bash + poetry run parse_whoami_package src/examples/turtlebot4_whoami/description + ``` + + 5. Rebuild the workspace + ```bash + colcon build --symlink-install + ``` + + Congratulations! Your `rai_whoami_node` is configured. In the follwing steps + your RAI agent will have turtlebot4 personality. + +5. Run rai agent: + + ```bash + ros launch ./src/examples/turtlebot.launch.xml \ + game_launcher:=/path/to/simulation_binary/Turtlebot4.GameLauncher \ + robot_description_package:=turtlebot4_whoami + ``` + +6. Open you internet browser and go to `localhost:8501` +7. You can interact with your RAI agent through the chat. On the left you can interact + with the RAI HMI Agent. On the right you will see status of missions that were send + to the RAI Node and are executed on your robot. + + For example you can try: + + - Is it able to bring you something from the kitchen? (testing robot's identity) + - What are its ros2 interfaces (discovery of ros2 interfaces) + - tell me what you see (interpretation of camera image) + - Drive forward (interaction with the robot using discovered ros2 interfaces) + +### Troubleshooting + +_My robot doesn't have the identity._ + +1. Query `rai_whoami_node` for the identity manually + + ```bash + ros2 service call /rai_whoami_identity_service std_srvs/srv/Trigger + ``` + +2. Make sure you have all required files were generated correctly in the `turtlebot4_whoami` package and have simillar sizes. + + ```bash + ls -sh src/examples/turtlebot4_whoami/description/ + total 52K + 4.0K documentation 4.0K images 28K index.faiss 8.0K index.pkl 4.0K robot_constitution.txt 4.0K robot_identity.txt + ``` + + You can also check the contents of `robot_indentify.txt` file (it is generated by LLM, but should be simillar to the one below). + + ```bash + cat src/examples/turtlebot4_whoami/description/robot_identity.txt + I am a TurtleBotยฎ 4, a robotics learning platform designed for education and + research in robotics. This next-generation mobile robot is built on the iRobotยฎ + Createยฎ 3 mobile base and comes fully assembled with ROS 2 installed and configured, + making it accessible for beginners and experienced developers alike. + ... + ``` + + If files above are incorrect please check your `OPENAI_API_KEY` and rerun point 4 + of this tutorial. + +--- + +_Robot doesn't move in the simulation._ + +1. Make sure you can see ros2 topic when simulation binary is running + + ```bash + ros2 topic list + ``` diff --git a/src/examples/turtlebot4/turtlebot.launch.xml b/src/examples/turtlebot4/turtlebot.launch.xml new file mode 100644 index 00000000..6fef8408 --- /dev/null +++ b/src/examples/turtlebot4/turtlebot.launch.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + From 5cd4ade5e1b42ea37181f5afc0194c74e41fc045 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Wed, 25 Sep 2024 16:19:01 +0200 Subject: [PATCH 02/58] simplify launchfile --- src/examples/turtlebot4/README.md | 5 ++--- src/examples/turtlebot4/turtlebot.launch.xml | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index 3b1c2c33..72768fe6 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -52,9 +52,8 @@ TBD 5. Run rai agent: ```bash - ros launch ./src/examples/turtlebot.launch.xml \ - game_launcher:=/path/to/simulation_binary/Turtlebot4.GameLauncher \ - robot_description_package:=turtlebot4_whoami + ros launch ./src/examples/turtlebot4/turtlebot.launch.xml \ + game_launcher:=/path/to/simulation_binary/Turtlebot4.GameLauncher ``` 6. Open you internet browser and go to `localhost:8501` diff --git a/src/examples/turtlebot4/turtlebot.launch.xml b/src/examples/turtlebot4/turtlebot.launch.xml index 6fef8408..141c56a1 100644 --- a/src/examples/turtlebot4/turtlebot.launch.xml +++ b/src/examples/turtlebot4/turtlebot.launch.xml @@ -15,7 +15,7 @@ --> - + From 2c50f58cc318fb1062ffe8271ebb0605fc466ace Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 8 Oct 2024 10:00:21 +0200 Subject: [PATCH 03/58] chore(`turtlebot`): impove launchfile, add rviz and nav2 configs --- src/examples/turtlebot4/config.rviz | 709 ++++++++++++++++++ .../turtlebot4/navigation_params_humble.yaml | 598 +++++++++++++++ .../turtlebot4/navigation_params_jazzy.yaml | 633 ++++++++++++++++ src/examples/turtlebot4/run-nav.bash | 6 + src/examples/turtlebot4/turtlebot.launch.xml | 8 + 5 files changed, 1954 insertions(+) create mode 100644 src/examples/turtlebot4/config.rviz create mode 100644 src/examples/turtlebot4/navigation_params_humble.yaml create mode 100644 src/examples/turtlebot4/navigation_params_jazzy.yaml create mode 100755 src/examples/turtlebot4/run-nav.bash diff --git a/src/examples/turtlebot4/config.rviz b/src/examples/turtlebot4/config.rviz new file mode 100644 index 00000000..75845df3 --- /dev/null +++ b/src/examples/turtlebot4/config.rviz @@ -0,0 +1,709 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Image1 + - /Image1/Topic1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 272 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.3333333432674408 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 + - Class: nav2_rviz_plugins/Selector + Name: Selector +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: "" + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + block: + Value: true + bumper: + Value: true + front_caster_link: + Value: true + front_left_bottom_weight_block: + Value: true + front_left_top_weight_block: + Value: true + front_left_tower_standoff: + Value: true + front_right_bottom_weight_block: + Value: true + front_right_top_weight_block: + Value: true + front_right_tower_standoff: + Value: true + imu_link: + Value: true + ir_intensity_front_center_left: + Value: true + ir_intensity_front_center_right: + Value: true + ir_intensity_front_left: + Value: true + ir_intensity_front_right: + Value: true + ir_intensity_left: + Value: true + ir_intensity_right: + Value: true + ir_intensity_side_left: + Value: true + ir_omni: + Value: true + left_wheel: + Value: true + lidar_sensor_frame: + Value: true + map: + Value: true + mouse: + Value: true + oakd_camera_bracket: + Value: true + oakd_imu_frame: + Value: true + oakd_left_camera_frame: + Value: true + oakd_left_camera_optical_frame: + Value: true + oakd_link: + Value: true + oakd_rgb_camera_frame: + Value: true + oakd_rgb_camera_optical_frame: + Value: true + oakd_right_camera_frame: + Value: true + oakd_right_camera_optical_frame: + Value: true + odom: + Value: true + rear_left_tower_standoff: + Value: true + rear_right_tower_standoff: + Value: true + right_wheel: + Value: true + rplidar_link: + Value: true + sensor_frame: + Value: true + tower_sensor_plate: + Value: true + wheel_drop_left: + Value: true + wheel_drop_right: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_link: + bumper: {} + front_caster_link: {} + front_left_bottom_weight_block: {} + front_left_top_weight_block: {} + front_left_tower_standoff: {} + front_right_bottom_weight_block: {} + front_right_top_weight_block: {} + front_right_tower_standoff: {} + imu_link: + sensor_frame: {} + ir_intensity_front_center_left: {} + ir_intensity_front_center_right: {} + ir_intensity_front_left: {} + ir_intensity_front_right: {} + ir_intensity_left: {} + ir_intensity_right: {} + ir_intensity_side_left: {} + ir_omni: {} + mouse: {} + oakd_camera_bracket: + oakd_link: + oakd_imu_frame: {} + oakd_left_camera_frame: + oakd_left_camera_optical_frame: {} + oakd_rgb_camera_frame: + oakd_rgb_camera_optical_frame: {} + oakd_right_camera_frame: + oakd_right_camera_optical_frame: {} + rear_left_tower_standoff: {} + rear_right_tower_standoff: {} + rplidar_link: + lidar_sensor_frame: {} + tower_sensor_plate: {} + wheel_drop_left: + left_wheel: {} + wheel_drop_right: + right_wheel: {} + block: {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: true + Max Arrow Length: 0.30000001192092896 + Min Arrow Length: 0.019999999552965164 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_cloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.30000001192092896 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealsenseCamera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/image_raw + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RealsenseDepthImage + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Realsense + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /camera_image_color + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -0.0008007669821381569 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 54 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: -5.409999847412109 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2126 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000018d000005affc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000027b0000015f0000010f00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003e2000002f50000022e00fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000001c00fffffffb0000001000530065006c006500630074006f007201000006df0000014b0000010000ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000da00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000004fa00000225fc0100000002fb0000000a0049006d0061006700650100000000000004fa0000006c00fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000365000005af00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RealsenseCamera: + collapsed: false + Selection: + collapsed: false + Selector: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1274 + X: 2560 + Y: 0 diff --git a/src/examples/turtlebot4/navigation_params_humble.yaml b/src/examples/turtlebot4/navigation_params_humble.yaml new file mode 100644 index 00000000..a806bb70 --- /dev/null +++ b/src/examples/turtlebot4/navigation_params_humble.yaml @@ -0,0 +1,598 @@ +--- +amcl: + ros__parameters: + use_sim_time: true + + global_frame_id: map + odom_frame_id: odom + base_frame_id: base_link + scan_topic: scan + + robot_model_type: nav2_amcl::DifferentialMotionModel + + set_initial_pose: true + always_reset_initial_pose: true + + tf_broadcast: true + transform_tolerance: 1.0 + + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + + # Beam skipping - ignores beams for which a majoirty of particles do not agree with the map + # prevents correct particles from getting down weighted because of unexpected obstacles + # such as humans + do_beamskip: false + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 12.0 + laser_min_range: 0.2 + laser_model_type: likelihood_field + max_beams: 60 + max_particles: 500 + min_particles: 200 + + pf_err: 0.05 + pf_z: 0.99 + + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + + resample_interval: 1 + save_pose_rate: 0.5 + sigma_hit: 0.2 + + update_min_a: 0.3 + update_min_d: 0.2 + + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + + initial_pose: + x: 0.0 + y: 0.0 + yaw: 0.0 + +amcl_map_client: + ros__parameters: + use_sim_time: true + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: true + +bt_navigator: + ros__parameters: + use_sim_time: true + global_frame: map + robot_base_frame: base_link + odom_topic: odometry/filtered + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: [navigate_to_pose, navigate_through_poses] + navigate_to_pose: + plugin: nav2_bt_navigator/NavigateToPoseNavigator + navigate_through_poses: + plugin: nav2_bt_navigator/NavigateThroughPosesNavigator + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: true + +controller_server: + ros__parameters: + use_sim_time: true + controller_frequency: 5.0 + min_x_velocity_threshold: 0.01 # Measured + min_y_velocity_threshold: 0.01 # Measured + min_theta_velocity_threshold: 0.5 # Measured + progress_checker_plugin: progress_checker + goal_checker_plugin: goal_checker + controller_plugins: [FollowPath] + + # Progress checker parameters + progress_checker: + plugin: nav2_controller::SimpleProgressChecker + required_movement_radius: 0.5 + movement_time_allowance: 30.0 + + # Goal checker parameters + goal_checker: + plugin: nav2_controller::SimpleGoalChecker + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.25 + stateful: true + + # MPPI controller + FollowPath: + plugin: nav2_mppi_controller::MPPIController + time_steps: 30 + model_dt: 0.3 + batch_size: 500 + vx_std: 0.2 + vy_std: 0.0 + wz_std: 0.35 + vx_max: 5.0 + vx_min: -0.2 + vy_max: 0.0 + wz_max: 5.0 + iteration_count: 1 + prune_distance: 1.8 + transform_tolerance: 0.25 + temperature: 0.3 + gamma: 0.015 + motion_model: DiffDrive + visualize: false + reset_period: 1.0 # (only in Humble) + regenerate_noises: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 5 + AckermannConstrains: + min_turning_r: 0.05 + critics: [ConstraintCritic, ObstaclesCritic, GoalCritic, GoalAngleCritic, PathAlignCritic, PathFollowCritic, + PathAngleCritic, PreferForwardCritic] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 25.0 + threshold_to_consider: 1.0 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + ObstaclesCritic: + enabled: true + cost_power: 1 + repulsion_weight: 1.5 + critical_weight: 20.0 + consider_footprint: false + collision_cost: 10000.0 + collision_margin_distance: 0.10 + near_goal_distance: 0.5 + inflation_radius: 1.0 # (only in Humble) + cost_scaling_factor: 3.0 # (only in Humble) + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 3 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 5 + threshold_to_consider: 1.0 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 100.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: true + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 3.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: true + rolling_window: true + width: 4 + height: 4 + resolution: 0.04 + footprint: '[[0.165, 0.145], [0.165, -0.145], [-0.165, -0.145], [-0.165, 0.145]]' + plugins: [static_layer, obstacle_layer, inflation_layer] + static_layer: + plugin: nav2_costmap_2d::StaticLayer + enabled: true + map_subscribe_transient_local: true + obstacle_layer: + plugin: nav2_costmap_2d::ObstacleLayer + enabled: true + observation_sources: scan + scan: + topic: scan + max_obstacle_height: 2.0 + clearing: true + marking: true + data_type: LaserScan + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + inflation_layer: + plugin: nav2_costmap_2d::InflationLayer + enabled: true + cost_scaling_factor: 3.0 + inflation_radius: 1.0 + + always_send_full_costmap: true + + local_costmap_client: + ros__parameters: + use_sim_time: true + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: true + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: true + footprint: '[[0.165, 0.145], [0.165, -0.145], [-0.165, -0.145], [-0.165, 0.145]]' + resolution: 0.05 + track_unknown_space: true + plugins: [static_layer, obstacle_layer, inflation_layer] + static_layer: + plugin: nav2_costmap_2d::StaticLayer + enabled: true + map_subscribe_transient_local: true + obstacle_layer: + plugin: nav2_costmap_2d::ObstacleLayer + enabled: true + observation_sources: scan + scan: + topic: scan + max_obstacle_height: 2.0 + clearing: true + marking: true + data_type: LaserScan + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + inflation_layer: + plugin: nav2_costmap_2d::InflationLayer + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: true + global_costmap_client: + ros__parameters: + use_sim_time: true + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: true + +map_server: + ros__parameters: + use_sim_time: true + yaml_filename: map.yaml + +map_saver: + ros__parameters: + use_sim_time: true + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: true + +planner_server: + ros__parameters: + use_sim_time: true + expected_planner_frequency: 1.0 + + planner_plugins: [GridBased] + GridBased: + plugin: nav2_smac_planner/SmacPlanner2D + tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: true # allow traveling in unknown space + max_iterations: 500000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 500 # maximum number of iterations to attempt to reach goal once in tolerance + max_planning_time: 3.0 # max time in s for planner to plan, smooth + cost_travel_multiplier: 2.5 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) + smoother: + max_iterations: 500 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-6 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: true + +behavior_server: + ros__parameters: + use_sim_time: true + + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.25 + + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 5.0 + + behavior_plugins: [spin, backup, drive_on_heading, assisted_teleop, wait] + spin: + plugin: nav2_behaviors/Spin + backup: + plugin: nav2_behaviors/BackUp + drive_on_heading: + plugin: nav2_behaviors/DriveOnHeading + wait: + plugin: nav2_behaviors/Wait + assisted_teleop: + plugin: nav2_behaviors/AssistedTeleop + + # spin & backup + simulate_ahead_time: 0.5 + + # spin + max_rotational_vel: 0.6 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: scan + use_map_saver: true + mode: mapping #localization + + # lifelong params + lifelong_search_use_tree: false + lifelong_minimum_score: 0.1 + lifelong_iou_match: 0.85 + lifelong_node_removal_score: 0.04 + lifelong_overlap_score_scale: 0.06 + lifelong_constraint_multiplier: 0.08 + lifelong_nearby_penalty: 0.001 + lifelong_candidates_scale: 0.03 + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.04 + map_update_interval: 3.0 + resolution: 0.05 + max_laser_range: 12.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 10. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.4 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + +waypoint_follower: + ros__parameters: + loop_rate: 5 + stop_on_failure: false + waypoint_task_executor_plugin: wait_at_waypoint + wait_at_waypoint: + plugin: nav2_waypoint_follower::WaitAtWaypoint + enabled: true + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: true + + smoothing_frequency: 5.0 + scale_velocities: false + feedback: OPEN_LOOP + max_velocity: [0.5, 0.0, 1.5] # Measured: MAX[1.0, 0.0, 3.14] (decreased for safety) + min_velocity: [-0.5, 0.0, -1.5] # Measured: MAX[1.0, 0.0, 3.14] (decreased for safety) + max_accel: [1.0, 0.0, 3.2] + max_decel: [-1.0, 0.0, -3.2] # Measured + # used in the CLOSED_LOOP feedback mode + # odom_topic: "odometry/filtered" + odom_duration: 0.1 + deadband_velocity: [0.01, 0.01, 0.1] # Measured + velocity_timeout: 1.0 + +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: scan + use_map_saver: true + mode: mapping #localization + + # lifelong params + lifelong_search_use_tree: false + lifelong_minimum_score: 0.1 + lifelong_iou_match: 0.85 + lifelong_node_removal_score: 0.04 + lifelong_overlap_score_scale: 0.06 + lifelong_constraint_multiplier: 0.08 + lifelong_nearby_penalty: 0.001 + lifelong_candidates_scale: 0.03 + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.04 + map_update_interval: 3.0 + resolution: 0.05 + max_laser_range: 12.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 10. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.4 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + +waypoint_follower: + ros__parameters: + loop_rate: 5 + stop_on_failure: false + waypoint_task_executor_plugin: wait_at_waypoint + wait_at_waypoint: + plugin: nav2_waypoint_follower::WaitAtWaypoint + enabled: true + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: true + + smoothing_frequency: 5.0 + scale_velocities: false + feedback: OPEN_LOOP + max_velocity: [0.5, 0.0, 1.5] # Measured: MAX[1.0, 0.0, 3.14] (decreased for safety) + min_velocity: [-0.5, 0.0, -1.5] # Measured: MAX[1.0, 0.0, 3.14] (decreased for safety) + max_accel: [1.0, 0.0, 3.2] + max_decel: [-1.0, 0.0, -3.2] # Measured + # used in the CLOSED_LOOP feedback mode + # odom_topic: "odometry/filtered" + odom_duration: 0.1 + deadband_velocity: [0.01, 0.01, 0.1] # Measured + velocity_timeout: 1.0 diff --git a/src/examples/turtlebot4/navigation_params_jazzy.yaml b/src/examples/turtlebot4/navigation_params_jazzy.yaml new file mode 100644 index 00000000..5d67effd --- /dev/null +++ b/src/examples/turtlebot4/navigation_params_jazzy.yaml @@ -0,0 +1,633 @@ +--- +amcl: + ros__parameters: + use_sim_time: true + + global_frame_id: map + odom_frame_id: odom + base_frame_id: base_link + scan_topic: scan + + robot_model_type: nav2_amcl::DifferentialMotionModel + + set_initial_pose: true + always_reset_initial_pose: true + + tf_broadcast: true + transform_tolerance: 1.0 + + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + + # Beam skipping - ignores beams for which a majoirty of particles do not agree with the map + # prevents correct particles from getting down weighted because of unexpected obstacles + # such as humans + do_beamskip: false + + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 12.0 + laser_min_range: 0.2 + laser_model_type: likelihood_field + max_beams: 60 + max_particles: 500 + min_particles: 200 + + pf_err: 0.05 + pf_z: 0.99 + + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + + resample_interval: 1 + save_pose_rate: 0.5 + sigma_hit: 0.2 + + update_min_a: 0.3 + update_min_d: 0.2 + + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + + initial_pose: + x: 0.0 + y: 0.0 + yaw: 0.0 + +amcl_map_client: + ros__parameters: + use_sim_time: true + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: true + +bt_navigator: + ros__parameters: + use_sim_time: true + global_frame: map + robot_base_frame: base_link + odom_topic: odometry/filtered + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: [navigate_to_pose, navigate_through_poses] + navigate_to_pose: + plugin: nav2_bt_navigator::NavigateToPoseNavigator + navigate_through_poses: + plugin: nav2_bt_navigator::NavigateThroughPosesNavigator + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: true + +controller_server: + ros__parameters: + use_sim_time: true + controller_frequency: 5.0 + min_x_velocity_threshold: 0.01 # Measured + min_y_velocity_threshold: 0.01 # Measured + min_theta_velocity_threshold: 0.5 # Measured + progress_checker_plugin: progress_checker + goal_checker_plugin: goal_checker + controller_plugins: [FollowPath] + + # Progress checker parameters + progress_checker: + plugin: nav2_controller::SimpleProgressChecker + required_movement_radius: 0.5 + movement_time_allowance: 30.0 + + # Goal checker parameters + goal_checker: + plugin: nav2_controller::SimpleGoalChecker + xy_goal_tolerance: 0.15 + yaw_goal_tolerance: 0.25 + stateful: true + + # MPPI controller + FollowPath: + plugin: nav2_mppi_controller::MPPIController + time_steps: 30 + model_dt: 0.3 + batch_size: 500 + vx_std: 0.2 + vy_std: 0.0 + wz_std: 0.35 + vx_max: 5.0 + vx_min: -0.2 + vy_max: 0.0 + wz_max: 5.0 + iteration_count: 1 + prune_distance: 1.8 + transform_tolerance: 0.25 + temperature: 0.3 + gamma: 0.015 + motion_model: DiffDrive + visualize: false + reset_period: 1.0 # (only in Humble) + regenerate_noises: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 5 + AckermannConstrains: + min_turning_r: 0.05 + critics: + [ + ConstraintCritic, + ObstaclesCritic, + GoalCritic, + GoalAngleCritic, + PathAlignCritic, + PathFollowCritic, + PathAngleCritic, + PreferForwardCritic, + ] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 25.0 + threshold_to_consider: 1.0 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + ObstaclesCritic: + enabled: true + cost_power: 1 + repulsion_weight: 1.5 + critical_weight: 20.0 + consider_footprint: false + collision_cost: 10000.0 + collision_margin_distance: 0.10 + near_goal_distance: 0.5 + inflation_radius: 1.0 # (only in Humble) + cost_scaling_factor: 3.0 # (only in Humble) + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 3 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 5 + threshold_to_consider: 1.0 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 100.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: true + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 3.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: true + rolling_window: true + width: 4 + height: 4 + resolution: 0.04 + footprint: "[[0.165, 0.145], [0.165, -0.145], [-0.165, -0.145], [-0.165, 0.145]]" + plugins: [static_layer, obstacle_layer, inflation_layer] + static_layer: + plugin: nav2_costmap_2d::StaticLayer + enabled: true + map_subscribe_transient_local: true + obstacle_layer: + plugin: nav2_costmap_2d::ObstacleLayer + enabled: true + observation_sources: scan + scan: + topic: scan + max_obstacle_height: 2.0 + clearing: true + marking: true + data_type: LaserScan + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + inflation_layer: + plugin: nav2_costmap_2d::InflationLayer + enabled: true + cost_scaling_factor: 3.0 + inflation_radius: 1.0 + + always_send_full_costmap: true + + local_costmap_client: + ros__parameters: + use_sim_time: true + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: true + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: true + footprint: "[[0.165, 0.145], [0.165, -0.145], [-0.165, -0.145], [-0.165, 0.145]]" + resolution: 0.05 + track_unknown_space: true + plugins: [static_layer, obstacle_layer, inflation_layer] + static_layer: + plugin: nav2_costmap_2d::StaticLayer + enabled: true + map_subscribe_transient_local: true + obstacle_layer: + plugin: nav2_costmap_2d::ObstacleLayer + enabled: true + observation_sources: scan + scan: + topic: scan + max_obstacle_height: 2.0 + clearing: true + marking: true + data_type: LaserScan + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + inflation_layer: + plugin: nav2_costmap_2d::InflationLayer + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: true + global_costmap_client: + ros__parameters: + use_sim_time: true + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: true + +map_server: + ros__parameters: + use_sim_time: true + yaml_filename: map.yaml + +map_saver: + ros__parameters: + use_sim_time: true + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: true + +planner_server: + ros__parameters: + use_sim_time: true + expected_planner_frequency: 1.0 + + planner_plugins: [GridBased] + GridBased: + plugin: nav2_smac_planner::SmacPlanner2D + tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: true # allow traveling in unknown space + max_iterations: 500000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 500 # maximum number of iterations to attempt to reach goal once in tolerance + max_planning_time: 3.0 # max time in s for planner to plan, smooth + cost_travel_multiplier: 2.5 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) + smoother: + max_iterations: 500 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-6 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: true + +behavior_server: + ros__parameters: + use_sim_time: true + + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.25 + + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 5.0 + + behavior_plugins: [spin, backup, drive_on_heading, assisted_teleop, wait] + spin: + plugin: nav2_behaviors::Spin + backup: + plugin: nav2_behaviors::BackUp + drive_on_heading: + plugin: nav2_behaviors::DriveOnHeading + wait: + plugin: nav2_behaviors::Wait + assisted_teleop: + plugin: nav2_behaviors::AssistedTeleop + + # spin & backup + simulate_ahead_time: 0.5 + + # spin + max_rotational_vel: 0.6 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +slam_toolbox: + ros__parameters: + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: scan + use_map_saver: true + mode: mapping #localization + + # lifelong params + lifelong_search_use_tree: false + lifelong_minimum_score: 0.1 + lifelong_iou_match: 0.85 + lifelong_node_removal_score: 0.04 + lifelong_overlap_score_scale: 0.06 + lifelong_constraint_multiplier: 0.08 + lifelong_nearby_penalty: 0.001 + lifelong_candidates_scale: 0.03 + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.04 + map_update_interval: 3.0 + resolution: 0.05 + max_laser_range: 12.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 10. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.4 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true + +waypoint_follower: + ros__parameters: + loop_rate: 5 + stop_on_failure: false + waypoint_task_executor_plugin: wait_at_waypoint + wait_at_waypoint: + plugin: nav2_waypoint_follower::WaitAtWaypoint + enabled: true + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: true + + smoothing_frequency: 5.0 + scale_velocities: false + feedback: OPEN_LOOP + max_velocity: [0.5, 0.0, 1.5] # Measured: MAX[1.0, 0.0, 3.14] (decreased for safety) + min_velocity: [-0.5, 0.0, -1.5] # Measured: MAX[1.0, 0.0, 3.14] (decreased for safety) + max_accel: [1.0, 0.0, 3.2] + max_decel: [-1.0, 0.0, -3.2] # Measured + # used in the CLOSED_LOOP feedback mode + # odom_topic: "odometry/filtered" + odom_duration: 0.1 + deadband_velocity: [0.01, 0.01, 0.1] # Measured + velocity_timeout: 1.0 + +docking_server: + ros__parameters: + # Types of docks + dock_plugins: ["nova_carter_dock"] + nova_carter_dock: + plugin: "opennav_docking::SimpleChargingDock" + # More parameters exist here that we will discuss later in the tutorial + + # Dock instances + docks: ["home_dock", "flex_dock1", "flex_dock2"] + home_dock: + type: "nova_carter_dock" + frame: map + pose: [0.0, 0.0, 0.0] + flex_dock1: + type: "nova_carter_dock" + frame: map + pose: [10.0, 10.0, 0.0] + flex_dock2: + type: "nova_carter_dock" + frame: map + pose: [30.0, 30.0, 0.0] + +collision_detector: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + transform_tolerance: 0.5 + source_timeout: 5.0 + base_shift_correction: True + polygons: ["PolygonFront"] + PolygonFront: + type: "polygon" + points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]" + action_type: "none" + min_points: 4 + visualize: True + polygon_pub_topic: "polygon_front" + observation_sources: ["scan"] + scan: + source_timeout: 0.2 + type: "scan" + topic: "scan" + enabled: True + pointcloud: + type: "pointcloud" + topic: "/intel_realsense_r200_depth/points" + min_height: 0.1 + max_height: 0.5 + enabled: True + +smoother_server: + ros__parameters: + costmap_topic: global_costmap/costmap_raw + footprint_topic: global_costmap/published_footprint + robot_base_frame: base_link + transform_timeout: 0.1 + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + do_refinement: True + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.5 + source_timeout: 5.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + enable_stamped_cmd_vel: False + use_realtime_priority: false + polygons: ["PolygonStop", "PolygonSlow", "FootprintApproach"] + PolygonStop: + type: "circle" + radius: 0.3 + action_type: "stop" + min_points: 4 # max_points: 3 for Humble + visualize: True + polygon_pub_topic: "polygon_stop" + enabled: False + PolygonSlow: + type: "polygon" + points: "[[1.0, 1.0], [1.0, -1.0], [-0.5, -1.0], [-0.5, 1.0]]" + action_type: "slowdown" + min_points: 4 # max_points: 3 for Humble + slowdown_ratio: 0.3 + visualize: True + polygon_pub_topic: "polygon_slowdown" + enabled: False + PolygonLimit: + type: "polygon" + points: "[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]" + action_type: "limit" + min_points: 4 # max_points: 3 for Humble + linear_limit: 0.4 + angular_limit: 0.5 + visualize: True + polygon_pub_topic: "polygon_limit" + enabled: False + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.02 + min_points: 6 # max_points: 5 for Humble + visualize: False + enabled: False + VelocityPolygonStop: + type: "velocity_polygon" + action_type: "stop" + min_points: 6 + visualize: True + enabled: False + polygon_pub_topic: "velocity_polygon_stop" + velocity_polygons: + ["rotation", "translation_forward", "translation_backward", "stopped"] + holonomic: false + rotation: + points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]" + linear_min: 0.0 + linear_max: 0.05 + theta_min: -1.0 + theta_max: 1.0 + translation_forward: + points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]" + linear_min: 0.0 + linear_max: 1.0 + theta_min: -1.0 + theta_max: 1.0 + translation_backward: + points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]" + linear_min: -1.0 + linear_max: 0.0 + theta_min: -1.0 + theta_max: 1.0 + # This is the last polygon to be checked, it should cover the entire range of robot's velocities + # It is used as the stopped polygon when the robot is not moving and as a fallback if the velocity + # is not covered by any of the other sub-polygons + stopped: + points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]" + linear_min: -1.0 + linear_max: 1.0 + theta_min: -1.0 + theta_max: 1.0 + observation_sources: ["scan", "pointcloud"] + scan: + source_timeout: 0.2 + type: "scan" + topic: "/scan" + enabled: False + pointcloud: + type: "pointcloud" + topic: "/points" + min_height: 0.1 + max_height: 0.5 + enabled: False diff --git a/src/examples/turtlebot4/run-nav.bash b/src/examples/turtlebot4/run-nav.bash new file mode 100755 index 00000000..c2b4409c --- /dev/null +++ b/src/examples/turtlebot4/run-nav.bash @@ -0,0 +1,6 @@ +#!/bin/bash + +ros2 launch nav2_bringup bringup_launch.py \ + slam:="${SLAM:-True}" \ + params_file:=./navigation_params_"${ROS_DISTRO}".yaml \ + use_sim_time:=True diff --git a/src/examples/turtlebot4/turtlebot.launch.xml b/src/examples/turtlebot4/turtlebot.launch.xml index 141c56a1..55934da8 100644 --- a/src/examples/turtlebot4/turtlebot.launch.xml +++ b/src/examples/turtlebot4/turtlebot.launch.xml @@ -33,4 +33,12 @@ cmd="$(var game_launcher) -bg_ConnectToAssetProcessor=0" output="screen"> + + + + + From 758961b39d8aea97747f18428893d2285b57055f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 8 Oct 2024 10:01:14 +0200 Subject: [PATCH 04/58] feat(`turtlebot`): add ros2 interfaces whitelist and improve system prompt --- examples/turtlebot_demo.py | 28 +++++++++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/examples/turtlebot_demo.py b/examples/turtlebot_demo.py index fd4c38fe..07040c9f 100644 --- a/examples/turtlebot_demo.py +++ b/examples/turtlebot_demo.py @@ -32,6 +32,7 @@ Ros2ShowMsgInterfaceTool, ) from rai.tools.ros.native_actions import Ros2RunActionSync +from rai.tools.ros.tools import GetCurrentPositionTool from rai.tools.time import WaitForSecondsTool @@ -43,10 +44,24 @@ def main(): SYSTEM_PROMPT = "" + ros2_whitelist = [ + "/cmd_vel", + "/rosout", + "/map", + "/odom", + "/camera_image_color", + "/backup", + "/drive_on_heading", + "/navigate_through_poses", + "/navigate_to_pose", + "/spin", + ] + node = RaiNode( llm=ChatOpenAI( model="gpt-4o-mini" ), # smaller model used to describe the environment + whitelist=ros2_whitelist, system_prompt=SYSTEM_PROMPT, ) @@ -57,6 +72,7 @@ def main(): Ros2ShowMsgInterfaceTool(), Ros2PubMessageTool(node=node), Ros2RunActionSync(node=node), + GetCurrentPositionTool(), ] state_retriever = node.get_robot_state @@ -67,7 +83,17 @@ def main(): Here are your tools: {render_text_description_and_args(tools)} - You can use ros2 topics, services and actions to operate. """ + You can use ros2 topics, services and actions to operate. + + Navigation tips: + - for driving forward/backward, if specified, ros2 actions are better. + - for driving for some specific time or in specific manner (like in circle) it good to use /cmd_vel topic + - you are currently unable to read map or point-cloud, so please avoid subscribing to such topics. + - if you are asked to drive towards some object, it's good to: + 1. check the camera image and verify if objects can be seen + 2. if only driving forward is required, do it + 3. if obstacle avoidance might be required, use ros2 actions navigate_*, but first check your currect position, then very accurately estimate the goal pose. + """ node.get_logger().info(f"{SYSTEM_PROMPT=}") From 82d99dd620c9edaf6dead47e9b6ef7063396be78 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 8 Oct 2024 10:01:55 +0200 Subject: [PATCH 05/58] chore: remove unnecessary TODO --- src/rai_hmi/rai_hmi/base.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/rai_hmi/rai_hmi/base.py b/src/rai_hmi/rai_hmi/base.py index d64c9067..41c82442 100644 --- a/src/rai_hmi/rai_hmi/base.py +++ b/src/rai_hmi/rai_hmi/base.py @@ -123,7 +123,6 @@ def __init__( self.faiss_index = self._load_documentation() self.tools = self._initialize_available_tools() - # TODO(boczekbartek): refactor, becuase mixin needs state self.initialize_task_action_client_and_server() self.task_running = dict() self.task_feedbacks = queue From 714e349494214f7e10b04b5d0ba7c49a34d1784f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 8 Oct 2024 10:02:51 +0200 Subject: [PATCH 06/58] WIP: add ros2 interfaces whitelist to hmi --- src/rai_hmi/rai_hmi/ros.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/rai_hmi/rai_hmi/ros.py b/src/rai_hmi/rai_hmi/ros.py index 89e885b9..18707c8b 100644 --- a/src/rai_hmi/rai_hmi/ros.py +++ b/src/rai_hmi/rai_hmi/ros.py @@ -36,7 +36,20 @@ def initialize_ros_nodes( ) # TODO(boczekbartek): this node shouldn't be required to initialize simple ros2 tools - rai_node = RaiBaseNode(node_name="__rai_node__") + ros2_whitelist = [ + "/cmd_vel", + "/rosout", + "/map", + "/odom", + "/camera_image_color", + "/backup", + "/drive_on_heading", + "/navigate_through_poses", + "/navigate_to_pose", + "/spin", + ] + + rai_node = RaiBaseNode(node_name="__rai_node__", whitelist=ros2_whitelist) executor = MultiThreadedExecutor() executor.add_node(hmi_node) From 417b6fb83d3d20e3b2fead2a6e3cfd0a51379293 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 8 Oct 2024 10:42:18 +0200 Subject: [PATCH 07/58] docs(`turtlebot`): add more examples of interaction --- src/examples/turtlebot4/README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index 72768fe6..b19a346f 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -66,7 +66,11 @@ TBD - Is it able to bring you something from the kitchen? (testing robot's identity) - What are its ros2 interfaces (discovery of ros2 interfaces) - tell me what you see (interpretation of camera image) - - Drive forward (interaction with the robot using discovered ros2 interfaces) + - Drive towards the table (when table is not visible, robot rejects task that it cannot do) + - Spin left by 90 degrees (interaction with the robot using ros2 interfaces) - table with the robotic are shoud be visible in the camera + - Use robotic arm to pick up a box from the table (identity and intefaces doesn't allow it) + - Drive towards the table (when table is visible, testing ability to interpret camera image and perform actions based on the knowledge) + - Drive behind the table (likely will fail in current `rai` version due to limitations in spatial reasoning) ### Troubleshooting From 6ce525977013632a2d8c1016f795790aba30ab64 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 8 Oct 2024 11:40:15 +0200 Subject: [PATCH 08/58] docs(`turtlebot`): add infomation about simulation binary --- src/examples/turtlebot4/README.md | 17 +++++++++++++++-- src/examples/turtlebot4/simulation/.gitkeep | 0 2 files changed, 15 insertions(+), 2 deletions(-) create mode 100644 src/examples/turtlebot4/simulation/.gitkeep diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index b19a346f..b232b8c6 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -10,7 +10,20 @@ TBD 1. O3DE simulation setup - TBD + 1. Download turtlebot4 simulation binary matching your platform from here (TODO link). + + > **NOTE** If you would like to make changes to the simulation and create your own binary please follow this [README.md](https://github.com/RobotecAI/ROSCon2024Tutorial/README.md) + + 2. Unzip it: + + ```bash + cd rai + # for Ubuntu 22.04 jammy and ros2 humlbe + unzip -d src/examples/turtlebot4/simulation Turtlebot4_PR1_jammyhumlbe.zip + + # for Ubuntu 24.04 noble and ros2 jazzy + unzip -d src/examples/turtlebot4/simulation Turtlebot4_PR1_noblejazzy.zip + ``` 2. Clone and install [rai](https://github.com/RobotecAI/rai) @@ -53,7 +66,7 @@ TBD ```bash ros launch ./src/examples/turtlebot4/turtlebot.launch.xml \ - game_launcher:=/path/to/simulation_binary/Turtlebot4.GameLauncher + game_launcher:=./src/examples/turtlebot4/simulation/Turtlebot4.GameLauncher ``` 6. Open you internet browser and go to `localhost:8501` diff --git a/src/examples/turtlebot4/simulation/.gitkeep b/src/examples/turtlebot4/simulation/.gitkeep new file mode 100644 index 00000000..e69de29b From 43604da36d4e107b194c58fdb42996c6cdc43be9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 15 Oct 2024 15:56:03 +0200 Subject: [PATCH 09/58] improve readme --- examples/turtlebot_demo.py | 117 ------------------- src/examples/turtlebot4/README.md | 36 +++--- src/examples/turtlebot4/turtlebot.launch.xml | 2 +- 3 files changed, 24 insertions(+), 131 deletions(-) delete mode 100644 examples/turtlebot_demo.py diff --git a/examples/turtlebot_demo.py b/examples/turtlebot_demo.py deleted file mode 100644 index 07040c9f..00000000 --- a/examples/turtlebot_demo.py +++ /dev/null @@ -1,117 +0,0 @@ -# Copyright (C) 2024 Robotec.AI -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# - - -import rclpy -import rclpy.callback_groups -import rclpy.executors -import rclpy.qos -import rclpy.subscription -import rclpy.task -from langchain.tools.render import render_text_description_and_args -from langchain_openai import ChatOpenAI - -from rai.agents.state_based import create_state_based_agent -from rai.node import RaiNode -from rai.tools.ros.native import ( - GetCameraImage, - GetMsgFromTopic, - Ros2PubMessageTool, - Ros2ShowMsgInterfaceTool, -) -from rai.tools.ros.native_actions import Ros2RunActionSync -from rai.tools.ros.tools import GetCurrentPositionTool -from rai.tools.time import WaitForSecondsTool - - -def main(): - rclpy.init() - llm = ChatOpenAI(model="gpt-4o") - - # TODO(boczekbartek): refactor system prompt - - SYSTEM_PROMPT = "" - - ros2_whitelist = [ - "/cmd_vel", - "/rosout", - "/map", - "/odom", - "/camera_image_color", - "/backup", - "/drive_on_heading", - "/navigate_through_poses", - "/navigate_to_pose", - "/spin", - ] - - node = RaiNode( - llm=ChatOpenAI( - model="gpt-4o-mini" - ), # smaller model used to describe the environment - whitelist=ros2_whitelist, - system_prompt=SYSTEM_PROMPT, - ) - - tools = [ - WaitForSecondsTool(), - GetMsgFromTopic(node=node), - GetCameraImage(node=node), - Ros2ShowMsgInterfaceTool(), - Ros2PubMessageTool(node=node), - Ros2RunActionSync(node=node), - GetCurrentPositionTool(), - ] - - state_retriever = node.get_robot_state - - SYSTEM_PROMPT = f"""You are an autonomous robot connected to ros2 environment. Your main goal is to fulfill the user's requests. - Do not make assumptions about the environment you are currently in. - - Here are your tools: - {render_text_description_and_args(tools)} - - You can use ros2 topics, services and actions to operate. - - Navigation tips: - - for driving forward/backward, if specified, ros2 actions are better. - - for driving for some specific time or in specific manner (like in circle) it good to use /cmd_vel topic - - you are currently unable to read map or point-cloud, so please avoid subscribing to such topics. - - if you are asked to drive towards some object, it's good to: - 1. check the camera image and verify if objects can be seen - 2. if only driving forward is required, do it - 3. if obstacle avoidance might be required, use ros2 actions navigate_*, but first check your currect position, then very accurately estimate the goal pose. - """ - - node.get_logger().info(f"{SYSTEM_PROMPT=}") - - node.system_prompt = node.initialize_system_prompt(SYSTEM_PROMPT) - - app = create_state_based_agent( - llm=llm, - tools=tools, - state_retriever=state_retriever, - logger=node.get_logger(), - ) - - node.set_app(app) - - executor = rclpy.executors.MultiThreadedExecutor() - executor.add_node(node) - executor.spin() - rclpy.shutdown() - - -main() diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index b232b8c6..d9abe0f4 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -1,6 +1,7 @@ # Turtlebot4 tutorial -Following tutorial will be presented during [ROSCon 2024](https://roscon.ros.org/2024/) talk about RAI. +The following tutorial will be presented during [ROSCon 2024](https://roscon.ros.org/2024/) +talk about RAI. ## Recording @@ -10,19 +11,23 @@ TBD 1. O3DE simulation setup - 1. Download turtlebot4 simulation binary matching your platform from here (TODO link). + 1. Download turtlebot4 simulation binary matching your platform from : - > **NOTE** If you would like to make changes to the simulation and create your own binary please follow this [README.md](https://github.com/RobotecAI/ROSCon2024Tutorial/README.md) + - Ubuntu 22.04 & ros2 humble: [link](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/EZLmGtPNgl9Kiu4royJJnVgB5tjS2Vze0myXDyVJtNcnRw?e=L42Z4z) + - Ubuntu 24.04 & ros2 jazzy: [link](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/Ebi0O3eWAtRAl3GL8LuEBqIB0-5fnHDCm7yOtcTucpNfzA?e=G6eJKP) + + > **NOTE** If you would like to make changes to the simulation and create your + > own binary please follow this [README.md](https://github.com/RobotecAI/ROSCon2024Tutorial/README.md) 2. Unzip it: ```bash cd rai # for Ubuntu 22.04 jammy and ros2 humlbe - unzip -d src/examples/turtlebot4/simulation Turtlebot4_PR1_jammyhumlbe.zip + unzip -d src/examples/turtlebot4/simulation Turtlebot4_jammyhumble_0.0.1.zip # for Ubuntu 24.04 noble and ros2 jazzy - unzip -d src/examples/turtlebot4/simulation Turtlebot4_PR1_noblejazzy.zip + unzip -d src/examples/turtlebot4/simulation Turtlebot4_noblejazzy_0.0.1.zip ``` 2. Clone and install [rai](https://github.com/RobotecAI/rai) @@ -37,13 +42,14 @@ TBD 1. Create `whoami` package for turtlebot4 in `src/examples/turtlebot4` ```bash + . ./setup_shell.sh poetry run create_rai_ws --name turtlebot4 --destination-directory src/examples ``` 2. Download official turtlebot4 [data sheet](https://bit.ly/3KCp3Du) into `src/examples/turtlebot4_whoami/description/documentation` 3. Download [image](https://s3.amazonaws.com/assets.clearpathrobotics.com/wp-content/uploads/2022/03/16113604/Turtlebot-4-20220207.44.png) of turtlebot4 into `src/examples/turtlebot4_whoami/description/images` - 4. Run theย `parse_whoami_package`. This will process the documentation, building + 4. Create robot's identity. Run theย `parse_whoami_package`. This will process the documentation, building it into a vector database, which is used by RAI agent to reason about its identity. > **NOTE**: Vector database is created using the OpenAI API. Parsing bigger documents @@ -52,6 +58,7 @@ TBD ```bash poetry run parse_whoami_package src/examples/turtlebot4_whoami/description + # you will be asked to press `y` to continue ``` 5. Rebuild the workspace @@ -65,8 +72,8 @@ TBD 5. Run rai agent: ```bash - ros launch ./src/examples/turtlebot4/turtlebot.launch.xml \ - game_launcher:=./src/examples/turtlebot4/simulation/Turtlebot4.GameLauncher + ros2 launch ./src/examples/turtlebot4/turtlebot.launch.xml \ + game_launcher:=./src/examples/turtlebot4/simulation/TurtleBot4DemoGamePackage/TurtleBot4Demo.GameLauncher ``` 6. Open you internet browser and go to `localhost:8501` @@ -74,13 +81,16 @@ TBD with the RAI HMI Agent. On the right you will see status of missions that were send to the RAI Node and are executed on your robot. - For example you can try: + - `HMI Agent` is responsible for human-robot interaction. + - `RAI Node` is responsible for executing tasks on the robot. + + For example you can try such prompt: - - Is it able to bring you something from the kitchen? (testing robot's identity) - - What are its ros2 interfaces (discovery of ros2 interfaces) + - Are you able to bring you something from the kitchen? (testing robot's identity) + - What are your ros2 interfaces? (discovery of ros2 interfaces) - tell me what you see (interpretation of camera image) - - Drive towards the table (when table is not visible, robot rejects task that it cannot do) - - Spin left by 90 degrees (interaction with the robot using ros2 interfaces) - table with the robotic are shoud be visible in the camera + - Drive towards the chair (when table is not visible, robot rejects task that it cannot do) + - Spin yourself left by 45 degrees (interaction with the robot using ros2 interfaces) - table with the robotic are shoud be visible in the camera - Use robotic arm to pick up a box from the table (identity and intefaces doesn't allow it) - Drive towards the table (when table is visible, testing ability to interpret camera image and perform actions based on the knowledge) - Drive behind the table (likely will fail in current `rai` version due to limitations in spatial reasoning) diff --git a/src/examples/turtlebot4/turtlebot.launch.xml b/src/examples/turtlebot4/turtlebot.launch.xml index 55934da8..cc078609 100644 --- a/src/examples/turtlebot4/turtlebot.launch.xml +++ b/src/examples/turtlebot4/turtlebot.launch.xml @@ -26,7 +26,7 @@ output="screen"> Date: Tue, 15 Oct 2024 16:07:08 +0200 Subject: [PATCH 10/58] fix humble config --- .../turtlebot4/navigation_params_humble.yaml | 130 +++--------------- 1 file changed, 17 insertions(+), 113 deletions(-) diff --git a/src/examples/turtlebot4/navigation_params_humble.yaml b/src/examples/turtlebot4/navigation_params_humble.yaml index a806bb70..03a40153 100644 --- a/src/examples/turtlebot4/navigation_params_humble.yaml +++ b/src/examples/turtlebot4/navigation_params_humble.yaml @@ -119,8 +119,8 @@ controller_server: ros__parameters: use_sim_time: true controller_frequency: 5.0 - min_x_velocity_threshold: 0.01 # Measured - min_y_velocity_threshold: 0.01 # Measured + min_x_velocity_threshold: 0.01 # Measured + min_y_velocity_threshold: 0.01 # Measured min_theta_velocity_threshold: 0.5 # Measured progress_checker_plugin: progress_checker goal_checker_plugin: goal_checker @@ -166,8 +166,17 @@ controller_server: time_step: 5 AckermannConstrains: min_turning_r: 0.05 - critics: [ConstraintCritic, ObstaclesCritic, GoalCritic, GoalAngleCritic, PathAlignCritic, PathFollowCritic, - PathAngleCritic, PreferForwardCritic] + critics: + [ + ConstraintCritic, + ObstaclesCritic, + GoalCritic, + GoalAngleCritic, + PathAlignCritic, + PathFollowCritic, + PathAngleCritic, + PreferForwardCritic, + ] ConstraintCritic: enabled: true cost_power: 1 @@ -240,7 +249,7 @@ local_costmap: width: 4 height: 4 resolution: 0.04 - footprint: '[[0.165, 0.145], [0.165, -0.145], [-0.165, -0.145], [-0.165, 0.145]]' + footprint: "[[0.165, 0.145], [0.165, -0.145], [-0.165, -0.145], [-0.165, 0.145]]" plugins: [static_layer, obstacle_layer, inflation_layer] static_layer: plugin: nav2_costmap_2d::StaticLayer @@ -283,7 +292,7 @@ global_costmap: global_frame: map robot_base_frame: base_link use_sim_time: true - footprint: '[[0.165, 0.145], [0.165, -0.145], [-0.165, -0.145], [-0.165, 0.145]]' + footprint: "[[0.165, 0.145], [0.165, -0.145], [-0.165, -0.145], [-0.165, 0.145]]" resolution: 0.05 track_unknown_space: true plugins: [static_layer, obstacle_layer, inflation_layer] @@ -391,111 +400,6 @@ behavior_server: slam_toolbox: ros__parameters: - - # Plugin params - solver_plugin: solver_plugins::CeresSolver - ceres_linear_solver: SPARSE_NORMAL_CHOLESKY - ceres_preconditioner: SCHUR_JACOBI - ceres_trust_strategy: LEVENBERG_MARQUARDT - ceres_dogleg_type: TRADITIONAL_DOGLEG - ceres_loss_function: None - - # ROS Parameters - odom_frame: odom - map_frame: map - base_frame: base_link - scan_topic: scan - use_map_saver: true - mode: mapping #localization - - # lifelong params - lifelong_search_use_tree: false - lifelong_minimum_score: 0.1 - lifelong_iou_match: 0.85 - lifelong_node_removal_score: 0.04 - lifelong_overlap_score_scale: 0.06 - lifelong_constraint_multiplier: 0.08 - lifelong_nearby_penalty: 0.001 - lifelong_candidates_scale: 0.03 - - debug_logging: false - throttle_scans: 1 - transform_publish_period: 0.04 - map_update_interval: 3.0 - resolution: 0.05 - max_laser_range: 12.0 #for rastering images - minimum_time_interval: 0.5 - transform_timeout: 0.2 - tf_buffer_duration: 10. - stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps - - # General Parameters - use_scan_matching: true - use_scan_barycenter: true - minimum_travel_distance: 0.4 - minimum_travel_heading: 0.5 - scan_buffer_size: 10 - scan_buffer_maximum_scan_distance: 10.0 - link_match_minimum_response_fine: 0.1 - link_scan_maximum_distance: 1.5 - loop_search_maximum_distance: 3.0 - do_loop_closing: true - loop_match_minimum_chain_size: 10 - loop_match_maximum_variance_coarse: 3.0 - loop_match_minimum_response_coarse: 0.35 - loop_match_minimum_response_fine: 0.45 - - # Correlation Parameters - Correlation Parameters - correlation_search_space_dimension: 0.5 - correlation_search_space_resolution: 0.01 - correlation_search_space_smear_deviation: 0.1 - - # Correlation Parameters - Loop Closure Parameters - loop_search_space_dimension: 8.0 - loop_search_space_resolution: 0.05 - loop_search_space_smear_deviation: 0.03 - - # Scan Matcher Parameters - distance_variance_penalty: 0.5 - angle_variance_penalty: 1.0 - - fine_search_angle_offset: 0.00349 - coarse_search_angle_offset: 0.349 - coarse_angle_resolution: 0.0349 - minimum_angle_penalty: 0.9 - minimum_distance_penalty: 0.5 - use_response_expansion: true - -waypoint_follower: - ros__parameters: - loop_rate: 5 - stop_on_failure: false - waypoint_task_executor_plugin: wait_at_waypoint - wait_at_waypoint: - plugin: nav2_waypoint_follower::WaitAtWaypoint - enabled: true - waypoint_pause_duration: 200 - -velocity_smoother: - ros__parameters: - use_sim_time: true - - smoothing_frequency: 5.0 - scale_velocities: false - feedback: OPEN_LOOP - max_velocity: [0.5, 0.0, 1.5] # Measured: MAX[1.0, 0.0, 3.14] (decreased for safety) - min_velocity: [-0.5, 0.0, -1.5] # Measured: MAX[1.0, 0.0, 3.14] (decreased for safety) - max_accel: [1.0, 0.0, 3.2] - max_decel: [-1.0, 0.0, -3.2] # Measured - # used in the CLOSED_LOOP feedback mode - # odom_topic: "odometry/filtered" - odom_duration: 0.1 - deadband_velocity: [0.01, 0.01, 0.1] # Measured - velocity_timeout: 1.0 - -slam_toolbox: - ros__parameters: - # Plugin params solver_plugin: solver_plugins::CeresSolver ceres_linear_solver: SPARSE_NORMAL_CHOLESKY @@ -587,10 +491,10 @@ velocity_smoother: smoothing_frequency: 5.0 scale_velocities: false feedback: OPEN_LOOP - max_velocity: [0.5, 0.0, 1.5] # Measured: MAX[1.0, 0.0, 3.14] (decreased for safety) + max_velocity: [0.5, 0.0, 1.5] # Measured: MAX[1.0, 0.0, 3.14] (decreased for safety) min_velocity: [-0.5, 0.0, -1.5] # Measured: MAX[1.0, 0.0, 3.14] (decreased for safety) max_accel: [1.0, 0.0, 3.2] - max_decel: [-1.0, 0.0, -3.2] # Measured + max_decel: [-1.0, 0.0, -3.2] # Measured # used in the CLOSED_LOOP feedback mode # odom_topic: "odometry/filtered" odom_duration: 0.1 From 7f68c2f1ab3a009c4f0bb46ad5608164ff1f1ad1 Mon Sep 17 00:00:00 2001 From: Bartek Boczek <22739059+boczekbartek@users.noreply.github.com> Date: Tue, 15 Oct 2024 18:57:44 +0200 Subject: [PATCH 11/58] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Adam Dฤ…browski --- src/examples/turtlebot4/README.md | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index d9abe0f4..6c65c5ef 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -32,7 +32,7 @@ TBD 2. Clone and install [rai](https://github.com/RobotecAI/rai) - 1. Follow setps from [1. Setting up the workspace](https://github.com/RobotecAI/rai?tab=readme-ov-file#1-setting-up-the-workspace) + 1. Follow steps from [1. Setting up the workspace](https://github.com/RobotecAI/rai?tab=readme-ov-file#1-setting-up-the-workspace) 2. Then follow steps from [2. Build the project](https://github.com/RobotecAI/rai?tab=readme-ov-file#2-build-the-project) 3. Setup your [OpenAI API](https://openai.com/index/openai-api/). Make sure you have set `OPENAI_API_KEY` variable. @@ -61,13 +61,13 @@ TBD # you will be asked to press `y` to continue ``` - 5. Rebuild the workspace + 5. Build the workspace which now includes the new package ```bash colcon build --symlink-install ``` - Congratulations! Your `rai_whoami_node` is configured. In the follwing steps - your RAI agent will have turtlebot4 personality. + Congratulations! Your `rai_whoami_node` is configured. In the following steps + your RAI agent will assume a turtlebot4 "personality". 5. Run rai agent: @@ -77,14 +77,14 @@ TBD ``` 6. Open you internet browser and go to `localhost:8501` -7. You can interact with your RAI agent through the chat. On the left you can interact +7. Open your internet browser with the address `localhost:8501` to can interact with your RAI agent through the chat. On the left you can communicate with the RAI HMI Agent. On the right you will see status of missions that were send to the RAI Node and are executed on your robot. - `HMI Agent` is responsible for human-robot interaction. - `RAI Node` is responsible for executing tasks on the robot. - For example you can try such prompt: + For example you can try such prompts: - Are you able to bring you something from the kitchen? (testing robot's identity) - What are your ros2 interfaces? (discovery of ros2 interfaces) @@ -93,11 +93,10 @@ TBD - Spin yourself left by 45 degrees (interaction with the robot using ros2 interfaces) - table with the robotic are shoud be visible in the camera - Use robotic arm to pick up a box from the table (identity and intefaces doesn't allow it) - Drive towards the table (when table is visible, testing ability to interpret camera image and perform actions based on the knowledge) - - Drive behind the table (likely will fail in current `rai` version due to limitations in spatial reasoning) ### Troubleshooting -_My robot doesn't have the identity._ +_My robot doesn't have an identity._ 1. Query `rai_whoami_node` for the identity manually @@ -105,7 +104,7 @@ _My robot doesn't have the identity._ ros2 service call /rai_whoami_identity_service std_srvs/srv/Trigger ``` -2. Make sure you have all required files were generated correctly in the `turtlebot4_whoami` package and have simillar sizes. +2. Make sure all required files were generated correctly in the `turtlebot4_whoami` package and have similar sizes to those listed below. ```bash ls -sh src/examples/turtlebot4_whoami/description/ From f95859a92c7340b60f2b8de89346398b4b3fe6c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 15 Oct 2024 19:31:10 +0200 Subject: [PATCH 12/58] readme updates --- src/examples/turtlebot4/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index 6c65c5ef..ae8df2be 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -14,7 +14,7 @@ TBD 1. Download turtlebot4 simulation binary matching your platform from : - Ubuntu 22.04 & ros2 humble: [link](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/EZLmGtPNgl9Kiu4royJJnVgB5tjS2Vze0myXDyVJtNcnRw?e=L42Z4z) - - Ubuntu 24.04 & ros2 jazzy: [link](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/Ebi0O3eWAtRAl3GL8LuEBqIB0-5fnHDCm7yOtcTucpNfzA?e=G6eJKP) + - Ubuntu 24.04 & ros2 jazzy: [link](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/ETkT-jvozlpBtuG1piDeqggBRmWl5eylIChc_g0v_EetpA?e=ina8Dt) > **NOTE** If you would like to make changes to the simulation and create your > own binary please follow this [README.md](https://github.com/RobotecAI/ROSCon2024Tutorial/README.md) @@ -35,7 +35,7 @@ TBD 1. Follow steps from [1. Setting up the workspace](https://github.com/RobotecAI/rai?tab=readme-ov-file#1-setting-up-the-workspace) 2. Then follow steps from [2. Build the project](https://github.com/RobotecAI/rai?tab=readme-ov-file#2-build-the-project) -3. Setup your [OpenAI API](https://openai.com/index/openai-api/). Make sure you have set `OPENAI_API_KEY` variable. +3. Setup your LLM vendor: [docs/vendors.md](../../../docs/vendors.md). OpenAI or AWS Bedrock are recommended for now. 4. Configure `rai_whoami_node` (based on ["Your robot identity in RAI"](https://github.com/RobotecAI/rai/blob/development/docs/create_robots_whoami.md) tutorial): From 0bc83e83cf68eada43edef352bc9d55739ccb47f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 15 Oct 2024 19:46:27 +0200 Subject: [PATCH 13/58] improve Troubleshooting --- src/examples/turtlebot4/README.md | 67 +++++++++++++++++++++++++++---- 1 file changed, 60 insertions(+), 7 deletions(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index ae8df2be..e8b36292 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -1,7 +1,7 @@ # Turtlebot4 tutorial -The following tutorial will be presented during [ROSCon 2024](https://roscon.ros.org/2024/) -talk about RAI. +The following tutorial will help you get started with [rai](https://github.com/RobotecAI/rai) +using Turtlebot4 simulation. ## Recording @@ -77,7 +77,8 @@ TBD ``` 6. Open you internet browser and go to `localhost:8501` -7. Open your internet browser with the address `localhost:8501` to can interact with your RAI agent through the chat. On the left you can communicate + +7. You can interact with your RAI agent through the chat. On the left you can communicate with the RAI HMI Agent. On the right you will see status of missions that were send to the RAI Node and are executed on your robot. @@ -130,8 +131,60 @@ _My robot doesn't have an identity._ _Robot doesn't move in the simulation._ -1. Make sure you can see ros2 topic when simulation binary is running +1. Make sure you can see ros2 topic simulation binary is running - ```bash - ros2 topic list - ``` + 1. Run the binary manually: + + ```bash + cd src/examples/turtlebot4/simulation/ + ./TurtleBot4DemoGamePackage/TurtleBot4Demo.GameLauncher -bg_ConnectToAssetProcessor=0 + ``` + + 2. Check ros2 topics: + + ```bash + ros2 topic list + ``` + + 3. Verify if Turtlebot simulation binary publishes such topics: + + ``` + /camera_color_info + /camera_image_color + /clock + /cmd_vel + /parameter_events + /rosout + /scan + /tf + ``` + +2. Make sure navigation stack actions work correctly: + + 1. Run binary as in the step above. + 2. Run navigation stack: + + ```bash + cd src/examples/turtlebot4 + ./run-nav.bash + ``` + + 3. Check available actions: + + ```bash + ros2 action list + ``` + + 4. Verify if such actions are available: + + ``` + /backup + /navigate_to_pose + /spin + ``` + + 5. Try to run one of them from command line: + + ```bash + ros2 action send_goal /spin nav2_msgs/action/Spin "{target_yaw: 3.14}" + ``` From 536c274376e3cc7978faa09a04dc7bfe4f1ec0be Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 15 Oct 2024 20:00:32 +0200 Subject: [PATCH 14/58] rearrange the order --- src/examples/turtlebot4/README.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index e8b36292..e8aae935 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -9,7 +9,12 @@ TBD ## Step by step instructions -1. O3DE simulation setup +1. Clone and install [rai](https://github.com/RobotecAI/rai) + + 1. Follow steps from [1. Setting up the workspace](https://github.com/RobotecAI/rai?tab=readme-ov-file#1-setting-up-the-workspace) + 2. Then follow steps from [2. Build the project](https://github.com/RobotecAI/rai?tab=readme-ov-file#2-build-the-project) + +2. O3DE simulation setup 1. Download turtlebot4 simulation binary matching your platform from : @@ -30,11 +35,6 @@ TBD unzip -d src/examples/turtlebot4/simulation Turtlebot4_noblejazzy_0.0.1.zip ``` -2. Clone and install [rai](https://github.com/RobotecAI/rai) - - 1. Follow steps from [1. Setting up the workspace](https://github.com/RobotecAI/rai?tab=readme-ov-file#1-setting-up-the-workspace) - 2. Then follow steps from [2. Build the project](https://github.com/RobotecAI/rai?tab=readme-ov-file#2-build-the-project) - 3. Setup your LLM vendor: [docs/vendors.md](../../../docs/vendors.md). OpenAI or AWS Bedrock are recommended for now. 4. Configure `rai_whoami_node` (based on ["Your robot identity in RAI"](https://github.com/RobotecAI/rai/blob/development/docs/create_robots_whoami.md) tutorial): From 93cd31413d4e56a228750f0ae4b7925e5ba6003d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 15 Oct 2024 20:16:27 +0200 Subject: [PATCH 15/58] add link to generated whoami --- src/examples/turtlebot4/README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index e8aae935..0eb89532 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -52,12 +52,15 @@ TBD 4. Create robot's identity. Run theย `parse_whoami_package`. This will process the documentation, building it into a vector database, which is used by RAI agent to reason about its identity. + > **NOTE**: generated files can be downloaded from [here](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/EbPZSEdXYaRGoeecu6oJg6QBsI4ZOe_mrU3uOtOflnIjQg?e=HX8ZHB) + > unzip them to `src/examples/turtlebot4_whoami/description/generated` > `unzip -d src/examples/turtlebot4_whoami/description turtlebot4_whoami_generated.zip` + > **NOTE**: Vector database is created using the OpenAI API. Parsing bigger documents > might lead to costs. Embedding model can be configured in > [config.toml](https://github.com/RobotecAI/rai/blob/development/config.toml#L13) ```bash - poetry run parse_whoami_package src/examples/turtlebot4_whoami/description + poetry run parse_whoami_package src/examples/turtlebot4_whoami # you will be asked to press `y` to continue ``` From 453ca053a631de3804d890f6173acdee9d9c6489 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 15 Oct 2024 20:17:20 +0200 Subject: [PATCH 16/58] fix typo --- src/examples/turtlebot4/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index 0eb89532..bd356a6c 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -53,7 +53,8 @@ TBD it into a vector database, which is used by RAI agent to reason about its identity. > **NOTE**: generated files can be downloaded from [here](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/EbPZSEdXYaRGoeecu6oJg6QBsI4ZOe_mrU3uOtOflnIjQg?e=HX8ZHB) - > unzip them to `src/examples/turtlebot4_whoami/description/generated` > `unzip -d src/examples/turtlebot4_whoami/description turtlebot4_whoami_generated.zip` + > unzip them to `src/examples/turtlebot4_whoami/description/generated` with a command: + > `unzip -d src/examples/turtlebot4_whoami/description turtlebot4_whoami_generated.zip` > **NOTE**: Vector database is created using the OpenAI API. Parsing bigger documents > might lead to costs. Embedding model can be configured in From 3439a0ca6215e277591ed849e1a89460fe6496b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 15 Oct 2024 20:19:46 +0200 Subject: [PATCH 17/58] add step to ensure whoami pkg is built --- src/examples/turtlebot4/README.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index bd356a6c..bb1cea12 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -66,10 +66,18 @@ TBD ``` 5. Build the workspace which now includes the new package + ```bash colcon build --symlink-install ``` + 6. Ensure `turtlebot4_whoami` package has been built: + + ```bash + . ./setup_shell.sh + ros2 pkg list | grep turtlebot4 + ``` + Congratulations! Your `rai_whoami_node` is configured. In the following steps your RAI agent will assume a turtlebot4 "personality". From 9a852503e9d49b8a677598fba7535addf2df8ab1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 15 Oct 2024 20:57:31 +0200 Subject: [PATCH 18/58] update link to binary --- src/examples/turtlebot4/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index bb1cea12..f3618bc4 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -19,7 +19,7 @@ TBD 1. Download turtlebot4 simulation binary matching your platform from : - Ubuntu 22.04 & ros2 humble: [link](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/EZLmGtPNgl9Kiu4royJJnVgB5tjS2Vze0myXDyVJtNcnRw?e=L42Z4z) - - Ubuntu 24.04 & ros2 jazzy: [link](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/ETkT-jvozlpBtuG1piDeqggBRmWl5eylIChc_g0v_EetpA?e=ina8Dt) + - Ubuntu 24.04 & ros2 jazzy: [link](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/ETkT-jvozlpBtuG1piDeqggBRmWl5eylIChc_g0v_EetpA?e=EjcJqe) > **NOTE** If you would like to make changes to the simulation and create your > own binary please follow this [README.md](https://github.com/RobotecAI/ROSCon2024Tutorial/README.md) From 43ea6febb186cfebdc3fbfe31ce1c640d7c801fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 15 Oct 2024 21:12:58 +0200 Subject: [PATCH 19/58] chore: add again turtlebot rai example script --- src/examples/turtlebot4/turtlebot_demo.py | 117 ++++++++++++++++++++++ 1 file changed, 117 insertions(+) create mode 100644 src/examples/turtlebot4/turtlebot_demo.py diff --git a/src/examples/turtlebot4/turtlebot_demo.py b/src/examples/turtlebot4/turtlebot_demo.py new file mode 100644 index 00000000..07040c9f --- /dev/null +++ b/src/examples/turtlebot4/turtlebot_demo.py @@ -0,0 +1,117 @@ +# Copyright (C) 2024 Robotec.AI +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + + +import rclpy +import rclpy.callback_groups +import rclpy.executors +import rclpy.qos +import rclpy.subscription +import rclpy.task +from langchain.tools.render import render_text_description_and_args +from langchain_openai import ChatOpenAI + +from rai.agents.state_based import create_state_based_agent +from rai.node import RaiNode +from rai.tools.ros.native import ( + GetCameraImage, + GetMsgFromTopic, + Ros2PubMessageTool, + Ros2ShowMsgInterfaceTool, +) +from rai.tools.ros.native_actions import Ros2RunActionSync +from rai.tools.ros.tools import GetCurrentPositionTool +from rai.tools.time import WaitForSecondsTool + + +def main(): + rclpy.init() + llm = ChatOpenAI(model="gpt-4o") + + # TODO(boczekbartek): refactor system prompt + + SYSTEM_PROMPT = "" + + ros2_whitelist = [ + "/cmd_vel", + "/rosout", + "/map", + "/odom", + "/camera_image_color", + "/backup", + "/drive_on_heading", + "/navigate_through_poses", + "/navigate_to_pose", + "/spin", + ] + + node = RaiNode( + llm=ChatOpenAI( + model="gpt-4o-mini" + ), # smaller model used to describe the environment + whitelist=ros2_whitelist, + system_prompt=SYSTEM_PROMPT, + ) + + tools = [ + WaitForSecondsTool(), + GetMsgFromTopic(node=node), + GetCameraImage(node=node), + Ros2ShowMsgInterfaceTool(), + Ros2PubMessageTool(node=node), + Ros2RunActionSync(node=node), + GetCurrentPositionTool(), + ] + + state_retriever = node.get_robot_state + + SYSTEM_PROMPT = f"""You are an autonomous robot connected to ros2 environment. Your main goal is to fulfill the user's requests. + Do not make assumptions about the environment you are currently in. + + Here are your tools: + {render_text_description_and_args(tools)} + + You can use ros2 topics, services and actions to operate. + + Navigation tips: + - for driving forward/backward, if specified, ros2 actions are better. + - for driving for some specific time or in specific manner (like in circle) it good to use /cmd_vel topic + - you are currently unable to read map or point-cloud, so please avoid subscribing to such topics. + - if you are asked to drive towards some object, it's good to: + 1. check the camera image and verify if objects can be seen + 2. if only driving forward is required, do it + 3. if obstacle avoidance might be required, use ros2 actions navigate_*, but first check your currect position, then very accurately estimate the goal pose. + """ + + node.get_logger().info(f"{SYSTEM_PROMPT=}") + + node.system_prompt = node.initialize_system_prompt(SYSTEM_PROMPT) + + app = create_state_based_agent( + llm=llm, + tools=tools, + state_retriever=state_retriever, + logger=node.get_logger(), + ) + + node.set_app(app) + + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(node) + executor.spin() + rclpy.shutdown() + + +main() From 1cf53c13851e7f9e66a99f22ef394f99c61978f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 15 Oct 2024 21:29:56 +0200 Subject: [PATCH 20/58] remove rviz from turtlebot example --- src/examples/turtlebot4/turtlebot.launch.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/examples/turtlebot4/turtlebot.launch.xml b/src/examples/turtlebot4/turtlebot.launch.xml index cc078609..65437bee 100644 --- a/src/examples/turtlebot4/turtlebot.launch.xml +++ b/src/examples/turtlebot4/turtlebot.launch.xml @@ -38,7 +38,5 @@ cwd="src/examples/turtlebot4" output="screen"> - - From 5c4e23afc1819e1150e184f9a2c80e3381dd9e43 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 15 Oct 2024 21:31:53 +0200 Subject: [PATCH 21/58] improve readme --- src/examples/turtlebot4/README.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index f3618bc4..65dfc9fb 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -68,15 +68,16 @@ TBD 5. Build the workspace which now includes the new package ```bash + deactivate # if poetry env is activated colcon build --symlink-install ``` 6. Ensure `turtlebot4_whoami` package has been built: - ```bash - . ./setup_shell.sh - ros2 pkg list | grep turtlebot4 - ``` + ```bash + . ./setup_shell.sh + ros2 pkg list | grep turtlebot4 + ``` Congratulations! Your `rai_whoami_node` is configured. In the following steps your RAI agent will assume a turtlebot4 "personality". @@ -99,10 +100,9 @@ TBD For example you can try such prompts: - - Are you able to bring you something from the kitchen? (testing robot's identity) + - Could you bring me something from the kitchen? (testing robot's identity) - What are your ros2 interfaces? (discovery of ros2 interfaces) - tell me what you see (interpretation of camera image) - - Drive towards the chair (when table is not visible, robot rejects task that it cannot do) - Spin yourself left by 45 degrees (interaction with the robot using ros2 interfaces) - table with the robotic are shoud be visible in the camera - Use robotic arm to pick up a box from the table (identity and intefaces doesn't allow it) - Drive towards the table (when table is visible, testing ability to interpret camera image and perform actions based on the knowledge) From 899b4ebafda987cdda1fab8767b9dbff0b245bbb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 15 Oct 2024 21:34:26 +0200 Subject: [PATCH 22/58] vendor agnosticy in Troubleshooting --- src/examples/turtlebot4/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index 65dfc9fb..91d77f0b 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -136,8 +136,8 @@ _My robot doesn't have an identity._ ... ``` - If files above are incorrect please check your `OPENAI_API_KEY` and rerun point 4 - of this tutorial. + If files above are incorrect please check your vendor configuration is as described + in [docs/vendors.md](../../../docs/vendors.md) and rerun point 4 of this tutorial. --- From bdb77edbe00f2f12f48988632e117216e6aafc0d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Thu, 17 Oct 2024 13:31:11 +0200 Subject: [PATCH 23/58] add minor improvement to readme --- src/examples/turtlebot4/README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index 91d77f0b..a748e6e5 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -89,6 +89,11 @@ TBD game_launcher:=./src/examples/turtlebot4/simulation/TurtleBot4DemoGamePackage/TurtleBot4Demo.GameLauncher ``` + You should be able to see a simulation scene. + + > **Tip**: Press 1, 2 or 3 on the keyboard when simulation window to change the + > camera. Use W,A,S,D to move the camera. + 6. Open you internet browser and go to `localhost:8501` 7. You can interact with your RAI agent through the chat. On the left you can communicate From d4faffce73e3facb3947ad868294a2fb02d97ef4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Thu, 17 Oct 2024 13:32:41 +0200 Subject: [PATCH 24/58] chore: remove unnecessary f-string --- src/examples/turtlebot4/turtlebot.launch.xml | 4 +- src/examples/turtlebot4/turtlebot_demo.py | 111 ++++++++----------- src/examples/turtlebot4/whitelist.txt | 10 ++ src/rai_hmi/rai_hmi/ros.py | 22 +--- src/rai_hmi/rai_hmi/text_hmi.py | 30 +++-- 5 files changed, 84 insertions(+), 93 deletions(-) create mode 100644 src/examples/turtlebot4/whitelist.txt diff --git a/src/examples/turtlebot4/turtlebot.launch.xml b/src/examples/turtlebot4/turtlebot.launch.xml index 65437bee..95f15332 100644 --- a/src/examples/turtlebot4/turtlebot.launch.xml +++ b/src/examples/turtlebot4/turtlebot.launch.xml @@ -22,11 +22,11 @@ Tuple[BaseHMINode, RaiBaseNode]: rclpy.init() @@ -34,22 +37,9 @@ def initialize_ros_nodes( queue=_feedbacks_queue, robot_description_package=robot_description_package, ) + whitelist = ros2_whitelist.read_text().splitlines() if ros2_whitelist else [] - # TODO(boczekbartek): this node shouldn't be required to initialize simple ros2 tools - ros2_whitelist = [ - "/cmd_vel", - "/rosout", - "/map", - "/odom", - "/camera_image_color", - "/backup", - "/drive_on_heading", - "/navigate_through_poses", - "/navigate_to_pose", - "/spin", - ] - - rai_node = RaiBaseNode(node_name="__rai_node__", whitelist=ros2_whitelist) + rai_node = RaiBaseNode(node_name="__rai_node__", whitelist=whitelist) executor = MultiThreadedExecutor() executor.add_node(hmi_node) diff --git a/src/rai_hmi/rai_hmi/text_hmi.py b/src/rai_hmi/rai_hmi/text_hmi.py index cecdfd79..1462ffe8 100644 --- a/src/rai_hmi/rai_hmi/text_hmi.py +++ b/src/rai_hmi/rai_hmi/text_hmi.py @@ -17,9 +17,10 @@ import logging import sys import time +from pathlib import Path from pprint import pformat from queue import Queue -from typing import Dict, List, Optional, cast +from typing import Dict, List, Optional, Tuple, cast import streamlit as st from langchain_core.messages import ( @@ -57,9 +58,10 @@ # ---------- Cached Resources ---------- @st.cache_resource -def parse_args(): +def parse_args() -> Tuple[Optional[str], Optional[Path]]: robot_description_package = sys.argv[1] if len(sys.argv) > 1 else None - return robot_description_package + whitelist = Path(sys.argv[2]) if len(sys.argv) > 2 else None + return robot_description_package, whitelist @st.cache_resource @@ -81,9 +83,13 @@ def initialize_mission_queue() -> Queue: @st.cache_resource def initialize_ros_nodes_streamlit( - _feedbacks_queue: Queue, robot_description_package: Optional[str] + _feedbacks_queue: Queue, + robot_description_package: Optional[str], + ros2_whitelist: Optional[Path], ): - return initialize_ros_nodes(_feedbacks_queue, robot_description_package) + return initialize_ros_nodes( + _feedbacks_queue, robot_description_package, ros2_whitelist + ) def display_agent_message( @@ -266,7 +272,7 @@ def stream(self): class StreamlitApp: - def __init__(self, robot_description_package) -> None: + def __init__(self, robot_description_package, ros2_whitelist: Path) -> None: self.logger = logging.getLogger(self.__class__.__name__) self.robot_description_package = robot_description_package @@ -277,7 +283,7 @@ def __init__(self, robot_description_package) -> None: self.mission_queue = initialize_mission_queue() self.hmi_ros_node, self.rai_ros_node = self.initialize_node( - self.mission_queue, robot_description_package + self.mission_queue, robot_description_package, ros2_whitelist ) self.agent = Agent(self.hmi_ros_node, self.rai_ros_node, self.memory) @@ -308,11 +314,13 @@ def get_system_status(self) -> SystemStatus: system_prompt=self.hmi_ros_node.system_prompt != "", ) - def initialize_node(self, feedbacks_queue, robot_description_package): + def initialize_node( + self, feedbacks_queue, robot_description_package, ros2_whitelist + ): self.logger.info("Initializing ROS 2 node...") with st.spinner("Initializing ROS 2 nodes..."): hmi_node, rai_node = initialize_ros_nodes_streamlit( - feedbacks_queue, robot_description_package + feedbacks_queue, robot_description_package, ros2_whitelist ) self.logger.info("ROS 2 node initialized") return hmi_node, rai_node @@ -368,6 +376,6 @@ def decode_base64_into_image(base64_image: str): if __name__ == "__main__": - robot_description_package = parse_args() - app = StreamlitApp(robot_description_package) + robot_description_package, ros2_whitelist = parse_args() + app = StreamlitApp(robot_description_package, ros2_whitelist) app.run() From f70e2e3392c3bf93fd646155152bb60c3324f4eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Thu, 17 Oct 2024 14:10:46 +0200 Subject: [PATCH 25/58] add link to tutorial video recording --- src/examples/turtlebot4/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index a748e6e5..a2cdc5a6 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -5,7 +5,7 @@ using Turtlebot4 simulation. ## Recording -TBD +[link](https://robotecai-my.sharepoint.com/:v:/g/personal/bartlomiej_boczek_robotec_ai/EfPTiZscCTROtmtoyHv_ykIBFKN5qh1pecxfLmLI6I4QeA?nav=eyJyZWZlcnJhbEluZm8iOnsicmVmZXJyYWxBcHAiOiJPbmVEcml2ZUZvckJ1c2luZXNzIiwicmVmZXJyYWxBcHBQbGF0Zm9ybSI6IldlYiIsInJlZmVycmFsTW9kZSI6InZpZXciLCJyZWZlcnJhbFZpZXciOiJNeUZpbGVzTGlua0NvcHkifX0&e=0QJnGk) ## Step by step instructions From e3e38b5608de8f5276d1abc3088db04d56920edf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Thu, 17 Oct 2024 15:25:10 +0200 Subject: [PATCH 26/58] fix: turtlebot demo doesn't use gdnio --- src/examples/turtlebot4/turtlebot_demo.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/examples/turtlebot4/turtlebot_demo.py b/src/examples/turtlebot4/turtlebot_demo.py index a4d19500..3cacadd7 100644 --- a/src/examples/turtlebot4/turtlebot_demo.py +++ b/src/examples/turtlebot4/turtlebot_demo.py @@ -24,7 +24,6 @@ import rclpy.qos import rclpy.subscription import rclpy.task -from rai_open_set_vision.tools import GetDetectionTool from rai.node import RaiStateBasedLlmNode from rai.tools.ros.native import ( @@ -88,7 +87,6 @@ def main(whitelist: Optional[Path] = None): GetMsgFromTopic, GetCameraImage, GetTransformTool, - GetDetectionTool, ], ) node.spin() From 360fc9fb92f32bb694e56463a856926d1b3d1052 Mon Sep 17 00:00:00 2001 From: Maciej Majek Date: Thu, 17 Oct 2024 20:03:01 +0200 Subject: [PATCH 27/58] chore: reduce verbosity feat: enhance typing --- src/rai/rai/node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rai/rai/node.py b/src/rai/rai/node.py index 15d43943..78fdb857 100644 --- a/src/rai/rai/node.py +++ b/src/rai/rai/node.py @@ -505,7 +505,7 @@ async def agent_loop(self, goal_handle: ServerGoalHandle): else: raise ValueError(f"Unexpected type of message: {type(msg)}") - # TODO: Find a better way to create meaninful feedback + # TODO(boczekbartek): Find a better way to create meaninful feedback last_msg = self.simple_llm.invoke( [ SystemMessage( From f9d13dc6cb9585dda1241573dbc457dd7c3f0e99 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Fri, 18 Oct 2024 09:27:12 +0200 Subject: [PATCH 28/58] [system_prompt] Add transform checking before and after nav2 actions to --- examples/rosbot-xl-generic-node-demo.py | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/rosbot-xl-generic-node-demo.py b/examples/rosbot-xl-generic-node-demo.py index 025e3083..1d3dcdc3 100644 --- a/examples/rosbot-xl-generic-node-demo.py +++ b/examples/rosbot-xl-generic-node-demo.py @@ -81,6 +81,7 @@ def main(): use /cmd_vel topic very carefully. Obstacle detection works only with nav2 stack, so be careful when it is not used. > be patient with running ros2 actions. usually the take some time to run. + Always check your transform before and after you perform ros2 actions, so that you can verify if it worked. Navigation tips: - it's good to start finding objects by rotating, then navigating to some diverse location with occasional rotations. Remember to frequency detect objects. From 8fd7cd9a81fae821bc18361479d180268d673aff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Fri, 18 Oct 2024 10:13:54 +0200 Subject: [PATCH 29/58] feat(`turtlebot4`): improve system prompt --- src/examples/turtlebot4/turtlebot_demo.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/examples/turtlebot4/turtlebot_demo.py b/src/examples/turtlebot4/turtlebot_demo.py index 3cacadd7..4c6ccabc 100644 --- a/src/examples/turtlebot4/turtlebot_demo.py +++ b/src/examples/turtlebot4/turtlebot_demo.py @@ -59,6 +59,7 @@ def main(whitelist: Optional[Path] = None): Avoid canceling ros2 actions if they don't cause a big danger Navigation tips: + - Always check your transform before and after you perform ros2 actions, so that you can verify if it worked. - for driving forward/backward, if specified, ros2 actions are better. - for driving for some specific time or in specific manner (like in circle) it good to use /cmd_vel topic - you are currently unable to read map or point-cloud, so please avoid subscribing to such topics. From 0ab080f0556571ee90a6f4666116922cf1081cd7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Fri, 18 Oct 2024 10:14:30 +0200 Subject: [PATCH 30/58] improve `RaiNode`'s summarizer --- src/rai/rai/node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rai/rai/node.py b/src/rai/rai/node.py index 78fdb857..9374d999 100644 --- a/src/rai/rai/node.py +++ b/src/rai/rai/node.py @@ -512,7 +512,7 @@ async def agent_loop(self, goal_handle: ServerGoalHandle): content=( "You are an experienced reporter deployed on a autonomous robot. " # type: ignore "Your task is to summarize the message in a way that is easy for other agents to understand. " - "Do not use markdown formatting. Keep it short and concise. If the message is empty, please return empty string." + "Do not use markdown formatting. Keep it short and concise. If the message is empty, please return empty string ('')." ) ), HumanMessage(content=last_msg), From 94ffea0d7a4fb52ce2d27d48e66b45277f2b92b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Fri, 18 Oct 2024 10:28:00 +0200 Subject: [PATCH 31/58] crear logs buffer after reading --- src/rai/rai/utils/ros.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/rai/rai/utils/ros.py b/src/rai/rai/utils/ros.py index c281de74..d5bb4d1d 100644 --- a/src/rai/rai/utils/ros.py +++ b/src/rai/rai/utils/ros.py @@ -52,6 +52,7 @@ def summarize(self): if len(self._buffer) == 0: return "No logs" buffer = self.get_raw_logs() + self.clear() response = self.llm.invoke({"rosout": buffer}) return str(response.content) From 5e88122e2e0f6c314fcfa9e6bad3b63930994b8e Mon Sep 17 00:00:00 2001 From: Bartek Boczek <22739059+boczekbartek@users.noreply.github.com> Date: Fri, 18 Oct 2024 18:30:16 +0200 Subject: [PATCH 32/58] Update src/examples/turtlebot4/README.md MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Paweล‚ Liberadzki --- src/examples/turtlebot4/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index a2cdc5a6..dc10b317 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -28,7 +28,7 @@ using Turtlebot4 simulation. ```bash cd rai - # for Ubuntu 22.04 jammy and ros2 humlbe + # for Ubuntu 22.04 jammy and ros2 humble unzip -d src/examples/turtlebot4/simulation Turtlebot4_jammyhumble_0.0.1.zip # for Ubuntu 24.04 noble and ros2 jazzy From 66822a3a7a8931dcd1fa18d565187988c6ee988d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Mon, 21 Oct 2024 01:31:34 +0200 Subject: [PATCH 33/58] feat(`vendors`): allow vendor overwrite for rai_whoami and use local model in turtlebot example --- config.toml | 8 ++++---- src/examples/turtlebot4/README.md | 15 +++++++++------ src/rai/rai/cli/rai_cli.py | 11 +++++++++-- src/rai/rai/utils/model_initialization.py | 13 +++++++++---- 4 files changed, 31 insertions(+), 16 deletions(-) diff --git a/config.toml b/config.toml index 5dd45417..c5331517 100644 --- a/config.toml +++ b/config.toml @@ -13,17 +13,17 @@ complex_model = "gpt-4o-2024-08-06" embeddings_model = "text-embedding-ada-002" [ollama] -simple_model = "llama3.1" +simple_model = "llama3.2" complex_model = "llama3.1:70b" -embeddings_model = "llama3.1" +embeddings_model = "llama3.2" base_url = "http://localhost:11434" [tracing] project = "rai" [tracing.langfuse] -use_langfuse = false -host = "https://cloud.langfuse.com" +use_langfuse = true +host = "http://localhost:3000" [tracing.langsmith] use_langsmith = false diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index dc10b317..bbd130a2 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -35,15 +35,14 @@ using Turtlebot4 simulation. unzip -d src/examples/turtlebot4/simulation Turtlebot4_noblejazzy_0.0.1.zip ``` -3. Setup your LLM vendor: [docs/vendors.md](../../../docs/vendors.md). OpenAI or AWS Bedrock are recommended for now. +3. Setup your LLM vendor: [docs/vendors.md](../../../docs/vendors.md). OpenAI or AWS Bedrock are recommended for now, since current local `ollama` models don't support vision & tool calling. 4. Configure `rai_whoami_node` (based on ["Your robot identity in RAI"](https://github.com/RobotecAI/rai/blob/development/docs/create_robots_whoami.md) tutorial): 1. Create `whoami` package for turtlebot4 in `src/examples/turtlebot4` ```bash - . ./setup_shell.sh - poetry run create_rai_ws --name turtlebot4 --destination-directory src/examples + ./scripts/create_rai_ws.sh --name turtlebot4 --destination-directory src/examples ``` 2. Download official turtlebot4 [data sheet](https://bit.ly/3KCp3Du) into @@ -56,12 +55,16 @@ using Turtlebot4 simulation. > unzip them to `src/examples/turtlebot4_whoami/description/generated` with a command: > `unzip -d src/examples/turtlebot4_whoami/description turtlebot4_whoami_generated.zip` - > **NOTE**: Vector database is created using the OpenAI API. Parsing bigger documents - > might lead to costs. Embedding model can be configured in + > **NOTE**: Vector database is can be created using local models like `llama3:2` + > as well as using the OpenAI API. With OpenAI Parsing bigger documents might lead + > to costs. Embedding model can be configured in > [config.toml](https://github.com/RobotecAI/rai/blob/development/config.toml#L13) + > + > You can also overwrite the vendor from config.toml with a `--vendor` argument like + > below: ```bash - poetry run parse_whoami_package src/examples/turtlebot4_whoami + ./scripts/parse_whoami_package.sh src/examples/turtlebot4_whoami --vendor ollama # you will be asked to press `y` to continue ``` diff --git a/src/rai/rai/cli/rai_cli.py b/src/rai/rai/cli/rai_cli.py index c7e00623..db388604 100644 --- a/src/rai/rai/cli/rai_cli.py +++ b/src/rai/rai/cli/rai_cli.py @@ -40,14 +40,21 @@ def parse_whoami_package(): parser.add_argument( "whoami_package_root", type=str, help="Path to the root of the whoami package" ) + parser.add_argument( + "--vendor", + type=str, + required=False, + default=None, + help="LLM and embeddings vendor. See config.toml for details", + ) args = parser.parse_args() save_dir = Path(args.whoami_package_root) / "description" / "generated" description_path = Path(args.whoami_package_root) / "description" save_dir.mkdir(parents=True, exist_ok=True) - llm = get_llm_model(model_type="simple_model") - embeddings_model = get_embeddings_model() + llm = get_llm_model(model_type="simple_model", vendor=args.vendor) + embeddings_model = get_embeddings_model(vendor=args.vendor) def calculate_urdf_tokens(): combined_urdf = "" diff --git a/src/rai/rai/utils/model_initialization.py b/src/rai/rai/utils/model_initialization.py index 852a6ec5..7d538fa7 100644 --- a/src/rai/rai/utils/model_initialization.py +++ b/src/rai/rai/utils/model_initialization.py @@ -85,9 +85,12 @@ def load_config() -> RAIConfig: ) -def get_llm_model(model_type: Literal["simple_model", "complex_model"]): +def get_llm_model( + model_type: Literal["simple_model", "complex_model"], vendor: str = None +): config = load_config() - vendor = config.vendor.name + if vendor is None: + vendor = config.vendor.name model_config = getattr(config, vendor) if vendor == "openai": @@ -111,9 +114,11 @@ def get_llm_model(model_type: Literal["simple_model", "complex_model"]): raise ValueError(f"Unknown LLM vendor: {vendor}") -def get_embeddings_model(): +def get_embeddings_model(vendor: str = None): config = load_config() - vendor = config.vendor.name + if vendor is None: + vendor = config.vendor.name + model_config = getattr(config, vendor) if vendor == "openai": From 4b190f10865f763ce5516a77bded2b5f35265f1f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Mon, 21 Oct 2024 01:34:57 +0200 Subject: [PATCH 34/58] chore(`whoami`): change poetry scripts to shell scripts --- docs/create_robots_whoami.md | 7 ++++--- pyproject.toml | 4 ---- scripts/create_rai_ws.sh | 2 ++ scripts/parse_whoami_package.sh | 2 ++ 4 files changed, 8 insertions(+), 7 deletions(-) create mode 100755 scripts/create_rai_ws.sh create mode 100755 scripts/parse_whoami_package.sh diff --git a/docs/create_robots_whoami.md b/docs/create_robots_whoami.md index c55d573b..cbdb47d4 100644 --- a/docs/create_robots_whoami.md +++ b/docs/create_robots_whoami.md @@ -16,7 +16,7 @@ Your robot's `whoami` package serves as a configuration package for the `rai_who 2. Create a whoami package for Panda ```shell - poetry run create_rai_ws --name panda --destination-directory src/examples + ./scripts/create_rai_ws.sh --name panda --destination-directory src/examples ``` 3. Fill in the `src/examples/panda_whoami/description` folder with data: @@ -30,11 +30,12 @@ Your robot's `whoami` package serves as a configuration package for the `rai_who 4. Run the `parse_whoami_package`. This will process the documentation, building it into a vector database, which is used by RAI agent to reason about its identity. > [!IMPORTANT] -> Parsing bigger documents might lead to costs. Embedding model can be configured in +> Parsing bigger documents with Cloud vendors might lead to costs. Consider using the +> local `ollama` provider for this task. Embedding model can be configured in > [config.toml](../config.toml) (`ollama` works locally, see [docs/vendors.md](./vendors.md#ollama)). ```shell -poetry run parse_whoami_package src/examples/panda_whoami +./scripts/parse_whoami_package.sh src/examples/panda_whoami --vendor ollama ``` 5. Optional: Examine the generated files diff --git a/pyproject.toml b/pyproject.toml index c922074f..a3128dcc 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -86,7 +86,3 @@ markers = [ "billable: marks test as billable (deselect with '-m \"not billable\"')", ] addopts = "-m 'not billable' --ignore=src" - -[tool.poetry.scripts] -create_rai_ws = "rai.cli.rai_cli:create_rai_ws" -parse_whoami_package = "rai.cli.rai_cli:parse_whoami_package" diff --git a/scripts/create_rai_ws.sh b/scripts/create_rai_ws.sh new file mode 100755 index 00000000..3641caf7 --- /dev/null +++ b/scripts/create_rai_ws.sh @@ -0,0 +1,2 @@ +#!/usr/bin/env bash +python -c "from rai.cli.rai_cli import create_rai_ws; create_rai_ws()" "$@" diff --git a/scripts/parse_whoami_package.sh b/scripts/parse_whoami_package.sh new file mode 100755 index 00000000..81cbe4d0 --- /dev/null +++ b/scripts/parse_whoami_package.sh @@ -0,0 +1,2 @@ +#!/usr/bin/env bash +python -c "from rai.cli.rai_cli import parse_whoami_package; parse_whoami_package()" "$@" From 045806f4b88da1dbb5a5ca6a35fb1fdc0093b342 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Mon, 21 Oct 2024 02:00:02 +0200 Subject: [PATCH 35/58] models` conguraion - more flexiblity --- config.toml | 4 +++- src/examples/turtlebot4/README.md | 18 +++++++------- src/rai/rai/utils/model_initialization.py | 29 ++++++++++++++++------- 3 files changed, 32 insertions(+), 19 deletions(-) diff --git a/config.toml b/config.toml index c5331517..0acc0c9d 100644 --- a/config.toml +++ b/config.toml @@ -1,5 +1,7 @@ [vendor] -name = "openai" # openai, aws, ollama +simple_model = "openai" # openai, aws, ollama +embeddings_model = "ollama" +complex_model = "openai" [aws] simple_model = "anthropic.claude-3-haiku-20240307-v1:0" diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index bbd130a2..19311c06 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -35,7 +35,10 @@ using Turtlebot4 simulation. unzip -d src/examples/turtlebot4/simulation Turtlebot4_noblejazzy_0.0.1.zip ``` -3. Setup your LLM vendor: [docs/vendors.md](../../../docs/vendors.md). OpenAI or AWS Bedrock are recommended for now, since current local `ollama` models don't support vision & tool calling. +3. Setup your LLM vendor: [docs/vendors.md](../../../docs/vendors.md). OpenAI or + AWS Bedrock are recommended for models, since current local `ollama` models don't + support vision & tool calling. + For `embeddings_model` `ollama` vendor works well. 4. Configure `rai_whoami_node` (based on ["Your robot identity in RAI"](https://github.com/RobotecAI/rai/blob/development/docs/create_robots_whoami.md) tutorial): @@ -51,20 +54,15 @@ using Turtlebot4 simulation. 4. Create robot's identity. Run theย `parse_whoami_package`. This will process the documentation, building it into a vector database, which is used by RAI agent to reason about its identity. + > **NOTE**: Be cautious if Cloud vendor was chosen step 3 as `embeddings_model`, + > because parsing bigger documents might lead to costs. + > **NOTE**: generated files can be downloaded from [here](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/EbPZSEdXYaRGoeecu6oJg6QBsI4ZOe_mrU3uOtOflnIjQg?e=HX8ZHB) > unzip them to `src/examples/turtlebot4_whoami/description/generated` with a command: > `unzip -d src/examples/turtlebot4_whoami/description turtlebot4_whoami_generated.zip` - > **NOTE**: Vector database is can be created using local models like `llama3:2` - > as well as using the OpenAI API. With OpenAI Parsing bigger documents might lead - > to costs. Embedding model can be configured in - > [config.toml](https://github.com/RobotecAI/rai/blob/development/config.toml#L13) - > - > You can also overwrite the vendor from config.toml with a `--vendor` argument like - > below: - ```bash - ./scripts/parse_whoami_package.sh src/examples/turtlebot4_whoami --vendor ollama + ./scripts/parse_whoami_package.sh src/examples/turtlebot4_whoami # you will be asked to press `y` to continue ``` diff --git a/src/rai/rai/utils/model_initialization.py b/src/rai/rai/utils/model_initialization.py index 7d538fa7..aa4ec878 100644 --- a/src/rai/rai/utils/model_initialization.py +++ b/src/rai/rai/utils/model_initialization.py @@ -12,17 +12,25 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging import os from dataclasses import dataclass from typing import List, Literal +import coloredlogs import tomli from langchain_core.callbacks.base import BaseCallbackHandler +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) +coloredlogs.install(level="INFO") # type: ignore + @dataclass class VendorConfig: - name: str + simple_model: str + complex_model: str + embeddings_model: str @dataclass @@ -90,26 +98,30 @@ def get_llm_model( ): config = load_config() if vendor is None: - vendor = config.vendor.name + if model_type == "simple_model": + vendor = config.vendor.simple_model + else: + vendor = config.vendor.complex_model + model_config = getattr(config, vendor) + model = getattr(model_config, model_type) + logger.info(f"Using LLM model: {vendor}-{model}") if vendor == "openai": from langchain_openai import ChatOpenAI - return ChatOpenAI(model=getattr(model_config, model_type)) + return ChatOpenAI(model=model) elif vendor == "aws": from langchain_aws import ChatBedrock return ChatBedrock( - model_id=getattr(model_config, model_type), + model_id=model, region_name=model_config.region_name, ) elif vendor == "ollama": from langchain_ollama import ChatOllama - return ChatOllama( - model=getattr(model_config, model_type), base_url=model_config.base_url - ) + return ChatOllama(model=model, base_url=model_config.base_url) else: raise ValueError(f"Unknown LLM vendor: {vendor}") @@ -117,10 +129,11 @@ def get_llm_model( def get_embeddings_model(vendor: str = None): config = load_config() if vendor is None: - vendor = config.vendor.name + vendor = config.vendor.embeddings_model model_config = getattr(config, vendor) + logger.info(f"Using embeddings model: {vendor}-{model_config.embeddings_model}") if vendor == "openai": from langchain_openai import OpenAIEmbeddings From 9ac945e8b2211c571a105c0a296fe8b1ecda1500 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Mon, 21 Oct 2024 02:00:58 +0200 Subject: [PATCH 36/58] rename whitelist to allowlist --- src/examples/turtlebot4/{whitelist.txt => allowlist.txt} | 0 src/examples/turtlebot4/turtlebot.launch.xml | 4 ++-- src/examples/turtlebot4/turtlebot_demo.py | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) rename src/examples/turtlebot4/{whitelist.txt => allowlist.txt} (100%) diff --git a/src/examples/turtlebot4/whitelist.txt b/src/examples/turtlebot4/allowlist.txt similarity index 100% rename from src/examples/turtlebot4/whitelist.txt rename to src/examples/turtlebot4/allowlist.txt diff --git a/src/examples/turtlebot4/turtlebot.launch.xml b/src/examples/turtlebot4/turtlebot.launch.xml index 95f15332..2fd4ab3b 100644 --- a/src/examples/turtlebot4/turtlebot.launch.xml +++ b/src/examples/turtlebot4/turtlebot.launch.xml @@ -22,11 +22,11 @@ Date: Mon, 21 Oct 2024 02:38:07 +0200 Subject: [PATCH 37/58] improve hmi system prompt --- src/rai_hmi/rai_hmi/base.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/rai_hmi/rai_hmi/base.py b/src/rai_hmi/rai_hmi/base.py index 41c82442..a6a69a5a 100644 --- a/src/rai_hmi/rai_hmi/base.py +++ b/src/rai_hmi/rai_hmi/base.py @@ -77,6 +77,7 @@ class BaseHMINode(Node): add as 1 task. They will be done by another agent resposible for communication with the robotic's stack. + You are the embodied AI robot, so please describe yourself and you action in 1st person. """ def __init__( From 87fe75a41ae21fc4ca9bd2bf8382e06d02440235 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Mon, 21 Oct 2024 02:38:26 +0200 Subject: [PATCH 38/58] fix: nav2 humble for turtlebot --- src/examples/turtlebot4/run-nav.bash | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/examples/turtlebot4/run-nav.bash b/src/examples/turtlebot4/run-nav.bash index c2b4409c..814d42d7 100755 --- a/src/examples/turtlebot4/run-nav.bash +++ b/src/examples/turtlebot4/run-nav.bash @@ -1,6 +1,14 @@ #!/bin/bash -ros2 launch nav2_bringup bringup_launch.py \ - slam:="${SLAM:-True}" \ - params_file:=./navigation_params_"${ROS_DISTRO}".yaml \ - use_sim_time:=True +if [ "$ROS_DISTRO" = "jazzy" ]; then + ros2 launch nav2_bringup bringup_launch.py \ + slam:="${SLAM:-True}" \ + params_file:=./navigation_params_"${ROS_DISTRO}".yaml \ + use_sim_time:=True +else + ros2 launch nav2_bringup bringup_launch.py \ + slam:="${SLAM:-True}" \ + params_file:=./navigation_params_"${ROS_DISTRO}".yaml \ + map:=./Examples/navigation/maps/map.yaml \ + use_sim_time:=True +fi From d3559a4d0a43c7961f96f0b20c96f685bb21ccd3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Mon, 21 Oct 2024 02:41:43 +0200 Subject: [PATCH 39/58] improve turtlebot readme w/ rag examples and nav2 install --- src/examples/turtlebot4/README.md | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index 19311c06..1735d848 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -9,6 +9,19 @@ using Turtlebot4 simulation. ## Step by step instructions +0. Install nav2 packates + + ```shell + sudo apt install "ros-${ROS_DISTRO}-nav2-*" + + # add to ~/.bashrc + . /opt/ros/humble/setup.bash + # or + . ./opt/ros/jazzy/setup.bash + export RMW_IMPLEMENTATION=rmw_fastrtps_cpp + + ``` + 1. Clone and install [rai](https://github.com/RobotecAI/rai) 1. Follow steps from [1. Setting up the workspace](https://github.com/RobotecAI/rai?tab=readme-ov-file#1-setting-up-the-workspace) @@ -106,12 +119,14 @@ using Turtlebot4 simulation. For example you can try such prompts: - - Could you bring me something from the kitchen? (testing robot's identity) - - What are your ros2 interfaces? (discovery of ros2 interfaces) + - who are you? (testing robot's identity) + - what is the voltage of the batery? (use RAG: query vector database for the answer) + - could you bring me something from the kitchen? (testing robot's identity) + - what are your ros2 interfaces? (discovery of ros2 interfaces) - tell me what you see (interpretation of camera image) - - Spin yourself left by 45 degrees (interaction with the robot using ros2 interfaces) - table with the robotic are shoud be visible in the camera - - Use robotic arm to pick up a box from the table (identity and intefaces doesn't allow it) - - Drive towards the table (when table is visible, testing ability to interpret camera image and perform actions based on the knowledge) + - spin yourself left by 45 degrees (interaction with the robot using ros2 interfaces) - table with the robotic are shoud be visible in the camera + - use robotic arm to pick up a box from the table (identity and intefaces doesn't allow it) + - drive towards the table (when table is visible, testing ability to interpret camera image and perform actions based on the knowledge) ### Troubleshooting From 1ec0050051bd3d9e836265db972d600252b67e4e Mon Sep 17 00:00:00 2001 From: Bartek Boczek <22739059+boczekbartek@users.noreply.github.com> Date: Mon, 21 Oct 2024 10:42:12 +0200 Subject: [PATCH 40/58] revert langfuse in config.toml Co-authored-by: Maciej Majek <46171033+maciejmajek@users.noreply.github.com> --- config.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config.toml b/config.toml index 0acc0c9d..62830555 100644 --- a/config.toml +++ b/config.toml @@ -24,8 +24,8 @@ base_url = "http://localhost:11434" project = "rai" [tracing.langfuse] -use_langfuse = true -host = "http://localhost:3000" +use_langfuse = false +host = "https://cloud.langfuse.com" [tracing.langsmith] use_langsmith = false From 3bf785669d351a1c5a47f2d6b4688f4e9adb6232 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Mon, 21 Oct 2024 10:50:21 +0200 Subject: [PATCH 41/58] revert vendor overwrite --- docs/create_robots_whoami.md | 2 +- src/rai/rai/cli/rai_cli.py | 11 ++--------- 2 files changed, 3 insertions(+), 10 deletions(-) diff --git a/docs/create_robots_whoami.md b/docs/create_robots_whoami.md index cbdb47d4..80e00568 100644 --- a/docs/create_robots_whoami.md +++ b/docs/create_robots_whoami.md @@ -35,7 +35,7 @@ Your robot's `whoami` package serves as a configuration package for the `rai_who > [config.toml](../config.toml) (`ollama` works locally, see [docs/vendors.md](./vendors.md#ollama)). ```shell -./scripts/parse_whoami_package.sh src/examples/panda_whoami --vendor ollama +./scripts/parse_whoami_package.sh src/examples/panda_whoami ``` 5. Optional: Examine the generated files diff --git a/src/rai/rai/cli/rai_cli.py b/src/rai/rai/cli/rai_cli.py index db388604..c7e00623 100644 --- a/src/rai/rai/cli/rai_cli.py +++ b/src/rai/rai/cli/rai_cli.py @@ -40,21 +40,14 @@ def parse_whoami_package(): parser.add_argument( "whoami_package_root", type=str, help="Path to the root of the whoami package" ) - parser.add_argument( - "--vendor", - type=str, - required=False, - default=None, - help="LLM and embeddings vendor. See config.toml for details", - ) args = parser.parse_args() save_dir = Path(args.whoami_package_root) / "description" / "generated" description_path = Path(args.whoami_package_root) / "description" save_dir.mkdir(parents=True, exist_ok=True) - llm = get_llm_model(model_type="simple_model", vendor=args.vendor) - embeddings_model = get_embeddings_model(vendor=args.vendor) + llm = get_llm_model(model_type="simple_model") + embeddings_model = get_embeddings_model() def calculate_urdf_tokens(): combined_urdf = "" From 865f2155f31b35cf9874f2d2199302986d499590 Mon Sep 17 00:00:00 2001 From: Bartek Boczek <22739059+boczekbartek@users.noreply.github.com> Date: Mon, 21 Oct 2024 11:02:04 +0200 Subject: [PATCH 42/58] Update src/examples/turtlebot4/README.md Co-authored-by: Maciej Majek <46171033+maciejmajek@users.noreply.github.com> --- src/examples/turtlebot4/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index 1735d848..489e4197 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -34,8 +34,8 @@ using Turtlebot4 simulation. - Ubuntu 22.04 & ros2 humble: [link](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/EZLmGtPNgl9Kiu4royJJnVgB5tjS2Vze0myXDyVJtNcnRw?e=L42Z4z) - Ubuntu 24.04 & ros2 jazzy: [link](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/ETkT-jvozlpBtuG1piDeqggBRmWl5eylIChc_g0v_EetpA?e=EjcJqe) - > **NOTE** If you would like to make changes to the simulation and create your - > own binary please follow this [README.md](https://github.com/RobotecAI/ROSCon2024Tutorial/README.md) + > [!NOTE] + > If you would like to make changes to the simulation and create your own binary please follow this [README.md](https://github.com/RobotecAI/ROSCon2024Tutorial/README.md) 2. Unzip it: From 74a186a05d623234fdda144018467c6b472a44ee Mon Sep 17 00:00:00 2001 From: Bartek Boczek <22739059+boczekbartek@users.noreply.github.com> Date: Mon, 21 Oct 2024 11:03:44 +0200 Subject: [PATCH 43/58] README: Apply suggestions from code review Co-authored-by: Maciej Majek <46171033+maciejmajek@users.noreply.github.com> --- src/examples/turtlebot4/README.md | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/examples/turtlebot4/README.md b/src/examples/turtlebot4/README.md index 489e4197..7704d5b2 100644 --- a/src/examples/turtlebot4/README.md +++ b/src/examples/turtlebot4/README.md @@ -54,7 +54,9 @@ using Turtlebot4 simulation. For `embeddings_model` `ollama` vendor works well. 4. Configure `rai_whoami_node` (based on ["Your robot identity in RAI"](https://github.com/RobotecAI/rai/blob/development/docs/create_robots_whoami.md) tutorial): - +> [!TIP] +> Generated files can be downloaded from [here](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/EbPZSEdXYaRGoeecu6oJg6QBsI4ZOe_mrU3uOtOflnIjQg?e=HX8ZHB) unzip them to `src/examples/turtlebot4_whoami/description/generated` with a command: +> `unzip -d src/examples/turtlebot4_whoami/description turtlebot4_whoami_generated.zip` 1. Create `whoami` package for turtlebot4 in `src/examples/turtlebot4` ```bash @@ -66,14 +68,6 @@ using Turtlebot4 simulation. 3. Download [image](https://s3.amazonaws.com/assets.clearpathrobotics.com/wp-content/uploads/2022/03/16113604/Turtlebot-4-20220207.44.png) of turtlebot4 into `src/examples/turtlebot4_whoami/description/images` 4. Create robot's identity. Run theย `parse_whoami_package`. This will process the documentation, building it into a vector database, which is used by RAI agent to reason about its identity. - - > **NOTE**: Be cautious if Cloud vendor was chosen step 3 as `embeddings_model`, - > because parsing bigger documents might lead to costs. - - > **NOTE**: generated files can be downloaded from [here](https://robotecai-my.sharepoint.com/:u:/g/personal/bartlomiej_boczek_robotec_ai/EbPZSEdXYaRGoeecu6oJg6QBsI4ZOe_mrU3uOtOflnIjQg?e=HX8ZHB) - > unzip them to `src/examples/turtlebot4_whoami/description/generated` with a command: - > `unzip -d src/examples/turtlebot4_whoami/description turtlebot4_whoami_generated.zip` - ```bash ./scripts/parse_whoami_package.sh src/examples/turtlebot4_whoami # you will be asked to press `y` to continue From ba705fd33dd5be30f1e67a65a18983600cdcb4a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Mon, 21 Oct 2024 13:53:50 +0200 Subject: [PATCH 44/58] improve RAG formating in streamlit --- src/rai_hmi/rai_hmi/base.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/rai_hmi/rai_hmi/base.py b/src/rai_hmi/rai_hmi/base.py index a6a69a5a..9eb09382 100644 --- a/src/rai_hmi/rai_hmi/base.py +++ b/src/rai_hmi/rai_hmi/base.py @@ -145,10 +145,14 @@ def status_callback(self): self.status_publisher.publish(String(data=status.value)) def query_faiss_index_with_scores( - self, query: str, k: int = 4 + self, query: str, k: int = 20 ) -> List[Tuple[Document, float]]: output = self.faiss_index.similarity_search_with_score(query, k) - return output + formatted_output = "" + for doc, score in output: + source = doc.metadata.get("source", "Unknown source") + formatted_output += f"Document source: {source}\nScore: {score}\nContent: \n{doc.page_content}\n\n" + return formatted_output def _initialize_system_prompt(self): return append_whoami_info_to_prompt( From afec0a13e7d016ad56b7583bc5d84fac85058df0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Thu, 17 Oct 2024 18:58:27 +0200 Subject: [PATCH 45/58] feat: integrate manipulation vision tools with RaiNode --- examples/rosbot-xl-generic-node-demo.py | 24 +++++-- src/rai/rai/node.py | 11 ++- src/rai/rai/tools/ros/manipulation.py | 12 ++-- src/rai/rai/tools/ros/native_actions.py | 4 +- src/rai/rai/tools/ros/utils.py | 14 +++- .../rai_open_set_vision/tools/gdino_tools.py | 6 +- .../tools/segmentation_tools.py | 71 ++++++++++++++----- 7 files changed, 104 insertions(+), 38 deletions(-) diff --git a/examples/rosbot-xl-generic-node-demo.py b/examples/rosbot-xl-generic-node-demo.py index 1d3dcdc3..ac73e350 100644 --- a/examples/rosbot-xl-generic-node-demo.py +++ b/examples/rosbot-xl-generic-node-demo.py @@ -16,25 +16,24 @@ import rclpy import rclpy.executors -from rai_open_set_vision.tools import GetDetectionTool, GetDistanceToObjectsTool from rai.node import RaiStateBasedLlmNode +from rai.tools.ros.manipulation import GetObjectPositionsTool from rai.tools.ros.native import ( GetCameraImage, - GetMsgFromTopic, Ros2PubMessageTool, Ros2ShowMsgInterfaceTool, ) # from rai.tools.ros.native_actions import Ros2RunActionSync from rai.tools.ros.native_actions import ( + GetTransformTool, Ros2CancelAction, Ros2GetActionResult, Ros2GetLastActionFeedback, Ros2IsActionComplete, Ros2RunActionAsync, ) -from rai.tools.ros.tools import GetCurrentPositionTool from rai.tools.time import WaitForSecondsTool @@ -50,7 +49,9 @@ def main(): topics_whitelist = [ "/rosout", "/camera/camera/color/image_raw", + "/camera/camera/color/camera_info", "/camera/camera/depth/image_rect_raw", + "/camera/camera/depth/camera_info", "/map", "/scan", "/diagnostics", @@ -137,12 +138,21 @@ def main(): Ros2GetActionResult, Ros2GetLastActionFeedback, Ros2ShowMsgInterfaceTool, - GetCurrentPositionTool, WaitForSecondsTool, - GetMsgFromTopic, + # GetMsgFromTopic, GetCameraImage, - GetDetectionTool, - GetDistanceToObjectsTool, + # GetDetectionTool, + GetTransformTool, + ( + GetObjectPositionsTool, + dict( + target_frame="odom", + source_frame="sensor_frame", + camera_topic="/camera/camera/color/image_raw", + depth_topic="/camera/camera/depth/image_rect_raw", + camera_info_topic="/camera/camera/color/camera_info", + ), + ), ], ) diff --git a/src/rai/rai/node.py b/src/rai/rai/node.py index 9374d999..473665c5 100644 --- a/src/rai/rai/node.py +++ b/src/rai/rai/node.py @@ -404,14 +404,21 @@ def __init__( def _initialize_tools(self, tools: List[Type[BaseTool]]): initialized_tools: List[BaseTool] = list() for tool_cls in tools: + args = None + if type(tool_cls) is tuple: + tool_cls, args = tool_cls if issubclass(tool_cls, Ros2BaseTool): if ( issubclass(tool_cls, Ros2BaseActionTool) or "DetectionTool" in tool_cls.__name__ or "GetDistance" in tool_cls.__name__ or "GetTransformTool" in tool_cls.__name__ + or "GetObjectPositionsTool" in tool_cls.__name__ ): # TODO(boczekbartek): develop a way to handle all mutially - tool = tool_cls(node=self._async_tool_node) + if args: + tool = tool_cls(node=self._async_tool_node, **args) + else: + tool = tool_cls(node=self._async_tool_node) else: tool = tool_cls(node=self) else: @@ -523,7 +530,7 @@ async def agent_loop(self, goal_handle: ServerGoalHandle): feedback_msg = TaskAction.Feedback() feedback_msg.current_status = f"{graph_node_name}: {last_msg}" - goal_handle.publish_feedback(feedback_msg) + goal_handle.publish_feedback(feedback_msg) # ---- Share Action Result ---- if state is None: diff --git a/src/rai/rai/tools/ros/manipulation.py b/src/rai/rai/tools/ros/manipulation.py index ef4ca57a..e9586fbb 100644 --- a/src/rai/rai/tools/ros/manipulation.py +++ b/src/rai/rai/tools/ros/manipulation.py @@ -29,7 +29,8 @@ from rclpy.node import Node from tf2_geometry_msgs import do_transform_pose -from rai.tools.utils import TF2TransformFetcher +from rai.tools.ros.native_actions import Ros2BaseActionTool +from rai.tools.ros.utils import get_transform from rai_interfaces.srv import ManipulatorMoveTo @@ -140,7 +141,7 @@ class GetObjectPositionsToolInput(BaseModel): ) -class GetObjectPositionsTool(BaseTool): +class GetObjectPositionsTool(Ros2BaseActionTool): name: str = "get_object_positions" description: str = ( "Retrieve the positions of all objects of a specified type in the target frame. " @@ -153,7 +154,6 @@ class GetObjectPositionsTool(BaseTool): camera_topic: str # rgb camera topic depth_topic: str camera_info_topic: str # rgb camera info topic - node: Node get_grabbing_point_tool: GetGrabbingPointTool def __init__(self, node: Node, **kwargs): @@ -168,9 +168,8 @@ def format_pose(pose: Pose): return f"Centroid(x={pose.position.x:.2f}, y={pose.position.y:2f}, z={pose.position.z:2f})" def _run(self, object_name: str): - transform = TF2TransformFetcher( - target_frame=self.target_frame, source_frame=self.source_frame - ).get_data() + transform = get_transform(self.node, self.source_frame, self.target_frame) + self.logger.info("Got transform: {transform}") results = self.get_grabbing_point_tool._run( camera_topic=self.camera_topic, @@ -178,6 +177,7 @@ def _run(self, object_name: str): camera_info_topic=self.camera_info_topic, object_name=object_name, ) + self.logger.info("Got result: {results}") poses = [] for result in results: diff --git a/src/rai/rai/tools/ros/native_actions.py b/src/rai/rai/tools/ros/native_actions.py index 94947b45..8baea5e2 100644 --- a/src/rai/rai/tools/ros/native_actions.py +++ b/src/rai/rai/tools/ros/native_actions.py @@ -184,14 +184,14 @@ class Ros2GetLastActionFeedback(Ros2BaseActionTool): args_schema: Type[Ros2BaseInput] = Ros2BaseInput def _run(self) -> str: - return str(self.node.action_feedback) + return str(self.node.feedback) class GetTransformTool(Ros2BaseActionTool): name: str = "GetTransform" description: str = "Get transform between two frames" - def _run(self, target_frame="map", source_frame="body_link") -> dict: + def _run(self, target_frame="odom", source_frame="body_link") -> dict: return message_to_ordereddict( get_transform(self.node, target_frame, source_frame) ) diff --git a/src/rai/rai/tools/ros/utils.py b/src/rai/rai/tools/ros/utils.py index 72107533..09bd73fa 100644 --- a/src/rai/rai/tools/ros/utils.py +++ b/src/rai/rai/tools/ros/utils.py @@ -14,6 +14,7 @@ # import base64 +import time from typing import Type, Union, cast import cv2 @@ -155,17 +156,24 @@ def wait_for_message( def get_transform( - node: rclpy.node.Node, target_frame: str, source_frame: str + node: rclpy.node.Node, target_frame: str, source_frame: str, timeout=30 ) -> TransformStamped: + node.get_logger().info( + "Waiting for transform from {} to {}".format(source_frame, target_frame) + ) tf_buffer = Buffer(node=node) tf_listener = TransformListener(tf_buffer, node) transform = None - while transform is None: - rclpy.spin_once(node) + for _ in range(timeout * 10): + rclpy.spin_once(node, timeout_sec=0.1) if tf_buffer.can_transform(target_frame, source_frame, rclpy.time.Time()): transform = tf_buffer.lookup_transform( target_frame, source_frame, rclpy.time.Time() ) + break + else: + time.sleep(0.1) + tf_listener.unregister() return transform diff --git a/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/gdino_tools.py b/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/gdino_tools.py index 0415cfb6..22af5be6 100644 --- a/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/gdino_tools.py +++ b/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/gdino_tools.py @@ -25,11 +25,13 @@ ParameterUninitializedException, ) from rclpy.task import Future +from rclpy.wait_for_message import wait_for_message from rai.node import RaiAsyncToolsNode from rai.tools.ros import Ros2BaseInput, Ros2BaseTool from rai.tools.ros.utils import convert_ros_img_to_ndarray -from rai.tools.utils import wait_for_message + +# from rai.tools.utils import wait_for_message from rai_interfaces.srv import RAIGroundingDino @@ -115,7 +117,7 @@ def _call_gdino_node( future = cli.call_async(req) return future - def get_img_from_topic(self, topic: str, timeout_sec: int = 2): + def get_img_from_topic(self, topic: str, timeout_sec: int = 4): success, msg = wait_for_message( sensor_msgs.msg.Image, self.node, diff --git a/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/segmentation_tools.py b/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/segmentation_tools.py index de2c0fea..8fe57be6 100644 --- a/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/segmentation_tools.py +++ b/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/segmentation_tools.py @@ -17,6 +17,7 @@ import cv2 import numpy as np import rclpy +import rclpy.qos import sensor_msgs.msg from pydantic import Field from rai_open_set_vision import GDINO_SERVICE_NAME @@ -26,8 +27,11 @@ ParameterUninitializedException, ) -from rai.node import RaiBaseNode -from rai.tools.ros import Ros2BaseInput, Ros2BaseTool +# from rai.tools.utils import wait_for_message +from rclpy.wait_for_message import wait_for_message + +from rai.tools.ros import Ros2BaseInput +from rai.tools.ros.native_actions import Ros2BaseActionTool from rai.tools.ros.utils import convert_ros_img_to_base64, convert_ros_img_to_ndarray from rai_interfaces.srv import RAIGroundedSam, RAIGroundingDino @@ -63,9 +67,7 @@ class GetGrabbingPointInput(Ros2BaseInput): # --------------------- Tools --------------------- -class GetSegmentationTool(Ros2BaseTool): - node: RaiBaseNode = Field(..., exclude=True) - +class GetSegmentationTool(Ros2BaseActionTool): name: str = "" description: str = "" @@ -102,12 +104,29 @@ def _get_gsam_response(self, future: Future) -> Optional[RAIGroundedSam.Response return response return None + def get_img_from_topic(self, topic: str, timeout_sec: int = 10): + success, msg = wait_for_message( + sensor_msgs.msg.Image, + self.node, + topic, + qos_profile=rclpy.qos.qos_profile_sensor_data, + time_to_wait=timeout_sec, + ) + + if success: + self.node.get_logger().info(f"Received message of type from topic {topic}") + return msg + else: + error = f"No message received in {timeout_sec} seconds from topic {topic}" + self.node.get_logger().error(error) + return error + def _get_image_message(self, topic: str) -> sensor_msgs.msg.Image: - msg = self.node.get_raw_message_from_topic(topic) + msg = self.get_img_from_topic(topic) if type(msg) is sensor_msgs.msg.Image: return msg else: - raise Exception("Received wrong message") + raise Exception(f"Received wrong message: {type(msg)}") def _call_gdino_node( self, camera_img_message: sensor_msgs.msg.Image, object_name: str @@ -213,13 +232,25 @@ class GetGrabbingPointTool(GetSegmentationTool): args_schema: Type[GetGrabbingPointInput] = GetGrabbingPointInput def _get_camera_info_message(self, topic: str) -> sensor_msgs.msg.CameraInfo: - for _ in range(3): - msg = self.node.get_raw_message_from_topic(topic, timeout_sec=3.0) - if isinstance(msg, sensor_msgs.msg.CameraInfo): - return msg - self.node.get_logger().warn("Received wrong message type. Retrying...") + self.node.get_logger().info(f"Waiting for CameraInfo from topic {topic}") + success, msg = wait_for_message( + sensor_msgs.msg.CameraInfo, + self.node, + topic, + qos_profile=rclpy.qos.qos_profile_sensor_data, + time_to_wait=3, + ) + print(msg) - raise Exception("Failed to receive correct CameraInfo message after 3 attempts") + if success: + self.node.get_logger().info(f"Received message of type from topic {topic}") + return msg + else: + error = f"No message received in 3 seconds from topic {topic}" + self.node.get_logger().error(error) + raise Exception( + "Failed to receive correct CameraInfo message after 3 attempts" + ) def _get_intrinsic_from_camera_info(self, camera_info: sensor_msgs.msg.CameraInfo): """Extract camera intrinsic parameters from the CameraInfo message.""" @@ -279,11 +310,14 @@ def _run( camera_info_topic: str, object_name: str, ): - camera_img_msg = self._get_image_message(camera_topic) - depth_msg = self._get_image_message(depth_topic) camera_info = self._get_camera_info_message(camera_info_topic) - + self.logger.info("Received camera info") + camera_img_msg = self.get_img_from_topic(camera_topic) + self.logger.info("Received camera image") + depth_msg = self.get_img_from_topic(depth_topic) + self.logger.info("Received depth image") intrinsic = self._get_intrinsic_from_camera_info(camera_info) + self.logger.info("Received camera intrinsic") future = self._call_gdino_node(camera_img_msg, object_name) logger = self.node.get_logger() @@ -300,13 +334,16 @@ def _run( ) conversion_ratio = 0.001 resolved = None + self.logger.info("Waiting gdino response") while rclpy.ok(): resolved = self._get_gdino_response(future) if resolved is not None: break assert resolved is not None + self.logger.info("Got gdino response") future = self._call_gsam_node(camera_img_msg, resolved) + self.logger.info("Waiting gsam response") ret = [] while rclpy.ok(): @@ -316,6 +353,8 @@ def _run( ret.append(convert_ros_img_to_base64(img_msg)) break assert resolved is not None + + self.logger.info("Got gsam response") rets = [] for mask_msg in resolved.masks: rets.append( From 1493b7ac8b6dfe7122a18803f7988a17e34e991d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Fri, 18 Oct 2024 09:52:23 +0200 Subject: [PATCH 46/58] rosbot-example: add detection tool and img describer again --- examples/rosbot-xl-generic-node-demo.py | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/examples/rosbot-xl-generic-node-demo.py b/examples/rosbot-xl-generic-node-demo.py index ac73e350..0330f9e2 100644 --- a/examples/rosbot-xl-generic-node-demo.py +++ b/examples/rosbot-xl-generic-node-demo.py @@ -16,16 +16,15 @@ import rclpy import rclpy.executors +from rai_open_set_vision import GetDetectionTool -from rai.node import RaiStateBasedLlmNode +from rai.node import RaiStateBasedLlmNode, describe_ros_image from rai.tools.ros.manipulation import GetObjectPositionsTool from rai.tools.ros.native import ( GetCameraImage, Ros2PubMessageTool, Ros2ShowMsgInterfaceTool, ) - -# from rai.tools.ros.native_actions import Ros2RunActionSync from rai.tools.ros.native_actions import ( GetTransformTool, Ros2CancelAction, @@ -40,11 +39,11 @@ def main(): rclpy.init() - # observe_topics = [ - # "/camera/camera/color/image_raw", - # ] - # - # observe_postprocessors = {"/camera/camera/color/image_raw": describe_ros_image} + observe_topics = [ + "/camera/camera/color/image_raw", + ] + + observe_postprocessors = {"/camera/camera/color/image_raw": describe_ros_image} topics_whitelist = [ "/rosout", @@ -126,8 +125,8 @@ def main(): """ node = RaiStateBasedLlmNode( - observe_topics=None, - observe_postprocessors=None, + observe_topics=observe_topics, + observe_postprocessors=observe_postprocessors, whitelist=topics_whitelist + actions_whitelist, system_prompt=SYSTEM_PROMPT, tools=[ @@ -139,9 +138,8 @@ def main(): Ros2GetLastActionFeedback, Ros2ShowMsgInterfaceTool, WaitForSecondsTool, - # GetMsgFromTopic, GetCameraImage, - # GetDetectionTool, + GetDetectionTool, GetTransformTool, ( GetObjectPositionsTool, From 66f521100e81dd522e0606fe3a7043dd510bde8e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Fri, 18 Oct 2024 10:07:56 +0200 Subject: [PATCH 47/58] remove image describer and cmd_vel --- examples/rosbot-xl-generic-node-demo.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/examples/rosbot-xl-generic-node-demo.py b/examples/rosbot-xl-generic-node-demo.py index 0330f9e2..7f3e36b0 100644 --- a/examples/rosbot-xl-generic-node-demo.py +++ b/examples/rosbot-xl-generic-node-demo.py @@ -18,7 +18,7 @@ import rclpy.executors from rai_open_set_vision import GetDetectionTool -from rai.node import RaiStateBasedLlmNode, describe_ros_image +from rai.node import RaiStateBasedLlmNode from rai.tools.ros.manipulation import GetObjectPositionsTool from rai.tools.ros.native import ( GetCameraImage, @@ -39,11 +39,11 @@ def main(): rclpy.init() - observe_topics = [ - "/camera/camera/color/image_raw", - ] - - observe_postprocessors = {"/camera/camera/color/image_raw": describe_ros_image} + # observe_topics = [ + # "/camera/camera/color/image_raw", + # ] + # + # observe_postprocessors = {"/camera/camera/color/image_raw": describe_ros_image} topics_whitelist = [ "/rosout", @@ -54,7 +54,7 @@ def main(): "/map", "/scan", "/diagnostics", - "/cmd_vel", + # "/cmd_vel", "/led_strip", ] @@ -125,8 +125,8 @@ def main(): """ node = RaiStateBasedLlmNode( - observe_topics=observe_topics, - observe_postprocessors=observe_postprocessors, + observe_topics=None, + observe_postprocessors=None, whitelist=topics_whitelist + actions_whitelist, system_prompt=SYSTEM_PROMPT, tools=[ From 58c38afb90164fc64a190069cbb4642a8508c9ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Fri, 18 Oct 2024 11:57:19 +0200 Subject: [PATCH 48/58] wip --- src/rai/rai/node.py | 1 + src/rai/rai/tools/utils.py | 1 - src/rai/rai/utils/ros.py | 4 ++++ .../rai_open_set_vision/tools/gdino_tools.py | 4 +--- .../rai_open_set_vision/tools/segmentation_tools.py | 7 ++++--- src/rai_hmi/rai_hmi/base.py | 1 + 6 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/rai/rai/node.py b/src/rai/rai/node.py index 473665c5..05ce229b 100644 --- a/src/rai/rai/node.py +++ b/src/rai/rai/node.py @@ -414,6 +414,7 @@ def _initialize_tools(self, tools: List[Type[BaseTool]]): or "GetDistance" in tool_cls.__name__ or "GetTransformTool" in tool_cls.__name__ or "GetObjectPositionsTool" in tool_cls.__name__ + or "GetDetectionTool" in tool_cls.__name__ ): # TODO(boczekbartek): develop a way to handle all mutially if args: tool = tool_cls(node=self._async_tool_node, **args) diff --git a/src/rai/rai/tools/utils.py b/src/rai/rai/tools/utils.py index a9aca8a5..fb17774e 100644 --- a/src/rai/rai/tools/utils.py +++ b/src/rai/rai/tools/utils.py @@ -43,7 +43,6 @@ from rai.messages import ToolMultimodalMessage -# Copied from https://github.com/ros2/rclpy/blob/jazzy/rclpy/rclpy/wait_for_message.py, to support humble def wait_for_message( msg_type, node: "Node", diff --git a/src/rai/rai/utils/ros.py b/src/rai/rai/utils/ros.py index d5bb4d1d..960e47fd 100644 --- a/src/rai/rai/utils/ros.py +++ b/src/rai/rai/utils/ros.py @@ -36,11 +36,15 @@ def __init__(self, llm: BaseChatModel, bufsize: int = 100) -> None: ) llm = llm self.llm = self.template | llm + self.filter_out = ["rviz", "rai"] def clear(self): self._buffer.clear() def append(self, line: str): + for w in self.filter_out: + if w in line: + return self._buffer.append(line) if len(self._buffer) > self.bufsize: self._buffer.popleft() diff --git a/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/gdino_tools.py b/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/gdino_tools.py index 22af5be6..591af011 100644 --- a/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/gdino_tools.py +++ b/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/gdino_tools.py @@ -25,13 +25,11 @@ ParameterUninitializedException, ) from rclpy.task import Future -from rclpy.wait_for_message import wait_for_message from rai.node import RaiAsyncToolsNode from rai.tools.ros import Ros2BaseInput, Ros2BaseTool from rai.tools.ros.utils import convert_ros_img_to_ndarray - -# from rai.tools.utils import wait_for_message +from rai.tools.utils import wait_for_message from rai_interfaces.srv import RAIGroundingDino diff --git a/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/segmentation_tools.py b/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/segmentation_tools.py index 8fe57be6..de2ab5a1 100644 --- a/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/segmentation_tools.py +++ b/src/rai_extensions/rai_open_set_vision/rai_open_set_vision/tools/segmentation_tools.py @@ -27,14 +27,15 @@ ParameterUninitializedException, ) -# from rai.tools.utils import wait_for_message -from rclpy.wait_for_message import wait_for_message - from rai.tools.ros import Ros2BaseInput from rai.tools.ros.native_actions import Ros2BaseActionTool from rai.tools.ros.utils import convert_ros_img_to_base64, convert_ros_img_to_ndarray +from rai.tools.utils import wait_for_message from rai_interfaces.srv import RAIGroundedSam, RAIGroundingDino +# from rclpy.wait_for_message import wait_for_message + + # --------------------- Inputs --------------------- diff --git a/src/rai_hmi/rai_hmi/base.py b/src/rai_hmi/rai_hmi/base.py index 9eb09382..be34d418 100644 --- a/src/rai_hmi/rai_hmi/base.py +++ b/src/rai_hmi/rai_hmi/base.py @@ -78,6 +78,7 @@ class BaseHMINode(Node): They will be done by another agent resposible for communication with the robotic's stack. You are the embodied AI robot, so please describe yourself and you action in 1st person. + If you are asked about logs, or what was write or wrong about the mission, use get_mission_memory tool to get such information. """ def __init__( From fd419fb0c442aba13389f1e835cd07e0f4b2fe31 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Fri, 18 Oct 2024 17:23:34 +0200 Subject: [PATCH 49/58] increase msg waiting timeout --- src/rai/rai/tools/ros/native.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rai/rai/tools/ros/native.py b/src/rai/rai/tools/ros/native.py index a5af1c1e..ebf486f7 100644 --- a/src/rai/rai/tools/ros/native.py +++ b/src/rai/rai/tools/ros/native.py @@ -230,6 +230,6 @@ class GetCameraImage(Ros2BaseTool): args_schema: Type[TopicInput] = TopicInput def _run(self, topic_name: str): - msg = self.node.get_raw_message_from_topic(topic_name) + msg = self.node.get_raw_message_from_topic(topic_name, timeout_sec=3.0) img = convert_ros_img_to_base64(msg) return "Got image", {"images": [img]} From 900402da2bb1765076cda4681760e9c581cc46b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Fri, 18 Oct 2024 20:58:04 +0200 Subject: [PATCH 50/58] allow 1 or 2 column layout for text hmi --- src/rai_hmi/rai_hmi/text_hmi.py | 105 ++++++++++++++++++++++---------- 1 file changed, 72 insertions(+), 33 deletions(-) diff --git a/src/rai_hmi/rai_hmi/text_hmi.py b/src/rai_hmi/rai_hmi/text_hmi.py index 1462ffe8..88648414 100644 --- a/src/rai_hmi/rai_hmi/text_hmi.py +++ b/src/rai_hmi/rai_hmi/text_hmi.py @@ -13,6 +13,7 @@ # limitations under the License. # import base64 +import enum import io import logging import sys @@ -56,6 +57,14 @@ MAX_DISPLAY = 5 +class AppLayout(enum.Enum): + ONE_COLUMN = 1 + TWO_COLUMNS = 2 + + +LAYOUT = AppLayout.ONE_COLUMN + + # ---------- Cached Resources ---------- @st.cache_resource def parse_args() -> Tuple[Optional[str], Optional[Path]]: @@ -158,8 +167,6 @@ class SystemStatus(BaseModel): class Layout: - """App has two columns: chat + robot state""" - def __init__(self, robot_description_package) -> None: if robot_description_package: st.title(f"{robot_description_package.replace('_whoami', '')} chat app") @@ -168,37 +175,74 @@ def __init__(self, robot_description_package) -> None: logging.warning( "No robot_description_package provided. Some functionalities may not work." ) - self.n_columns = 2 + self.n_columns = 1 self.tool_placeholders = dict() - def draw(self, system_status: SystemStatus): - self.draw_app_status_expander(system_status) - self.draw_columns() - def draw_app_status_expander(self, system_status: SystemStatus): with st.expander("System status", expanded=False): st.json(system_status.model_dump()) + def draw(self, system_status: SystemStatus): + self.draw_app_status_expander(system_status) + + def create_tool_expanders(self, tool_calls: List[ToolCall]): + for tool_call in tool_calls: + tool_call = cast(ToolCall, tool_call) + with st.expander(f"Tool call: {tool_call['name']}"): + st.markdown(f"Arguments: {tool_call['args']}") + self.tool_placeholders[tool_call["id"]] = st.empty() + + def write_tool_message(self, msg: ToolMessage, tool_call: ToolCall): + display_agent_message( + msg, self.tool_placeholders[msg.tool_call_id], tool_call=tool_call + ) + + def write_chat_msg(self, msg: BaseMessage): + display_agent_message(msg) + + def show_chat(self, history, tool_calls: Dict[str, ToolCall]): + self.show_history(history, tool_calls) + + def show_history(self, history, tool_calls): + def display(message, no_expand=False): + if isinstance(message, ToolMessage): + display_agent_message( + message, + no_expand=no_expand, + tool_call=tool_calls[message.tool_call_id], + ) + else: + display_agent_message(message, no_expand=no_expand) + + for message in history: + display(message) + + +class TwoColLayout(Layout): + """App has two columns: chat + robot state""" + + def __init__(self, robot_description_package) -> None: + super().__init__(robot_description_package) + self.n_columns = 2 + + def draw(self, system_status: SystemStatus): + self.draw_app_status_expander(system_status) + self.draw_columns() + def draw_columns(self): self.chat_column, self.mission_column = st.columns(self.n_columns) def create_tool_expanders(self, tool_calls: List[ToolCall]): with self.chat_column: - for tool_call in tool_calls: - tool_call = cast(ToolCall, tool_call) - with st.expander(f"Tool call: {tool_call['name']}"): - st.markdown(f"Arguments: {tool_call['args']}") - self.tool_placeholders[tool_call["id"]] = st.empty() + super().create_tool_expanders(tool_calls) def write_tool_message(self, msg: ToolMessage, tool_call: ToolCall): with self.chat_column: - display_agent_message( - msg, self.tool_placeholders[msg.tool_call_id], tool_call=tool_call - ) + super().write_tool_message(msg, tool_call) def write_chat_msg(self, msg: BaseMessage): with self.chat_column: - display_agent_message(msg) + super().write_chat_msg(msg) def write_mission_msg(self, msg: MissionMessage): with self.mission_column: @@ -207,25 +251,18 @@ def write_mission_msg(self, msg: MissionMessage): def show_chat(self, history, tool_calls: Dict[str, ToolCall]): with self.chat_column: - self.__show_history(history, tool_calls) + super().show_chat(history, tool_calls) def show_mission(self, history, tool_calls: Dict[str, ToolCall]): with self.mission_column: - self.__show_history(history, tool_calls) + self.show_history(history, tool_calls) - def __show_history(self, history, tool_calls): - def display(message, no_expand=False): - if isinstance(message, ToolMessage): - display_agent_message( - message, - no_expand=no_expand, - tool_call=tool_calls[message.tool_call_id], - ) - else: - display_agent_message(message, no_expand=no_expand) - for message in history: - display(message) +def get_layout(robot_description_package) -> Layout: + if LAYOUT == AppLayout.TWO_COLUMNS: + return TwoColLayout(robot_description_package) + else: + return Layout(robot_description_package) class Chat: @@ -253,7 +290,8 @@ def tool(self, msg): def mission(self, msg: MissionMessage): logger.info(f'Mission said: "{msg}"') self.memory.add_mission(msg) - self.layout.write_mission_msg(msg) + if isinstance(self.layout, TwoColLayout): + self.layout.write_mission_msg(msg) class Agent: @@ -277,7 +315,7 @@ def __init__(self, robot_description_package, ros2_whitelist: Path) -> None: self.robot_description_package = robot_description_package - self.layout = Layout(self.robot_description_package) + self.layout = get_layout(self.robot_description_package) self.memory = initialize_memory() self.chat = Chat(self.memory, self.layout) @@ -331,7 +369,8 @@ def recreate_from_memory(self): See: https://docs.streamlit.io/get-started/fundamentals/main-concepts """ self.layout.show_chat(self.memory.chat_memory, self.memory.tool_calls) - self.layout.show_mission(self.memory.mission_memory, self.memory.tool_calls) + if isinstance(self.layout, TwoColLayout): + self.layout.show_mission(self.memory.mission_memory, self.memory.tool_calls) def prompt_callback(self): prompt = st.session_state.prompt From 9bcb17fc9f096e17e9040d488bffe545e9d71903 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Fri, 18 Oct 2024 20:58:29 +0200 Subject: [PATCH 51/58] make vector database optional --- src/rai_hmi/rai_hmi/base.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/rai_hmi/rai_hmi/base.py b/src/rai_hmi/rai_hmi/base.py index be34d418..9404d87a 100644 --- a/src/rai_hmi/rai_hmi/base.py +++ b/src/rai_hmi/rai_hmi/base.py @@ -79,12 +79,14 @@ class BaseHMINode(Node): stack. You are the embodied AI robot, so please describe yourself and you action in 1st person. If you are asked about logs, or what was write or wrong about the mission, use get_mission_memory tool to get such information. + If you are asked about interpration about camera image, add such mission as well """ def __init__( self, node_name: str, queue: Queue, + load_vector_database: bool = False, robot_description_package: Optional[str] = None, ): super().__init__(node_name=node_name) @@ -122,7 +124,10 @@ def __init__( self.agent = None # order of the initialization is important self.system_prompt = self._initialize_system_prompt() - self.faiss_index = self._load_documentation() + if load_vector_database: + self.faiss_index = self._load_documentation() + else: + self.faiss_index = None self.tools = self._initialize_available_tools() self.initialize_task_action_client_and_server() @@ -183,9 +188,6 @@ def _load_documentation(self) -> Optional[FAISS]: def initialize_task_action_client_and_server(self): """Initialize the action client and server for task handling.""" self.task_action_client = ActionClient(self, TaskAction, "perform_task") - # self.task_feedback_action_server = ActionServer( - # self, TaskFeedback, "provide_task_feedback", self.handle_task_feedback - # ) def execute_mission(self, task: Task): """Sends a task to the action server to be handled by the rai node.""" From e82a039d931d2377efbff5bb195dc7a9635a232d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Fri, 18 Oct 2024 21:34:10 +0200 Subject: [PATCH 52/58] use not spining node for reading from topics --- src/rai/rai/node.py | 111 +++++++++++++++++------- src/rai/rai/tools/ros/native.py | 34 +------- src/rai/rai/tools/ros/native_actions.py | 34 +++++++- src/rai/rai/tools/ros/tools.py | 2 +- src/rai_hmi/rai_hmi/agent.py | 9 +- src/rai_hmi/rai_hmi/ros.py | 3 +- src/rai_hmi/rai_hmi/text_hmi.py | 16 +++- 7 files changed, 136 insertions(+), 73 deletions(-) diff --git a/src/rai/rai/node.py b/src/rai/rai/node.py index 05ce229b..cf2ec98a 100644 --- a/src/rai/rai/node.py +++ b/src/rai/rai/node.py @@ -142,9 +142,62 @@ def ros2_build_msg(msg_type: str, msg_args: Dict[str, Any]) -> Tuple[object, Typ return msg, msg_cls -class RaiAsyncToolsNode(Node): - def __init__(self): - super().__init__("rai_internal_action_node") +class RaiBaseNode(Node): + def __init__( + self, + whitelist: Optional[List[str]] = None, + *args, + **kwargs, + ): + super().__init__(*args, **kwargs) + + self.robot_state: Dict[str, Any] = dict() # where Any is ROS 2 message type + + self.DISCOVERY_FREQ = 2.0 + self.timer = self.create_timer( + self.DISCOVERY_FREQ, + self.discovery, + ) + self.ros_discovery_info = NodeDiscovery(whitelist=whitelist) + self.discovery() + self.qos_profile = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + liveliness=LivelinessPolicy.AUTOMATIC, + ) + + self.state_subscribers = dict() + + def spin(self): + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(self) + executor.spin() + + def get_msg_type(self, topic: str, n_tries: int = 5) -> Any: + """Sometimes node fails to do full discovery, therefore we need to retry""" + for _ in range(n_tries): + if topic in self.ros_discovery_info.topics_and_types: + msg_type = self.ros_discovery_info.topics_and_types[topic] + return import_message_from_str(msg_type) + else: + self.get_logger().info(f"Waiting for topic: {topic}") + self.discovery() + time.sleep(self.DISCOVERY_FREQ) + raise KeyError(f"Topic {topic} not found") + + def discovery(self): + self.ros_discovery_info.set( + self.get_topic_names_and_types(), + self.get_service_names_and_types(), + get_action_names_and_types(self), + ) + + +class RaiAsyncToolsNode(RaiBaseNode): + def __init__(self, node_name="rai_internal_action_node"): + super().__init__(node_name=node_name) self.goal_handle = None self.result_future = None @@ -240,6 +293,7 @@ def _cancel_task(self): rclpy.spin_until_future_complete(self, future) return True +<<<<<<< HEAD class RaiBaseNode(Node): def __init__( @@ -287,32 +341,26 @@ def discovery(self): get_action_names_and_types(self), ) - def get_raw_message_from_topic(self, topic: str, timeout_sec: int = 1) -> Any: + def get_raw_message_from_topic(self, topic: str, timeout_sec: int = 3) -> Any: self.get_logger().debug(f"Getting msg from topic: {topic}") - if topic in self.state_subscribers and topic in self.robot_state: - self.get_logger().debug("Returning cached message") - return self.robot_state[topic] - else: - msg_type = self.get_msg_type(topic) - success, msg = wait_for_message( - msg_type, - self, - topic, - qos_profile=self.qos_profile, - time_to_wait=timeout_sec, - ) + msg_type = self.get_msg_type(topic) + success, msg = wait_for_message( + msg_type, + self, + topic, + qos_profile=self.qos_profile, + time_to_wait=timeout_sec, + ) - if success: - self.get_logger().debug( - f"Received message of type {msg_type.__class__.__name__} from topic {topic}" - ) - return msg - else: - error = ( - f"No message received in {timeout_sec} seconds from topic {topic}" - ) - self.get_logger().error(error) - return error + if success: + self.get_logger().debug( + f"Received message of type {msg_type.__class__.__name__} from topic {topic}" + ) + return msg + else: + error = f"No message received in {timeout_sec} seconds from topic {topic}" + self.get_logger().error(error) + return error def get_msg_type(self, topic: str, n_tries: int = 5) -> Any: """Sometimes node fails to do full discovery, therefore we need to retry""" @@ -323,7 +371,7 @@ def get_msg_type(self, topic: str, n_tries: int = 5) -> Any: else: self.get_logger().info(f"Waiting for topic: {topic}") self.discovery() - time.sleep(self.DISCOVERY_FREQ) + rclpy.spin_once(self, timeout_sec=self.DISCOVERY_FREQ) raise KeyError(f"Topic {topic} not found") @@ -365,6 +413,9 @@ def __init__( qos_profile=self.qos_profile, ) + # ------- additional ROS2 node that can be spinned by tools ------- + self._spinnable_tool_node = RaiAsyncToolsNode() + # ---------- Robot State ---------- self.robot_state = dict() self.state_topics = observe_topics if observe_topics is not None else [] @@ -417,9 +468,9 @@ def _initialize_tools(self, tools: List[Type[BaseTool]]): or "GetDetectionTool" in tool_cls.__name__ ): # TODO(boczekbartek): develop a way to handle all mutially if args: - tool = tool_cls(node=self._async_tool_node, **args) + tool = tool_cls(node=self._spinnable_tool_node, **args) else: - tool = tool_cls(node=self._async_tool_node) + tool = tool_cls(node=self._spinnable_tool_node) else: tool = tool_cls(node=self) else: diff --git a/src/rai/rai/tools/ros/native.py b/src/rai/rai/tools/ros/native.py index ebf486f7..a7ae7e67 100644 --- a/src/rai/rai/tools/ros/native.py +++ b/src/rai/rai/tools/ros/native.py @@ -26,12 +26,11 @@ import rclpy.task import rosidl_runtime_py.set_message import rosidl_runtime_py.utilities -import sensor_msgs.msg from langchain.tools import BaseTool from pydantic import BaseModel, Field from rclpy.impl.rcutils_logger import RcutilsLogger -from .utils import convert_ros_img_to_base64, import_message_from_str +from .utils import import_message_from_str # --------------------- Inputs --------------------- @@ -202,34 +201,3 @@ def callback(): f"Published messages for {timeout_seconds}s to topic '{topic_name}' with rate {rate}" ) return - - -class TopicInput(Ros2BaseInput): - topic_name: str = Field(..., description="Ros2 topic name") - - -class GetMsgFromTopic(Ros2BaseTool): - name: str = "get_msg_from_topic" - description: str = "Get message from topic" - args_schema: Type[TopicInput] = TopicInput - response_format: str = "content_and_artifact" - - def _run(self, topic_name: str): - msg = self.node.get_raw_message_from_topic(topic_name) - if type(msg) is sensor_msgs.msg.Image: - img = convert_ros_img_to_base64(msg) - return "Got image", {"images": [img]} - else: - return str(msg), {} - - -class GetCameraImage(Ros2BaseTool): - name: str = "get_camera_image" - description: str = "get image from robots camera" - response_format: str = "content_and_artifact" - args_schema: Type[TopicInput] = TopicInput - - def _run(self, topic_name: str): - msg = self.node.get_raw_message_from_topic(topic_name, timeout_sec=3.0) - img = convert_ros_img_to_base64(msg) - return "Got image", {"images": [img]} diff --git a/src/rai/rai/tools/ros/native_actions.py b/src/rai/rai/tools/ros/native_actions.py index 8baea5e2..a0ca64b1 100644 --- a/src/rai/rai/tools/ros/native_actions.py +++ b/src/rai/rai/tools/ros/native_actions.py @@ -17,13 +17,14 @@ import rosidl_runtime_py.set_message import rosidl_runtime_py.utilities +import sensor_msgs.msg from action_msgs.msg import GoalStatus from pydantic import BaseModel, Field from rclpy.action.client import ActionClient from rosidl_runtime_py import message_to_ordereddict from .native import Ros2BaseInput, Ros2BaseTool -from .utils import get_transform +from .utils import convert_ros_img_to_base64, get_transform # --------------------- Inputs --------------------- @@ -195,3 +196,34 @@ def _run(self, target_frame="odom", source_frame="body_link") -> dict: return message_to_ordereddict( get_transform(self.node, target_frame, source_frame) ) + + +class TopicInput(Ros2BaseInput): + topic_name: str = Field(..., description="Ros2 topic name") + + +class GetMsgFromTopic(Ros2BaseActionTool): + name: str = "get_msg_from_topic" + description: str = "Get message from topic" + args_schema: Type[TopicInput] = TopicInput + response_format: str = "content_and_artifact" + + def _run(self, topic_name: str): + msg = self.node.get_raw_message_from_topic(topic_name, timeout_sec=3.0) + if type(msg) is sensor_msgs.msg.Image: + img = convert_ros_img_to_base64(msg) + return "Got image", {"images": [img]} + else: + return str(msg), {} + + +class GetCameraImage(Ros2BaseActionTool): + name: str = "get_camera_image" + description: str = "get image from robots camera" + response_format: str = "content_and_artifact" + args_schema: Type[TopicInput] = TopicInput + + def _run(self, topic_name: str): + msg = self.node.get_raw_message_from_topic(topic_name, timeout_sec=3.0) + img = convert_ros_img_to_base64(msg) + return "Got image", {"images": [img]} diff --git a/src/rai/rai/tools/ros/tools.py b/src/rai/rai/tools/ros/tools.py index 9344362a..5521d44d 100644 --- a/src/rai/rai/tools/ros/tools.py +++ b/src/rai/rai/tools/ros/tools.py @@ -30,7 +30,7 @@ from rai.tools.ros.deprecated import SingleMessageGrabber from rai.tools.utils import TF2TransformFetcher -from .native import TopicInput +from .native_actions import TopicInput logger = logging.getLogger(__name__) diff --git a/src/rai_hmi/rai_hmi/agent.py b/src/rai_hmi/rai_hmi/agent.py index a74e907b..d5839f66 100644 --- a/src/rai_hmi/rai_hmi/agent.py +++ b/src/rai_hmi/rai_hmi/agent.py @@ -19,8 +19,9 @@ from langchain.tools import tool from rai.agents.conversational_agent import create_conversational_agent -from rai.node import RaiBaseNode -from rai.tools.ros.native import GetCameraImage, Ros2GetRobotInterfaces +from rai.node import RaiAsyncToolsNode +from rai.tools.ros.native import Ros2GetRobotInterfaces +from rai.tools.ros.native_actions import GetCameraImage from rai.utils.model_initialization import get_llm_model from rai_hmi.base import BaseHMINode from rai_hmi.chat_msgs import MissionMessage @@ -28,7 +29,9 @@ from rai_hmi.text_hmi_utils import Memory -def initialize_agent(hmi_node: BaseHMINode, rai_node: RaiBaseNode, memory: Memory): +def initialize_agent( + hmi_node: BaseHMINode, rai_node: RaiAsyncToolsNode, memory: Memory +): llm = get_llm_model(model_type="complex_model") @tool diff --git a/src/rai_hmi/rai_hmi/ros.py b/src/rai_hmi/rai_hmi/ros.py index de0e812e..64259cf3 100644 --- a/src/rai_hmi/rai_hmi/ros.py +++ b/src/rai_hmi/rai_hmi/ros.py @@ -21,7 +21,7 @@ import rclpy from rclpy.executors import MultiThreadedExecutor -from rai.node import RaiBaseNode +from rai.node import RaiAsyncToolsNode from rai_hmi.base import BaseHMINode @@ -43,7 +43,6 @@ def initialize_ros_nodes( executor = MultiThreadedExecutor() executor.add_node(hmi_node) - executor.add_node(rai_node) threading.Thread(target=executor.spin, daemon=True).start() diff --git a/src/rai_hmi/rai_hmi/text_hmi.py b/src/rai_hmi/rai_hmi/text_hmi.py index 88648414..761b3cf4 100644 --- a/src/rai_hmi/rai_hmi/text_hmi.py +++ b/src/rai_hmi/rai_hmi/text_hmi.py @@ -38,7 +38,7 @@ from streamlit.delta_generator import DeltaGenerator from rai.messages import HumanMultimodalMessage -from rai.node import RaiBaseNode +from rai.node import RaiAsyncToolsNode from rai.utils.artifacts import get_stored_artifacts from rai_hmi.agent import initialize_agent from rai_hmi.base import BaseHMINode @@ -62,7 +62,8 @@ class AppLayout(enum.Enum): TWO_COLUMNS = 2 -LAYOUT = AppLayout.ONE_COLUMN +# NOTE(boczekbartek): ONE_COLUMN is not stable yet +LAYOUT = AppLayout.TWO_COLUMNS # ---------- Cached Resources ---------- @@ -80,7 +81,7 @@ def initialize_memory() -> Memory: @st.cache_resource def initialize_agent_streamlit( - _hmi_node: BaseHMINode, _rai_node: RaiBaseNode, _memory: Memory + _hmi_node: BaseHMINode, _rai_node: RaiAsyncToolsNode, _memory: Memory ): return initialize_agent(_hmi_node, _rai_node, _memory) @@ -166,6 +167,14 @@ class SystemStatus(BaseModel): system_prompt: bool +class Empty(object): + def __enter__(self): + pass + + def __exit__(self, *args): + pass + + class Layout: def __init__(self, robot_description_package) -> None: if robot_description_package: @@ -177,6 +186,7 @@ def __init__(self, robot_description_package) -> None: ) self.n_columns = 1 self.tool_placeholders = dict() + self.chat_column = Empty def draw_app_status_expander(self, system_status: SystemStatus): with st.expander("System status", expanded=False): From 042c4cf2ac03f5c90d242dffbbee2c2f837511df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Sat, 19 Oct 2024 03:32:20 +0200 Subject: [PATCH 53/58] basic_streamlit_demo --- src/rai/rai/node.py | 8 +- src/rai/rai/tools/ros/native.py | 10 + src/rai/rai/tools/ros/utils.py | 2 +- src/rai/rai/utils/ros.py | 6 +- src/rai_hmi/rai_hmi/base.py | 1 + src/rai_hmi/rai_hmi/basic_nav.py | 193 +++++++++++++++++++ src/rai_hmi/rai_hmi/basic_nav_async.py | 249 +++++++++++++++++++++++++ 7 files changed, 461 insertions(+), 8 deletions(-) create mode 100644 src/rai_hmi/rai_hmi/basic_nav.py create mode 100644 src/rai_hmi/rai_hmi/basic_nav_async.py diff --git a/src/rai/rai/node.py b/src/rai/rai/node.py index cf2ec98a..3c3843af 100644 --- a/src/rai/rai/node.py +++ b/src/rai/rai/node.py @@ -145,7 +145,7 @@ def ros2_build_msg(msg_type: str, msg_args: Dict[str, Any]) -> Tuple[object, Typ class RaiBaseNode(Node): def __init__( self, - whitelist: Optional[List[str]] = None, + allow_list: Optional[List[str]] = None, *args, **kwargs, ): @@ -158,7 +158,7 @@ def __init__( self.DISCOVERY_FREQ, self.discovery, ) - self.ros_discovery_info = NodeDiscovery(whitelist=whitelist) + self.ros_discovery_info = NodeDiscovery(allow_list=allow_list) self.discovery() self.qos_profile = QoSProfile( history=HistoryPolicy.KEEP_LAST, @@ -391,14 +391,14 @@ def __init__( system_prompt: str, observe_topics: Optional[List[str]] = None, observe_postprocessors: Optional[Dict[str, Callable[[Any], Any]]] = None, - whitelist: Optional[List[str]] = None, + allow_list: Optional[List[str]] = None, tools: Optional[List[Type[BaseTool]]] = None, *args, **kwargs, ): super().__init__( node_name="rai_node", - whitelist=whitelist, + allow_list=allow_list, *args, **kwargs, ) diff --git a/src/rai/rai/tools/ros/native.py b/src/rai/rai/tools/ros/native.py index a7ae7e67..89c6c9fa 100644 --- a/src/rai/rai/tools/ros/native.py +++ b/src/rai/rai/tools/ros/native.py @@ -135,6 +135,16 @@ def _run(self, msg_name: str): ) +def publish_msg_to_topic(node, topic, msg): + reliable_qos = rclpy.qos.QoSProfile( + reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, + history=rclpy.qos.HistoryPolicy.KEEP_LAST, + depth=1, + ) + publisher = node.create_publisher(msg, topic, reliable_qos) + publisher.publish(msg) + + class Ros2PubMessageTool(Ros2BaseTool): name: str = "PubRos2MessageTool" description: str = """A tool for publishing a message to a ros2 topic diff --git a/src/rai/rai/tools/ros/utils.py b/src/rai/rai/tools/ros/utils.py index 09bd73fa..51cad0e2 100644 --- a/src/rai/rai/tools/ros/utils.py +++ b/src/rai/rai/tools/ros/utils.py @@ -166,7 +166,7 @@ def get_transform( transform = None for _ in range(timeout * 10): - rclpy.spin_once(node, timeout_sec=0.1) + rclpy.spin_once(node, timeout_sec=0.2) if tf_buffer.can_transform(target_frame, source_frame, rclpy.time.Time()): transform = tf_buffer.lookup_transform( target_frame, source_frame, rclpy.time.Time() diff --git a/src/rai/rai/utils/ros.py b/src/rai/rai/utils/ros.py index 960e47fd..2279aa47 100644 --- a/src/rai/rai/utils/ros.py +++ b/src/rai/rai/utils/ros.py @@ -66,7 +66,7 @@ class NodeDiscovery: topics_and_types: Dict[str, str] = field(default_factory=dict) services_and_types: Dict[str, str] = field(default_factory=dict) actions_and_types: Dict[str, str] = field(default_factory=dict) - whitelist: Optional[List[str]] = field(default_factory=list) + allow_list: Optional[List[str]] = field(default_factory=list) def set(self, topics, services, actions): def to_dict(info: List[Tuple[str, List[str]]]) -> Dict[str, str]: @@ -75,8 +75,8 @@ def to_dict(info: List[Tuple[str, List[str]]]) -> Dict[str, str]: self.topics_and_types = to_dict(topics) self.services_and_types = to_dict(services) self.actions_and_types = to_dict(actions) - if self.whitelist is not None: - self.__filter(self.whitelist) + if self.allow_list is not None: + self.__filter(self.allow_list) def __filter(self, whitelist: List[str]): for d in [ diff --git a/src/rai_hmi/rai_hmi/base.py b/src/rai_hmi/rai_hmi/base.py index 9404d87a..64962727 100644 --- a/src/rai_hmi/rai_hmi/base.py +++ b/src/rai_hmi/rai_hmi/base.py @@ -80,6 +80,7 @@ class BaseHMINode(Node): You are the embodied AI robot, so please describe yourself and you action in 1st person. If you are asked about logs, or what was write or wrong about the mission, use get_mission_memory tool to get such information. If you are asked about interpration about camera image, add such mission as well + You are the emobdied AI. You are the robot. Say everything in 1st person. """ def __init__( diff --git a/src/rai_hmi/rai_hmi/basic_nav.py b/src/rai_hmi/rai_hmi/basic_nav.py new file mode 100644 index 00000000..34089bf0 --- /dev/null +++ b/src/rai_hmi/rai_hmi/basic_nav.py @@ -0,0 +1,193 @@ +from langchain_core.tools import render_text_description_and_args, tool + +import time +from nav2_msgs.action import DriveOnHeading +from typing import List +from rai.utils.ros import NodeDiscovery +import rclpy +import rclpy.node +import rclpy.action.client +import sensor_msgs.msg +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from rclpy.action.graph import get_action_names_and_types +import geometry_msgs.msg +import builtin_interfaces.msg +from rai.tools.ros.utils import get_transform +import sensor_msgs +import numpy as np + +import itertools + +class RosbotBasicNavigator(BasicNavigator): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.drive_on_heading_client = rclpy.action.client.ActionClient( + self, DriveOnHeading, 'drive_on_heading' + ) + + def drive_on_heading(self, target: float, speed: float, time_allowance: int): + self.debug("Waiting for 'DriveOnHeading' action server") + while not self.drive_on_heading_client.wait_for_server(timeout_sec=1.0): + self.info("'DriveOnHeading' action server not available, waiting...") + goal_msg = DriveOnHeading.Goal() + goal_msg.target = geometry_msgs.msg.Point(x=float(target)) + goal_msg.speed = speed + goal_msg.time_allowance = builtin_interfaces.msg.Duration(sec=time_allowance) + + self.info(f'DriveOnHeading {goal_msg.target.x} m at {goal_msg.speed} m/s....') + send_goal_future = self.drive_on_heading_client.send_goal_async(goal_msg, self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('DriveOnHeading request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True +if not rclpy.ok(): + rclpy.init() +navigator = RosbotBasicNavigator() +navigator._waitForNodeToActivate('/bt_navigator') + +tool_node = rclpy.create_node(node_name="test") +rclpy.spin_once(tool_node, timeout_sec=5.0) + +allowed_interfaces = [ + "/rosout", + "/camera/camera/aligned_depth_to_color/image_raw", + "/camera/camera/aligned_depth_to_color/camera_info", + "/camera/camera/color/image_raw", + "/camera/camera/color/camera_info", + "/led_strip", + "/backup", + "/drive_on_heading", + "/navigate_through_poses", + "/navigate_to_pose", + "/spin", +] + + +MAP_FRAME = 'map' + +def wait_for_finish(navigator: BasicNavigator) -> TaskResult: + while not navigator.isTaskComplete(): + time.sleep(1.0) + + return navigator.getResult() + + +# @tool +# def python_interpreter_tool(code: str): +# f""" +# Tool to generate the code using other tools in python +# """ +# return eval(code) + + +@tool +def get_ros2_interfaces() -> dict: + """ Get ros2 interfaces """ + return NodeDiscovery( + topics_and_types=tool_node.get_topic_names_and_types(), + services_and_types=tool_node.get_service_names_and_types(), + actions_and_types=get_action_names_and_types(tool_node), + allow_list=allowed_interfaces + ).dict() + +@tool +def wait_n_sec_tool(n_sec: int): + """ Wait for given amount of seconds """ + time.sleep(n_sec) + + return "done" + +@tool +def drive_on_heading(dist_to_travel: float, speed: float, time_allowance: int): + """ + Invokes the DriveOnHeading ROS 2 action server, which causes the robot to drive on the current heading by a specific displacement. It performs a linear translation by a given distance. The nav2_behaviors module implements the DriveOnHeading action server. + """ + if not navigator.isTaskComplete(): + return "Task is still running" + navigator.drive_on_heading(dist_to_travel, speed, time_allowance) + + return str(wait_for_finish(navigator)) + +@tool +def backup(dist_to_travel: float, speed: float, time_allowance: int): + """ + Invokes the BackUp ROS 2 action server, which causes the robot to back up by a specific displacement. It performs an linear translation by a given distance. + """ + navigator.backup(dist_to_travel, speed, time_allowance) + + return str(wait_for_finish(navigator)) + +@tool +def spin(angle_to_spin: float, time_allowance: int): + """ + Invokes the Spin ROS 2 action server, which is implemented by the nav2_behaviors module. It performs an in-place rotation by a given angle. + For spinning left use negative angle, for spinning right use positive angle. + Angle is in radians. + """ + navigator.spin(angle_to_spin, time_allowance) + + return str(wait_for_finish(navigator)) + +@tool +def go_to_pose(x: float ,y: float ,qx: float , qy: float, qz: float , qw: float): + """ + Navigate to specific pose in the /map variable + x, y - position + qx, qy, qz, qw - orientation + """ + pose = geometry_msgs.msg.PoseStamped() + pose.header.frame_id = MAP_FRAME + pose.header.stamp = navigator.get_clock().now().to_msg() + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.orientation.w = qw + pose.pose.orientation.x = qx + pose.pose.orientation.y = qy + pose.pose.orientation.z = qz + navigator.goToPose(pose) + return str(wait_for_finish(navigator)) + +@tool +def get_location(): + """ Returns robot's transform in the map frame """ + tf: geometry_msgs.msg.TransformStamped= get_transform(tool_node, "base_link", MAP_FRAME) + return str(tf) + +@tool +def led_strip(r: int,g: int,b: int): + """ Sets led strip to specific color """ + color = (r,g,b) + led_colors = np.full((1, 18, 3), color, dtype=np.uint8) + publisher = tool_node.create_publisher(sensor_msgs.msg.Image, "/led_strip", 10) + msg = sensor_msgs.msg.Image() + msg.header.stamp = tool_node.get_clock().now().to_msg() + msg.height = 1 + msg.width = 18 + msg.encoding = "rgb8" + msg.is_bigendian = False + msg.step = 18 * 3 + msg.data = led_colors.flatten().tolist() + publisher.publish(msg) + +@tool +def led_strip_array(colors: List[int]): + """ Sets entire led strip to specific pattern. Colors are in RGB order. Exactly 54 values are needed """ + + colors = list(itertools.islice(itertools.cycle(colors),54)) + + publisher = tool_node.create_publisher(sensor_msgs.msg.Image, "/led_strip", 10) + msg = sensor_msgs.msg.Image() + msg.header.stamp = tool_node.get_clock().now().to_msg() + msg.height = 1 + msg.width = 18 + msg.encoding = "rgb8" + msg.is_bigendian = False + msg.step = 18 * 3 + msg.data = colors + publisher.publish(msg) + diff --git a/src/rai_hmi/rai_hmi/basic_nav_async.py b/src/rai_hmi/rai_hmi/basic_nav_async.py new file mode 100644 index 00000000..d6a8c446 --- /dev/null +++ b/src/rai_hmi/rai_hmi/basic_nav_async.py @@ -0,0 +1,249 @@ +from langchain_core.tools import render_text_description_and_args, tool + +import time +from nav2_msgs.action import DriveOnHeading +from typing import List +from rai.tools.utils import wait_for_message +from rai.utils.ros import NodeDiscovery +import rclpy +import rclpy.node +import rclpy.action.client +import rclpy.qos +import sensor_msgs.msg +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from rclpy.action.graph import get_action_names_and_types +import geometry_msgs.msg +import builtin_interfaces.msg +from rai.tools.ros.utils import get_transform, import_message_from_str +import sensor_msgs +import numpy as np +from rai.node import describe_ros_image + +import itertools + +class RosbotBasicNavigator(BasicNavigator): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.drive_on_heading_client = rclpy.action.client.ActionClient( + self, DriveOnHeading, 'drive_on_heading' + ) + + def drive_on_heading(self, target: float, speed: float, time_allowance: int): + self.debug("Waiting for 'DriveOnHeading' action server") + while not self.drive_on_heading_client.wait_for_server(timeout_sec=1.0): + self.info("'DriveOnHeading' action server not available, waiting...") + goal_msg = DriveOnHeading.Goal() + goal_msg.target = geometry_msgs.msg.Point(x=float(target)) + goal_msg.speed = speed + goal_msg.time_allowance = builtin_interfaces.msg.Duration(sec=time_allowance) + + self.info(f'DriveOnHeading {goal_msg.target.x} m at {goal_msg.speed} m/s....') + send_goal_future = self.drive_on_heading_client.send_goal_async(goal_msg, self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('DriveOnHeading request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True +if not rclpy.ok(): + rclpy.init() +navigator = RosbotBasicNavigator() +navigator._waitForNodeToActivate('/bt_navigator') + +tool_node = rclpy.create_node(node_name="test") +rclpy.spin_once(tool_node, timeout_sec=5.0) + +allowed_interfaces = [ + "/rosout", + "/camera/camera/aligned_depth_to_color/image_raw", + "/camera/camera/aligned_depth_to_color/camera_info", + "/camera/camera/color/image_raw", + "/camera/camera/color/camera_info", + "/led_strip", + "/backup", + "/drive_on_heading", + "/navigate_through_poses", + "/navigate_to_pose", + "/spin", +] + + +MAP_FRAME = 'map' + +@tool +def wait_for_finish() -> TaskResult: + """ Block until the navigator is done and get the result """ + while not navigator.isTaskComplete(): + time.sleep(1.0) + + return navigator.getResult() + + +def get_topic_type(topic_name): + for topic, topic_type in tool_node.get_topic_names_and_types(): + if topic == topic_name: + return topic_type[0] + return None + +@tool +def is_nav_task_complete(): + """ Check if navigation task is complete """ + return navigator.isTaskComplete() + +@tool +def cancel_task(): + """ Cancel navigation task """ + navigator.cancelTask() + return "Task cancelled" + +@tool +def get_last_task_feedback(): + """ Get last navigation ros2 action feedback """ + return str(navigator.getFeedback()) + +@tool +def describe_camera_image(ros2_topic: str): + """ Describe camera image. it takes around 5-10 seconds to describe the image. Take it into account """ + msg_type_str = get_topic_type(ros2_topic) + print(msg_type_str) + if msg_type_str is None: + return f"Topic '{ros2_topic}' not found" + + msg_type = import_message_from_str(msg_type_str) + if msg_type is not sensor_msgs.msg.Image: + return f"Topic '{ros2_topic}' is not an sensor_msgs.msg.Image message, but {msg_type}" + + success, msg = wait_for_message( + msg_type, + tool_node, + ros2_topic, + qos_profile=rclpy.qos.qos_profile_sensor_data, + time_to_wait=5 + ) + if success: + return describe_ros_image(msg) + else: + return msg + +def get_ros2_interfaces() -> dict: + nd = NodeDiscovery(allow_list=allowed_interfaces) + nd.set( + tool_node.get_topic_names_and_types(), + tool_node.get_service_names_and_types(), + get_action_names_and_types(tool_node), + ) + return nd.dict() + +@tool +def get_ros2_interfaces_tool() -> dict: + """ Get ros2 interfaces """ + return get_ros2_interfaces() + +@tool +def wait_n_sec_tool(n_sec: int): + """ Wait for given amount of seconds """ + time.sleep(n_sec) + + return "done" + +@tool +def drive_on_heading(dist_to_travel: float, speed: float, time_allowance: int): + """ + Invokes the DriveOnHeading ROS 2 action server, which causes the robot to drive on the current heading by a specific displacement. It performs a linear translation by a given distance. The nav2_behaviors module implements the DriveOnHeading action server. + """ + if not navigator.isTaskComplete(): + return "Another navigation task is still running" + navigator.drive_on_heading(dist_to_travel, speed, time_allowance) + + return "Task started successfully" + +@tool +def backup(dist_to_travel: float, speed: float, time_allowance: int): + """ + Invokes the BackUp ROS 2 action server, which causes the robot to back up by a specific displacement. It performs an linear translation by a given distance. + """ + if not navigator.isTaskComplete(): + return "Another navigation task is still running" + navigator.backup(dist_to_travel, speed, time_allowance) + + return "Task started successfully" + +@tool +def spin(angle_to_spin: float, time_allowance: int): + """ + Invokes the Spin ROS 2 action server, which is implemented by the nav2_behaviors module. It performs an in-place rotation by a given angle. + For spinning left use positive angle, for spinning right use positive angle. + Angle is in radians. + """ + if not navigator.isTaskComplete(): + return "Another navigation task is still running" + navigator.spin(angle_to_spin, time_allowance) + + return "Task started successfully" + return str(wait_for_finish(navigator)) + +@tool +def go_to_pose(x: float ,y: float ,qx: float , qy: float, qz: float , qw: float): + """ + Navigate to specific pose in the /map variable + x, y - position + qx, qy, qz, qw - orientation + """ + if not navigator.isTaskComplete(): + return "Another navigation task is still running" + pose = geometry_msgs.msg.PoseStamped() + pose.header.frame_id = MAP_FRAME + pose.header.stamp = navigator.get_clock().now().to_msg() + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.orientation.w = qw + pose.pose.orientation.x = qx + pose.pose.orientation.y = qy + pose.pose.orientation.z = qz + navigator.goToPose(pose) + return "Task started successfully" + +@tool +def get_location(): + """ Returns robot's transform in the map frame """ + tf: geometry_msgs.msg.TransformStamped= get_transform(tool_node, "base_link", MAP_FRAME) + return str(tf) + +@tool +def led_strip(r: int,g: int,b: int): + """ Sets led strip to specific color """ + color = (r,g,b) + led_colors = np.full((1, 18, 3), color, dtype=np.uint8) + publisher = tool_node.create_publisher(sensor_msgs.msg.Image, "/led_strip", 10) + msg = sensor_msgs.msg.Image() + msg.header.stamp = tool_node.get_clock().now().to_msg() + msg.height = 1 + msg.width = 18 + msg.encoding = "rgb8" + msg.is_bigendian = False + msg.step = 18 * 3 + msg.data = led_colors.flatten().tolist() + publisher.publish(msg) + publisher.destroy() + +@tool +def led_strip_array(colors: List[int]): + """ Sets entire led strip to specific pattern. Colors are in RGB order. Exactly 54 values are needed """ + + colors = list(itertools.islice(itertools.cycle(colors),54)) + + publisher = tool_node.create_publisher(sensor_msgs.msg.Image, "/led_strip", 10) + msg = sensor_msgs.msg.Image() + msg.header.stamp = tool_node.get_clock().now().to_msg() + msg.height = 1 + msg.width = 18 + msg.encoding = "rgb8" + msg.is_bigendian = False + msg.step = 18 * 3 + msg.data = colors + publisher.publish(msg) + publisher.destroy() + From f10d63fff6fadd0127c97ed919e89e9dcc2d1d35 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Mon, 21 Oct 2024 21:15:51 +0200 Subject: [PATCH 54/58] fix basic nav tools --- src/rai_hmi/rai_hmi/basic_nav.py | 94 ++++++++++++++++++-------------- 1 file changed, 53 insertions(+), 41 deletions(-) diff --git a/src/rai_hmi/rai_hmi/basic_nav.py b/src/rai_hmi/rai_hmi/basic_nav.py index 34089bf0..e894bb69 100644 --- a/src/rai_hmi/rai_hmi/basic_nav.py +++ b/src/rai_hmi/rai_hmi/basic_nav.py @@ -1,28 +1,29 @@ -from langchain_core.tools import render_text_description_and_args, tool - +import itertools import time -from nav2_msgs.action import DriveOnHeading from typing import List -from rai.utils.ros import NodeDiscovery + +import builtin_interfaces.msg +import geometry_msgs.msg +import numpy as np import rclpy -import rclpy.node import rclpy.action.client +import rclpy.node +import sensor_msgs import sensor_msgs.msg +from langchain_core.tools import tool +from nav2_msgs.action import DriveOnHeading from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult from rclpy.action.graph import get_action_names_and_types -import geometry_msgs.msg -import builtin_interfaces.msg + from rai.tools.ros.utils import get_transform -import sensor_msgs -import numpy as np +from rai.utils.ros import NodeDiscovery -import itertools class RosbotBasicNavigator(BasicNavigator): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self.drive_on_heading_client = rclpy.action.client.ActionClient( - self, DriveOnHeading, 'drive_on_heading' + self, DriveOnHeading, "drive_on_heading" ) def drive_on_heading(self, target: float, speed: float, time_allowance: int): @@ -34,21 +35,25 @@ def drive_on_heading(self, target: float, speed: float, time_allowance: int): goal_msg.speed = speed goal_msg.time_allowance = builtin_interfaces.msg.Duration(sec=time_allowance) - self.info(f'DriveOnHeading {goal_msg.target.x} m at {goal_msg.speed} m/s....') - send_goal_future = self.drive_on_heading_client.send_goal_async(goal_msg, self._feedbackCallback) + self.info(f"DriveOnHeading {goal_msg.target.x} m at {goal_msg.speed} m/s....") + send_goal_future = self.drive_on_heading_client.send_goal_async( + goal_msg, self._feedbackCallback + ) rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() if not self.goal_handle.accepted: - self.error('DriveOnHeading request was rejected!') + self.error("DriveOnHeading request was rejected!") return False self.result_future = self.goal_handle.get_result_async() return True + + if not rclpy.ok(): rclpy.init() navigator = RosbotBasicNavigator() -navigator._waitForNodeToActivate('/bt_navigator') +navigator._waitForNodeToActivate("/bt_navigator") tool_node = rclpy.create_node(node_name="test") rclpy.spin_once(tool_node, timeout_sec=5.0) @@ -68,40 +73,40 @@ def drive_on_heading(self, target: float, speed: float, time_allowance: int): ] -MAP_FRAME = 'map' +MAP_FRAME = "map" + def wait_for_finish(navigator: BasicNavigator) -> TaskResult: while not navigator.isTaskComplete(): time.sleep(1.0) - return navigator.getResult() + return navigator.getResult() -# @tool -# def python_interpreter_tool(code: str): -# f""" -# Tool to generate the code using other tools in python -# """ -# return eval(code) +@tool +def get_ros2_interfaces_tool() -> dict: + """Get ros2 interfaces""" + return get_ros2_interfaces() -@tool def get_ros2_interfaces() -> dict: - """ Get ros2 interfaces """ + """Get ros2 interfaces""" return NodeDiscovery( topics_and_types=tool_node.get_topic_names_and_types(), services_and_types=tool_node.get_service_names_and_types(), actions_and_types=get_action_names_and_types(tool_node), - allow_list=allowed_interfaces + allow_list=allowed_interfaces, ).dict() + @tool def wait_n_sec_tool(n_sec: int): - """ Wait for given amount of seconds """ + """Wait for given amount of seconds""" time.sleep(n_sec) return "done" + @tool def drive_on_heading(dist_to_travel: float, speed: float, time_allowance: int): """ @@ -113,6 +118,7 @@ def drive_on_heading(dist_to_travel: float, speed: float, time_allowance: int): return str(wait_for_finish(navigator)) + @tool def backup(dist_to_travel: float, speed: float, time_allowance: int): """ @@ -122,6 +128,7 @@ def backup(dist_to_travel: float, speed: float, time_allowance: int): return str(wait_for_finish(navigator)) + @tool def spin(angle_to_spin: float, time_allowance: int): """ @@ -133,11 +140,12 @@ def spin(angle_to_spin: float, time_allowance: int): return str(wait_for_finish(navigator)) + @tool -def go_to_pose(x: float ,y: float ,qx: float , qy: float, qz: float , qw: float): - """ +def go_to_pose(x: float, y: float, qx: float, qy: float, qz: float, qw: float): + """ Navigate to specific pose in the /map variable - x, y - position + x, y - position qx, qy, qz, qw - orientation """ pose = geometry_msgs.msg.PoseStamped() @@ -152,18 +160,22 @@ def go_to_pose(x: float ,y: float ,qx: float , qy: float, qz: float , qw: float) navigator.goToPose(pose) return str(wait_for_finish(navigator)) + @tool def get_location(): - """ Returns robot's transform in the map frame """ - tf: geometry_msgs.msg.TransformStamped= get_transform(tool_node, "base_link", MAP_FRAME) + """Returns robot's transform in the map frame""" + tf: geometry_msgs.msg.TransformStamped = get_transform( + tool_node, "base_link", MAP_FRAME + ) return str(tf) + @tool -def led_strip(r: int,g: int,b: int): - """ Sets led strip to specific color """ - color = (r,g,b) +def led_strip(r: int, g: int, b: int): + """Sets led strip to specific color""" + color = (r, g, b) led_colors = np.full((1, 18, 3), color, dtype=np.uint8) - publisher = tool_node.create_publisher(sensor_msgs.msg.Image, "/led_strip", 10) + publisher = tool_node.create_publisher(sensor_msgs.msg.Image, "/led_strip", 10) msg = sensor_msgs.msg.Image() msg.header.stamp = tool_node.get_clock().now().to_msg() msg.height = 1 @@ -174,13 +186,14 @@ def led_strip(r: int,g: int,b: int): msg.data = led_colors.flatten().tolist() publisher.publish(msg) + @tool def led_strip_array(colors: List[int]): - """ Sets entire led strip to specific pattern. Colors are in RGB order. Exactly 54 values are needed """ + """Sets entire led strip to specific pattern. Colors are in RGB order. Exactly 54 values are needed""" - colors = list(itertools.islice(itertools.cycle(colors),54)) + colors = list(itertools.islice(itertools.cycle(colors), 54)) - publisher = tool_node.create_publisher(sensor_msgs.msg.Image, "/led_strip", 10) + publisher = tool_node.create_publisher(sensor_msgs.msg.Image, "/led_strip", 10) msg = sensor_msgs.msg.Image() msg.header.stamp = tool_node.get_clock().now().to_msg() msg.height = 1 @@ -188,6 +201,5 @@ def led_strip_array(colors: List[int]): msg.encoding = "rgb8" msg.is_bigendian = False msg.step = 18 * 3 - msg.data = colors + msg.data = colors publisher.publish(msg) - From b746478bcb4dd8d15dd360ab5b6f5b5cfbcc0071 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Mon, 21 Oct 2024 21:16:12 +0200 Subject: [PATCH 55/58] add files with async nav tools --- src/rai_hmi/rai_hmi/text_hmi-async.py | 315 ++++++++++++++++++++++++++ 1 file changed, 315 insertions(+) create mode 100644 src/rai_hmi/rai_hmi/text_hmi-async.py diff --git a/src/rai_hmi/rai_hmi/text_hmi-async.py b/src/rai_hmi/rai_hmi/text_hmi-async.py new file mode 100644 index 00000000..a4df722e --- /dev/null +++ b/src/rai_hmi/rai_hmi/text_hmi-async.py @@ -0,0 +1,315 @@ +# Copyright (C) 2024 Robotec.AI +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +import base64 +import io +import logging +import sys +from queue import Queue +from typing import Dict, Optional, cast + +import rclpy +import std_msgs.msg +import streamlit as st +from langchain_core.messages import ( + AIMessage, + BaseMessage, + HumanMessage, + SystemMessage, + ToolCall, + ToolMessage, +) +from langchain_core.tools import render_text_description_and_args +from langchain_openai.chat_models import ChatOpenAI +from PIL import Image +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy + +from rai.agents.conversational_agent import create_conversational_agent +from rai.messages import HumanMultimodalMessage +from rai.node import RaiAsyncToolsNode +from rai.tools.ros.native_actions import GetCameraImage +from rai.utils.artifacts import get_stored_artifacts +from rai.utils.model_initialization import get_llm_model +from rai_hmi.base import BaseHMINode + +# from rai_hmi.basic_nav import get_ros2_interfaces, drive_on_heading, backup, spin, go_to_pose, get_location, led_strip, led_strip_array, wait_n_sec_tool +from rai_hmi.basic_nav_async import ( + backup, + cancel_task, + describe_camera_image, + drive_on_heading, + get_last_task_feedback, + get_location, + get_ros2_interfaces, + get_ros2_interfaces_tool, + go_to_pose, + is_nav_task_complete, + led_strip, + led_strip_array, + spin, + wait_n_sec_tool, +) + +logger = logging.getLogger(__name__) + +st.set_page_config(page_title="LangChain Chat App", page_icon="๐Ÿฆœ") +robot_description_package = sys.argv[1] if len(sys.argv) > 1 else None + +if robot_description_package: + st.title(f"{robot_description_package.replace('_whoami', '')} chat app") +else: + st.title("ROS 2 Chat App") + logging.warning( + "No robot_description_package provided. Some functionalities may not work." + ) + + +@st.cache_resource +def initialize_ros_node(robot_description_package: Optional[str]): + node = BaseHMINode( + f"{robot_description_package}_hmi_node", + robot_description_package=robot_description_package, + queue=Queue(), + ) + + return node + + +def decode_base64_into_image(base64_image: str): + image = Image.open(io.BytesIO(base64.b64decode(base64_image))) + return image + + +@st.cache_resource +def initialize_agent(_node: BaseHMINode): + llm = ChatOpenAI( + temperature=0.5, + model="gpt-4o", + streaming=True, + ) + rai_node = RaiAsyncToolsNode(node_name="rai_tool_node") + rclpy.spin_once(rai_node, timeout_sec=1.0) + tools = [ + get_ros2_interfaces_tool, + drive_on_heading, + backup, + spin, + go_to_pose, + get_location, + led_strip, + led_strip_array, + wait_n_sec_tool, + is_nav_task_complete, + cancel_task, + get_last_task_feedback, + describe_camera_image, + GetCameraImage(node=rai_node), + ] + tools_desc = ( + f"\nThese are available tools:\n{render_text_description_and_args(tools)}" + ) + interfaces_desc = f"\nThese are available interfaces:\n{get_ros2_interfaces()}" + rules = """ + Before every navigation action check you location, to verify if nav2 action took expected result. Actions might fail, but make navigation progress. You need to check that and adapt + While the navigation task is running you can do other things. You can also check the task status. + When you are asked to perform a task carefully, you can split navigation tasks into a couple of smaller ones. + < rule> don't decide to respond to the user until you are sure that the navigation task is completed + """ + system_prompt = _node.system_prompt + tools_desc + interfaces_desc + rules + _node.system_prompt = system_prompt + print(f"Agents system prompt: {system_prompt=}") + agent = create_conversational_agent( + llm, + _node.tools + tools, + system_prompt, + logger=_node.get_logger(), + ) + return agent + + +def initialize_session_memory(system_prompt: str = ""): + if "memory" not in st.session_state: + st.session_state.memory = [SystemMessage(content=system_prompt)] + if "tool_calls" not in st.session_state: + st.session_state.tool_calls = {} + + +def convert_langchain_message_to_streamlit_message( + message: BaseMessage, +) -> Dict[str, str]: + message.content = cast(str, message.content) # type: ignore + if isinstance(message, HumanMessage): + return {"type": "user", "avatar": "๐Ÿง‘โ€๐Ÿ’ป", "content": message.content} + elif isinstance(message, AIMessage): + return {"type": "bot", "avatar": "๐Ÿค–", "content": message.content} + elif isinstance(message, ToolMessage): + return {"type": "bot", "avatar": "๐Ÿ› ๏ธ", "content": message.content} + else: + return {"type": "unknown", "avatar": "โ“", "content": message.content} + + +def handle_history_message(message: BaseMessage): + message.content = cast(str, message.content) # type: ignore + if isinstance(message, HumanMessage): + if isinstance( + message, HumanMultimodalMessage + ): # we do not handle user's images + return + user_chat_obj = st.chat_message("user", avatar="๐Ÿง‘โ€๐Ÿ’ป") + user_chat_obj.markdown(message.content) + elif isinstance(message, AIMessage): + if message.content == "": + return + bot_chat_obj = st.chat_message("bot", avatar="๐Ÿค–") + bot_chat_obj.markdown(message.content) + elif isinstance(message, ToolMessage): + tool_call = st.session_state.tool_calls[message.tool_call_id] + label = tool_call.name + " status: " + status = "โœ…" if message.status == "success" else "โŒ" + tool_chat_obj = st.expander(label=label + status).chat_message( + "bot", avatar="๐Ÿ› ๏ธ" + ) + with tool_chat_obj: + st.markdown(message.content) + artifacts = get_stored_artifacts(message.tool_call_id) + for artifact in artifacts: + if "images" in artifact: + base_64_image = artifact["images"][0] + image = decode_base64_into_image(base_64_image) + st.image(image) + elif isinstance(message, SystemMessage): + return # we do not handle system messages + else: + raise ValueError("Unknown message type") + + +def publish_response_to_ros(node: Node, response: str): + llm = get_llm_model("simple_model") + response = llm.invoke( + [ + SystemMessage( + content=( + "You are an experienced reporter deployed on a autonomous robot. " # type: ignore + "Your task is to summarize the message in a way that is very easy to understand" + "for the user. Also the message will be played using tts, so please make it concise " + "and don't include any special characters." + "Do not use markdown formatting. Keep it short and concise. If the message is empty, please return empty string." + ) + ), + HumanMessage(content=response), + ] + ).content + node.get_logger().info(f"LLM shortened the response to:\n{response}") + + reliable_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_ALL, + ) + publisher = node.create_publisher(std_msgs.msg.String, "/to_human", reliable_qos) + msg = std_msgs.msg.String() + msg.data = response + publisher.publish(msg) + publisher.destroy() + + +if __name__ == "__main__": + with st.spinner("Initializing ROS 2 node..."): + node = initialize_ros_node(robot_description_package) + agent = initialize_agent(_node=node) + initialize_session_memory(system_prompt=node.system_prompt) + + status = { + "robot_database": node.faiss_index is not None, + "system_prompt": node.system_prompt == "", + } + with st.expander("System status", expanded=False): + st.json(status) + + for message in st.session_state.memory: + handle_history_message(message) + + if prompt := st.chat_input("What is your question?"): + user_chat_obj = st.chat_message("user", avatar="๐Ÿง‘โ€๐Ÿ’ป") + user_chat_obj.markdown(prompt) + st.session_state.memory.append(HumanMessage(content=prompt)) + + message_placeholder = st.container() + with message_placeholder: + with st.spinner("Thinking..."): + tool_placeholders = {} + for state in agent.stream( + {"messages": st.session_state.memory}, + config={"recursion_limit": 500}, + ): + node_name = list(state.keys())[0] + if node_name == "thinker": + last_message = state[node_name]["messages"][-1] + if last_message.content: + st_message = convert_langchain_message_to_streamlit_message( + last_message + ) + st.chat_message( + st_message["type"], avatar=st_message["avatar"] + ).markdown(st_message["content"]) + publish_response_to_ros(node, st_message["content"]) + + called_tools = last_message.tool_calls + for tool_call in called_tools: + tool_call = cast(ToolCall, tool_call) + st.session_state.tool_calls[tool_call["id"]] = tool_call + with st.expander(f"Tool call: {tool_call['name']}"): + st.markdown(f"Arguments: {tool_call['args']}") + tool_placeholders[tool_call["id"]] = st.empty() + + elif node_name == "tools": + tool_messages = [] + last_ai_msg_idx = 0 + for message in state[node_name]["messages"]: + if isinstance(message, AIMessage): + last_ai_msg_idx = state[node_name]["messages"].index( + message + ) + + for message in state[node_name]["messages"][ + last_ai_msg_idx + 1 : # noqa: E203 + ]: + if message.type == "tool": + st.session_state.tool_calls[message.tool_call_id] = ( + message + ) + if message.tool_call_id in tool_placeholders: + st_message = ( + convert_langchain_message_to_streamlit_message( + message + ) + ) + with tool_placeholders[message.tool_call_id]: + st.chat_message( + st_message["type"], + avatar=st_message["avatar"], + ).markdown(st_message["content"]) + artifacts = get_stored_artifacts( + message.tool_call_id + ) + for artifact in artifacts: + if "images" in artifact: + base_64_image = artifact["images"][0] + image = decode_base64_into_image( + base_64_image + ) + st.image(image) + else: + break From 93e1ec4553bf27e3628c0de182a066f40fd2b9ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Mon, 21 Oct 2024 23:36:37 +0200 Subject: [PATCH 56/58] Improve docs and prompt --- src/rai_hmi/rai_hmi/base.py | 2 ++ src/rai_hmi/rai_hmi/basic_nav.py | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/rai_hmi/rai_hmi/base.py b/src/rai_hmi/rai_hmi/base.py index 64962727..b79a195d 100644 --- a/src/rai_hmi/rai_hmi/base.py +++ b/src/rai_hmi/rai_hmi/base.py @@ -78,6 +78,8 @@ class BaseHMINode(Node): They will be done by another agent resposible for communication with the robotic's stack. You are the embodied AI robot, so please describe yourself and you action in 1st person. + For navigation actions please check you location before and after every tool call. + You are obligated to verify if these actions were successfull. If you are asked about logs, or what was write or wrong about the mission, use get_mission_memory tool to get such information. If you are asked about interpration about camera image, add such mission as well You are the emobdied AI. You are the robot. Say everything in 1st person. diff --git a/src/rai_hmi/rai_hmi/basic_nav.py b/src/rai_hmi/rai_hmi/basic_nav.py index e894bb69..d48856f4 100644 --- a/src/rai_hmi/rai_hmi/basic_nav.py +++ b/src/rai_hmi/rai_hmi/basic_nav.py @@ -133,7 +133,7 @@ def backup(dist_to_travel: float, speed: float, time_allowance: int): def spin(angle_to_spin: float, time_allowance: int): """ Invokes the Spin ROS 2 action server, which is implemented by the nav2_behaviors module. It performs an in-place rotation by a given angle. - For spinning left use negative angle, for spinning right use positive angle. + For spinning left use positive angle, for spinning right use negative angle. Angle is in radians. """ navigator.spin(angle_to_spin, time_allowance) From 85db3c2b753118675c74f3ee42c6da25a9e2b0ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 22 Oct 2024 02:32:19 +0200 Subject: [PATCH 57/58] add 1 col setup for streamlit + cleanup after merge --- src/rai/rai/node.py | 1 - src/rai_hmi/rai_hmi/text_hmi-1col.py | 306 ++++++++++++++ src/rai_hmi/rai_hmi/text_hmi.py | 570 +++++++++++---------------- 3 files changed, 529 insertions(+), 348 deletions(-) create mode 100644 src/rai_hmi/rai_hmi/text_hmi-1col.py diff --git a/src/rai/rai/node.py b/src/rai/rai/node.py index 3c3843af..4aa5d911 100644 --- a/src/rai/rai/node.py +++ b/src/rai/rai/node.py @@ -293,7 +293,6 @@ def _cancel_task(self): rclpy.spin_until_future_complete(self, future) return True -<<<<<<< HEAD class RaiBaseNode(Node): def __init__( diff --git a/src/rai_hmi/rai_hmi/text_hmi-1col.py b/src/rai_hmi/rai_hmi/text_hmi-1col.py new file mode 100644 index 00000000..1f1d4bff --- /dev/null +++ b/src/rai_hmi/rai_hmi/text_hmi-1col.py @@ -0,0 +1,306 @@ +# Copyright (C) 2024 Robotec.AI +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +import base64 +import io +import logging +import sys +from queue import Queue +from typing import Dict, Optional, cast + +import rclpy +import std_msgs.msg +import streamlit as st +from langchain_core.messages import ( + AIMessage, + BaseMessage, + HumanMessage, + SystemMessage, + ToolCall, + ToolMessage, +) +from langchain_core.tools import render_text_description_and_args +from langchain_openai.chat_models import ChatOpenAI +from PIL import Image +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy + +from rai.agents.conversational_agent import create_conversational_agent +from rai.messages import HumanMultimodalMessage +from rai.node import RaiAsyncToolsNode +from rai.tools.ros.native_actions import GetCameraImage +from rai.utils.artifacts import get_stored_artifacts +from rai.utils.model_initialization import get_llm_model +from rai_hmi.base import BaseHMINode +from rai_hmi.basic_nav import ( + backup, + drive_on_heading, + get_location, + get_ros2_interfaces, + get_ros2_interfaces_tool, + go_to_pose, + led_strip, + led_strip_array, + spin, + wait_n_sec_tool, +) + +logger = logging.getLogger(__name__) + +st.set_page_config(page_title="LangChain Chat App", page_icon="๐Ÿฆœ") +robot_description_package = sys.argv[1] if len(sys.argv) > 1 else None + +if robot_description_package: + st.title(f"{robot_description_package.replace('_whoami', '')} chat app") +else: + st.title("ROS 2 Chat App") + logging.warning( + "No robot_description_package provided. Some functionalities may not work." + ) + + +@st.cache_resource +def initialize_ros_node(robot_description_package: Optional[str]): + node = BaseHMINode( + f"{robot_description_package}_hmi_node", + robot_description_package=robot_description_package, + queue=Queue(), + ) + + return node + + +def decode_base64_into_image(base64_image: str): + image = Image.open(io.BytesIO(base64.b64decode(base64_image))) + return image + + +@st.cache_resource +def initialize_agent(_node: BaseHMINode): + llm = ChatOpenAI( + temperature=0.5, + model="gpt-4o", + streaming=True, + ) + rai_node = RaiAsyncToolsNode(node_name="rai_tool_node") + rclpy.spin_once(rai_node, timeout_sec=1.0) + tools = [ + get_ros2_interfaces_tool, + drive_on_heading, + backup, + spin, + go_to_pose, + get_location, + led_strip, + led_strip_array, + wait_n_sec_tool, + GetCameraImage(node=rai_node), + ] + tools_desc = ( + f"\nThese are available tools:\n{render_text_description_and_args(tools)}" + ) + interfaces_desc = f"\nThese are available interfaces:\n{get_ros2_interfaces()}" + rules = """ + Before every navigation action check you location, to verify if nav2 action took expected result. Actions might fail, but make navigation progress. You need to check that and adapt + While the navigation task is running you can do other things. You can also check the task status. + When you are asked to perform a task carefully, you can split navigation tasks into a couple of smaller ones. + < rule> don't decide to respond to the user until you are sure that the navigation task is completed + """ + system_prompt = _node.system_prompt + tools_desc + interfaces_desc + rules + _node.system_prompt = system_prompt + print(f"Agents system prompt: {system_prompt=}") + agent = create_conversational_agent( + llm, + _node.tools + tools, + system_prompt, + logger=_node.get_logger(), + ) + return agent + + +def initialize_session_memory(system_prompt: str = ""): + if "memory" not in st.session_state: + st.session_state.memory = [SystemMessage(content=system_prompt)] + if "tool_calls" not in st.session_state: + st.session_state.tool_calls = {} + + +def convert_langchain_message_to_streamlit_message( + message: BaseMessage, +) -> Dict[str, str]: + message.content = cast(str, message.content) # type: ignore + if isinstance(message, HumanMessage): + return {"type": "user", "avatar": "๐Ÿง‘โ€๐Ÿ’ป", "content": message.content} + elif isinstance(message, AIMessage): + return {"type": "bot", "avatar": "๐Ÿค–", "content": message.content} + elif isinstance(message, ToolMessage): + return {"type": "bot", "avatar": "๐Ÿ› ๏ธ", "content": message.content} + else: + return {"type": "unknown", "avatar": "โ“", "content": message.content} + + +def handle_history_message(message: BaseMessage): + message.content = cast(str, message.content) # type: ignore + if isinstance(message, HumanMessage): + if isinstance( + message, HumanMultimodalMessage + ): # we do not handle user's images + return + user_chat_obj = st.chat_message("user", avatar="๐Ÿง‘โ€๐Ÿ’ป") + user_chat_obj.markdown(message.content) + elif isinstance(message, AIMessage): + if message.content == "": + return + bot_chat_obj = st.chat_message("bot", avatar="๐Ÿค–") + bot_chat_obj.markdown(message.content) + elif isinstance(message, ToolMessage): + tool_call = st.session_state.tool_calls[message.tool_call_id] + label = tool_call.name + " status: " + status = "โœ…" if message.status == "success" else "โŒ" + tool_chat_obj = st.expander(label=label + status).chat_message( + "bot", avatar="๐Ÿ› ๏ธ" + ) + with tool_chat_obj: + st.markdown(message.content) + artifacts = get_stored_artifacts(message.tool_call_id) + for artifact in artifacts: + if "images" in artifact: + base_64_image = artifact["images"][0] + image = decode_base64_into_image(base_64_image) + st.image(image) + elif isinstance(message, SystemMessage): + return # we do not handle system messages + else: + raise ValueError("Unknown message type") + + +def publish_response_to_ros(node: Node, response: str): + llm = get_llm_model("simple_model") + response = llm.invoke( + [ + SystemMessage( + content=( + "You are an experienced reporter deployed on a autonomous robot. " # type: ignore + "Your task is to summarize the message in a way that is very easy to understand" + "for the user. Also the message will be played using tts, so please make it concise " + "and don't include any special characters." + "Do not use markdown formatting. Keep it short and concise. If the message is empty, please return empty string." + "The message have to be in first person!" + ) + ), + HumanMessage(content=response), + ] + ).content + node.get_logger().info(f"LLM shortened the response to:\n{response}") + + reliable_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_ALL, + ) + publisher = node.create_publisher(std_msgs.msg.String, "/to_human", reliable_qos) + msg = std_msgs.msg.String() + msg.data = response + publisher.publish(msg) + publisher.destroy() + + +if __name__ == "__main__": + with st.spinner("Initializing ROS 2 node..."): + node = initialize_ros_node(robot_description_package) + agent = initialize_agent(_node=node) + initialize_session_memory(system_prompt=node.system_prompt) + + status = { + "robot_database": node.faiss_index is not None, + "system_prompt": node.system_prompt == "", + } + with st.expander("System status", expanded=False): + st.json(status) + + for message in st.session_state.memory: + handle_history_message(message) + + if prompt := st.chat_input("What is your question?"): + user_chat_obj = st.chat_message("user", avatar="๐Ÿง‘โ€๐Ÿ’ป") + user_chat_obj.markdown(prompt) + st.session_state.memory.append(HumanMessage(content=prompt)) + + message_placeholder = st.container() + with message_placeholder: + with st.spinner("Thinking..."): + tool_placeholders = {} + for state in agent.stream( + {"messages": st.session_state.memory}, + config={"recursion_limit": 500}, + ): + node_name = list(state.keys())[0] + if node_name == "thinker": + last_message = state[node_name]["messages"][-1] + if last_message.content: + st_message = convert_langchain_message_to_streamlit_message( + last_message + ) + st.chat_message( + st_message["type"], avatar=st_message["avatar"] + ).markdown(st_message["content"]) + publish_response_to_ros(node, st_message["content"]) + + called_tools = last_message.tool_calls + for tool_call in called_tools: + tool_call = cast(ToolCall, tool_call) + st.session_state.tool_calls[tool_call["id"]] = tool_call + with st.expander(f"Tool call: {tool_call['name']}"): + st.markdown(f"Arguments: {tool_call['args']}") + tool_placeholders[tool_call["id"]] = st.empty() + + elif node_name == "tools": + tool_messages = [] + last_ai_msg_idx = 0 + for message in state[node_name]["messages"]: + if isinstance(message, AIMessage): + last_ai_msg_idx = state[node_name]["messages"].index( + message + ) + + for message in state[node_name]["messages"][ + last_ai_msg_idx + 1 : # noqa: E203 + ]: + if message.type == "tool": + st.session_state.tool_calls[message.tool_call_id] = ( + message + ) + if message.tool_call_id in tool_placeholders: + st_message = ( + convert_langchain_message_to_streamlit_message( + message + ) + ) + with tool_placeholders[message.tool_call_id]: + st.chat_message( + st_message["type"], + avatar=st_message["avatar"], + ).markdown(st_message["content"]) + artifacts = get_stored_artifacts( + message.tool_call_id + ) + for artifact in artifacts: + if "images" in artifact: + base_64_image = artifact["images"][0] + image = decode_base64_into_image( + base_64_image + ) + st.image(image) + else: + break diff --git a/src/rai_hmi/rai_hmi/text_hmi.py b/src/rai_hmi/rai_hmi/text_hmi.py index 761b3cf4..1f1d4bff 100644 --- a/src/rai_hmi/rai_hmi/text_hmi.py +++ b/src/rai_hmi/rai_hmi/text_hmi.py @@ -9,20 +9,18 @@ # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language goveself.rning permissions and +# See the License for the specific language governing permissions and # limitations under the License. # import base64 -import enum import io import logging import sys -import time -from pathlib import Path -from pprint import pformat from queue import Queue -from typing import Dict, List, Optional, Tuple, cast +from typing import Dict, Optional, cast +import rclpy +import std_msgs.msg import streamlit as st from langchain_core.messages import ( AIMessage, @@ -32,116 +30,147 @@ ToolCall, ToolMessage, ) +from langchain_core.tools import render_text_description_and_args +from langchain_openai.chat_models import ChatOpenAI from PIL import Image -from pydantic import BaseModel from rclpy.node import Node -from streamlit.delta_generator import DeltaGenerator +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from rai.agents.conversational_agent import create_conversational_agent from rai.messages import HumanMultimodalMessage from rai.node import RaiAsyncToolsNode +from rai.tools.ros.native_actions import GetCameraImage from rai.utils.artifacts import get_stored_artifacts -from rai_hmi.agent import initialize_agent +from rai.utils.model_initialization import get_llm_model from rai_hmi.base import BaseHMINode -from rai_hmi.chat_msgs import EMOJIS, MissionMessage -from rai_hmi.ros import initialize_ros_nodes -from rai_hmi.text_hmi_utils import Memory - -logging.basicConfig( - level=logging.INFO, format="%(asctime)s - %(name)s - %(levelname)s - %(message)s" +from rai_hmi.basic_nav import ( + backup, + drive_on_heading, + get_location, + get_ros2_interfaces, + get_ros2_interfaces_tool, + go_to_pose, + led_strip, + led_strip_array, + spin, + wait_n_sec_tool, ) -logger = logging.getLogger(__name__) - -st.set_page_config(page_title="LangChain Chat App", page_icon="๐Ÿฆœ", layout="wide") -MODEL = "gpt-4o" -MAX_DISPLAY = 5 - - -class AppLayout(enum.Enum): - ONE_COLUMN = 1 - TWO_COLUMNS = 2 +logger = logging.getLogger(__name__) +st.set_page_config(page_title="LangChain Chat App", page_icon="๐Ÿฆœ") +robot_description_package = sys.argv[1] if len(sys.argv) > 1 else None -# NOTE(boczekbartek): ONE_COLUMN is not stable yet -LAYOUT = AppLayout.TWO_COLUMNS +if robot_description_package: + st.title(f"{robot_description_package.replace('_whoami', '')} chat app") +else: + st.title("ROS 2 Chat App") + logging.warning( + "No robot_description_package provided. Some functionalities may not work." + ) -# ---------- Cached Resources ---------- @st.cache_resource -def parse_args() -> Tuple[Optional[str], Optional[Path]]: - robot_description_package = sys.argv[1] if len(sys.argv) > 1 else None - whitelist = Path(sys.argv[2]) if len(sys.argv) > 2 else None - return robot_description_package, whitelist - +def initialize_ros_node(robot_description_package: Optional[str]): + node = BaseHMINode( + f"{robot_description_package}_hmi_node", + robot_description_package=robot_description_package, + queue=Queue(), + ) -@st.cache_resource -def initialize_memory() -> Memory: - return Memory() + return node -@st.cache_resource -def initialize_agent_streamlit( - _hmi_node: BaseHMINode, _rai_node: RaiAsyncToolsNode, _memory: Memory -): - return initialize_agent(_hmi_node, _rai_node, _memory) +def decode_base64_into_image(base64_image: str): + image = Image.open(io.BytesIO(base64.b64decode(base64_image))) + return image @st.cache_resource -def initialize_mission_queue() -> Queue: - return Queue() +def initialize_agent(_node: BaseHMINode): + llm = ChatOpenAI( + temperature=0.5, + model="gpt-4o", + streaming=True, + ) + rai_node = RaiAsyncToolsNode(node_name="rai_tool_node") + rclpy.spin_once(rai_node, timeout_sec=1.0) + tools = [ + get_ros2_interfaces_tool, + drive_on_heading, + backup, + spin, + go_to_pose, + get_location, + led_strip, + led_strip_array, + wait_n_sec_tool, + GetCameraImage(node=rai_node), + ] + tools_desc = ( + f"\nThese are available tools:\n{render_text_description_and_args(tools)}" + ) + interfaces_desc = f"\nThese are available interfaces:\n{get_ros2_interfaces()}" + rules = """ + Before every navigation action check you location, to verify if nav2 action took expected result. Actions might fail, but make navigation progress. You need to check that and adapt + While the navigation task is running you can do other things. You can also check the task status. + When you are asked to perform a task carefully, you can split navigation tasks into a couple of smaller ones. + < rule> don't decide to respond to the user until you are sure that the navigation task is completed + """ + system_prompt = _node.system_prompt + tools_desc + interfaces_desc + rules + _node.system_prompt = system_prompt + print(f"Agents system prompt: {system_prompt=}") + agent = create_conversational_agent( + llm, + _node.tools + tools, + system_prompt, + logger=_node.get_logger(), + ) + return agent -@st.cache_resource -def initialize_ros_nodes_streamlit( - _feedbacks_queue: Queue, - robot_description_package: Optional[str], - ros2_whitelist: Optional[Path], -): - return initialize_ros_nodes( - _feedbacks_queue, robot_description_package, ros2_whitelist - ) +def initialize_session_memory(system_prompt: str = ""): + if "memory" not in st.session_state: + st.session_state.memory = [SystemMessage(content=system_prompt)] + if "tool_calls" not in st.session_state: + st.session_state.tool_calls = {} -def display_agent_message( - message, # TODO(boczekbartek): add typhint - tool_chat_obj: Optional[DeltaGenerator] = None, - no_expand: bool = False, - tool_call: Optional[ToolCall] = None, -): - """ - Display LLM message in streamlit UI. +def convert_langchain_message_to_streamlit_message( + message: BaseMessage, +) -> Dict[str, str]: + message.content = cast(str, message.content) # type: ignore + if isinstance(message, HumanMessage): + return {"type": "user", "avatar": "๐Ÿง‘โ€๐Ÿ’ป", "content": message.content} + elif isinstance(message, AIMessage): + return {"type": "bot", "avatar": "๐Ÿค–", "content": message.content} + elif isinstance(message, ToolMessage): + return {"type": "bot", "avatar": "๐Ÿ› ๏ธ", "content": message.content} + else: + return {"type": "unknown", "avatar": "โ“", "content": message.content} - Args: - message: The message to display - tool_chat_obj: Pre-existing Streamlit object for the tool message. - no_expand: Skip expanders due - Streamlit does not support nested expanders. - tool_call: The tool call associated with the ToolMessage. - """ +def handle_history_message(message: BaseMessage): message.content = cast(str, message.content) # type: ignore - if not message.content: - return # Tool messages might not have any content, skip displying them if isinstance(message, HumanMessage): if isinstance( message, HumanMultimodalMessage ): # we do not handle user's images return - st.chat_message("user", avatar=EMOJIS.human).markdown(message.content) + user_chat_obj = st.chat_message("user", avatar="๐Ÿง‘โ€๐Ÿ’ป") + user_chat_obj.markdown(message.content) elif isinstance(message, AIMessage): - st.chat_message("bot", avatar=EMOJIS.bot).markdown(message.content) + if message.content == "": + return + bot_chat_obj = st.chat_message("bot", avatar="๐Ÿค–") + bot_chat_obj.markdown(message.content) elif isinstance(message, ToolMessage): - if tool_call is None: - raise ValueError("`tool_call` argument is required for ToolMessage.") - + tool_call = st.session_state.tool_calls[message.tool_call_id] label = tool_call.name + " status: " - status = EMOJIS.success if message.status == "success" else EMOJIS.failure - if not tool_chat_obj: - if not no_expand: - tool_chat_obj = st.expander(label=label + status).chat_message( - "bot", avatar=EMOJIS.tool - ) - else: - tool_chat_obj = st.chat_message("bot", avatar=EMOJIS.tool) + status = "โœ…" if message.status == "success" else "โŒ" + tool_chat_obj = st.expander(label=label + status).chat_message( + "bot", avatar="๐Ÿ› ๏ธ" + ) with tool_chat_obj: st.markdown(message.content) artifacts = get_stored_artifacts(message.tool_call_id) @@ -152,279 +181,126 @@ def display_agent_message( st.image(image) elif isinstance(message, SystemMessage): return # we do not handle system messages - elif isinstance(message, MissionMessage): - logger.info("Displaying mission message") - with st.expander(label=message.STATUS): - avatar, content = message.render_steamlit() - st.chat_message("bot", avatar=avatar).markdown(content) else: raise ValueError("Unknown message type") -# ---------- Streamlit Application ---------- -class SystemStatus(BaseModel): - robot_database: bool - system_prompt: bool - - -class Empty(object): - def __enter__(self): - pass - - def __exit__(self, *args): - pass - - -class Layout: - def __init__(self, robot_description_package) -> None: - if robot_description_package: - st.title(f"{robot_description_package.replace('_whoami', '')} chat app") - else: - st.title("ROS 2 Chat App") - logging.warning( - "No robot_description_package provided. Some functionalities may not work." - ) - self.n_columns = 1 - self.tool_placeholders = dict() - self.chat_column = Empty - - def draw_app_status_expander(self, system_status: SystemStatus): - with st.expander("System status", expanded=False): - st.json(system_status.model_dump()) - - def draw(self, system_status: SystemStatus): - self.draw_app_status_expander(system_status) - - def create_tool_expanders(self, tool_calls: List[ToolCall]): - for tool_call in tool_calls: - tool_call = cast(ToolCall, tool_call) - with st.expander(f"Tool call: {tool_call['name']}"): - st.markdown(f"Arguments: {tool_call['args']}") - self.tool_placeholders[tool_call["id"]] = st.empty() - - def write_tool_message(self, msg: ToolMessage, tool_call: ToolCall): - display_agent_message( - msg, self.tool_placeholders[msg.tool_call_id], tool_call=tool_call - ) - - def write_chat_msg(self, msg: BaseMessage): - display_agent_message(msg) - - def show_chat(self, history, tool_calls: Dict[str, ToolCall]): - self.show_history(history, tool_calls) - - def show_history(self, history, tool_calls): - def display(message, no_expand=False): - if isinstance(message, ToolMessage): - display_agent_message( - message, - no_expand=no_expand, - tool_call=tool_calls[message.tool_call_id], +def publish_response_to_ros(node: Node, response: str): + llm = get_llm_model("simple_model") + response = llm.invoke( + [ + SystemMessage( + content=( + "You are an experienced reporter deployed on a autonomous robot. " # type: ignore + "Your task is to summarize the message in a way that is very easy to understand" + "for the user. Also the message will be played using tts, so please make it concise " + "and don't include any special characters." + "Do not use markdown formatting. Keep it short and concise. If the message is empty, please return empty string." + "The message have to be in first person!" ) - else: - display_agent_message(message, no_expand=no_expand) - - for message in history: - display(message) - - -class TwoColLayout(Layout): - """App has two columns: chat + robot state""" - - def __init__(self, robot_description_package) -> None: - super().__init__(robot_description_package) - self.n_columns = 2 - - def draw(self, system_status: SystemStatus): - self.draw_app_status_expander(system_status) - self.draw_columns() - - def draw_columns(self): - self.chat_column, self.mission_column = st.columns(self.n_columns) - - def create_tool_expanders(self, tool_calls: List[ToolCall]): - with self.chat_column: - super().create_tool_expanders(tool_calls) - - def write_tool_message(self, msg: ToolMessage, tool_call: ToolCall): - with self.chat_column: - super().write_tool_message(msg, tool_call) - - def write_chat_msg(self, msg: BaseMessage): - with self.chat_column: - super().write_chat_msg(msg) - - def write_mission_msg(self, msg: MissionMessage): - with self.mission_column: - logger.info(f'Mission said: "{msg}"') - display_agent_message(msg) - - def show_chat(self, history, tool_calls: Dict[str, ToolCall]): - with self.chat_column: - super().show_chat(history, tool_calls) - - def show_mission(self, history, tool_calls: Dict[str, ToolCall]): - with self.mission_column: - self.show_history(history, tool_calls) - - -def get_layout(robot_description_package) -> Layout: - if LAYOUT == AppLayout.TWO_COLUMNS: - return TwoColLayout(robot_description_package) - else: - return Layout(robot_description_package) - - -class Chat: - def __init__(self, memory: Memory, layout: Layout) -> None: - self.memory = memory - self.layout = layout - - def user(self, txt): - logger.info(f'User said: "{txt}"') - msg = HumanMessage(content=txt) - self.memory.chat_memory.append(msg) - self.layout.write_chat_msg(msg) - - def bot(self, msg): - logger.info(f'Bot said: "{msg}"') - self.memory.chat_memory.append(msg) - self.layout.write_chat_msg(msg) - - def tool(self, msg): - logger.info(f'Tool said: "{msg}"') - self.memory.chat_memory.append(msg) - tool_call = self.memory.tool_calls[msg.tool_call_id] - self.layout.write_tool_message(msg, tool_call) - - def mission(self, msg: MissionMessage): - logger.info(f'Mission said: "{msg}"') - self.memory.add_mission(msg) - if isinstance(self.layout, TwoColLayout): - self.layout.write_mission_msg(msg) - - -class Agent: - def __init__(self, hmi_node: Node, rai_node: Node, memory) -> None: - self.memory = memory - self.agent = initialize_agent_streamlit(hmi_node, rai_node, self.memory) - - def stream(self): - # Copy, because agent's memory != streamlit app memory. App memory is used to - # recreate the page, agent might manipulate it's history to perform the task. - # In simplest case it adds thinker message to the state. - messages = self.memory.chat_memory.copy() - logger.info(f"Sending messages:\n{pformat(messages)}") - - return self.agent.stream({"messages": messages}) - - -class StreamlitApp: - def __init__(self, robot_description_package, ros2_whitelist: Path) -> None: - self.logger = logging.getLogger(self.__class__.__name__) - - self.robot_description_package = robot_description_package - - self.layout = get_layout(self.robot_description_package) - self.memory = initialize_memory() - self.chat = Chat(self.memory, self.layout) - - self.mission_queue = initialize_mission_queue() - self.hmi_ros_node, self.rai_ros_node = self.initialize_node( - self.mission_queue, robot_description_package, ros2_whitelist - ) - self.agent = Agent(self.hmi_ros_node, self.rai_ros_node, self.memory) - - def update_mission(self): - while True: - if self.mission_queue.empty(): - time.sleep(0.5) - continue - logger.info("Got new mission update!") - msg = self.mission_queue.get() - self.chat.mission(msg) - - def run(self): - system_status = self.get_system_status() - self.layout.draw(system_status) - - self.recreate_from_memory() - - st.chat_input( - "What is your question?", on_submit=self.prompt_callback, key="prompt" - ) - - self.update_mission() - - def get_system_status(self) -> SystemStatus: - return SystemStatus( - robot_database=self.hmi_ros_node.faiss_index is not None, - system_prompt=self.hmi_ros_node.system_prompt != "", - ) - - def initialize_node( - self, feedbacks_queue, robot_description_package, ros2_whitelist - ): - self.logger.info("Initializing ROS 2 node...") - with st.spinner("Initializing ROS 2 nodes..."): - hmi_node, rai_node = initialize_ros_nodes_streamlit( - feedbacks_queue, robot_description_package, ros2_whitelist - ) - self.logger.info("ROS 2 node initialized") - return hmi_node, rai_node + ), + HumanMessage(content=response), + ] + ).content + node.get_logger().info(f"LLM shortened the response to:\n{response}") + + reliable_qos = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_ALL, + ) + publisher = node.create_publisher(std_msgs.msg.String, "/to_human", reliable_qos) + msg = std_msgs.msg.String() + msg.data = response + publisher.publish(msg) + publisher.destroy() - def recreate_from_memory(self): - """ - Recreates the page from the memory. It is because Streamlit reloads entire page every widget action. - See: https://docs.streamlit.io/get-started/fundamentals/main-concepts - """ - self.layout.show_chat(self.memory.chat_memory, self.memory.tool_calls) - if isinstance(self.layout, TwoColLayout): - self.layout.show_mission(self.memory.mission_memory, self.memory.tool_calls) - def prompt_callback(self): - prompt = st.session_state.prompt - self.chat.user(prompt) +if __name__ == "__main__": + with st.spinner("Initializing ROS 2 node..."): + node = initialize_ros_node(robot_description_package) + agent = initialize_agent(_node=node) + initialize_session_memory(system_prompt=node.system_prompt) + + status = { + "robot_database": node.faiss_index is not None, + "system_prompt": node.system_prompt == "", + } + with st.expander("System status", expanded=False): + st.json(status) + + for message in st.session_state.memory: + handle_history_message(message) + + if prompt := st.chat_input("What is your question?"): + user_chat_obj = st.chat_message("user", avatar="๐Ÿง‘โ€๐Ÿ’ป") + user_chat_obj.markdown(prompt) + st.session_state.memory.append(HumanMessage(content=prompt)) message_placeholder = st.container() - with self.layout.chat_column: - with message_placeholder: - with st.spinner("Thinking..."): - for state in self.agent.stream(): - # logger.info(f"State:\n{pformat(state)}") - node_name = list(state.keys())[0] - if node_name == "thinker": - last_message = state[node_name]["messages"][-1] - self.chat.bot(last_message) - self.memory.register_tool_calls(last_message.tool_calls) - self.layout.create_tool_expanders(last_message.tool_calls) - - elif node_name == "tools": - last_ai_msg_idx = 0 - for message in state[node_name]["messages"]: - if isinstance(message, AIMessage): - last_ai_msg_idx = state[node_name][ - "messages" - ].index(message) - - for message in state[node_name]["messages"][ - last_ai_msg_idx + 1 : # noqa: E203 - ]: - if message.type == "tool": - self.memory.tool_calls[message.tool_call_id] = ( - message + with message_placeholder: + with st.spinner("Thinking..."): + tool_placeholders = {} + for state in agent.stream( + {"messages": st.session_state.memory}, + config={"recursion_limit": 500}, + ): + node_name = list(state.keys())[0] + if node_name == "thinker": + last_message = state[node_name]["messages"][-1] + if last_message.content: + st_message = convert_langchain_message_to_streamlit_message( + last_message + ) + st.chat_message( + st_message["type"], avatar=st_message["avatar"] + ).markdown(st_message["content"]) + publish_response_to_ros(node, st_message["content"]) + + called_tools = last_message.tool_calls + for tool_call in called_tools: + tool_call = cast(ToolCall, tool_call) + st.session_state.tool_calls[tool_call["id"]] = tool_call + with st.expander(f"Tool call: {tool_call['name']}"): + st.markdown(f"Arguments: {tool_call['args']}") + tool_placeholders[tool_call["id"]] = st.empty() + + elif node_name == "tools": + tool_messages = [] + last_ai_msg_idx = 0 + for message in state[node_name]["messages"]: + if isinstance(message, AIMessage): + last_ai_msg_idx = state[node_name]["messages"].index( + message + ) + + for message in state[node_name]["messages"][ + last_ai_msg_idx + 1 : # noqa: E203 + ]: + if message.type == "tool": + st.session_state.tool_calls[message.tool_call_id] = ( + message + ) + if message.tool_call_id in tool_placeholders: + st_message = ( + convert_langchain_message_to_streamlit_message( + message + ) ) - self.chat.tool(message) - else: - break - - -def decode_base64_into_image(base64_image: str): - image = Image.open(io.BytesIO(base64.b64decode(base64_image))) - return image - - -if __name__ == "__main__": - robot_description_package, ros2_whitelist = parse_args() - app = StreamlitApp(robot_description_package, ros2_whitelist) - app.run() + with tool_placeholders[message.tool_call_id]: + st.chat_message( + st_message["type"], + avatar=st_message["avatar"], + ).markdown(st_message["content"]) + artifacts = get_stored_artifacts( + message.tool_call_id + ) + for artifact in artifacts: + if "images" in artifact: + base_64_image = artifact["images"][0] + image = decode_base64_into_image( + base_64_image + ) + st.image(image) + else: + break From 3fe6eb628bd4cb86e871e898252640c70cd4f631 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Boczek?= Date: Tue, 22 Oct 2024 07:11:03 +0200 Subject: [PATCH 58/58] fixes in streamlit --- src/rai/rai/node.py | 2 +- src/rai_hmi/rai_hmi/text_hmi-1col.py | 4 ++-- src/rai_hmi/rai_hmi/text_hmi.py | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/rai/rai/node.py b/src/rai/rai/node.py index 4aa5d911..ac25b1ba 100644 --- a/src/rai/rai/node.py +++ b/src/rai/rai/node.py @@ -311,7 +311,7 @@ def __init__( self.DISCOVERY_FREQ, self.discovery, ) - self.ros_discovery_info = NodeDiscovery(whitelist=whitelist) + self.ros_discovery_info = NodeDiscovery(allow_list=whitelist) self.discovery() self.qos_profile = QoSProfile( history=HistoryPolicy.KEEP_LAST, diff --git a/src/rai_hmi/rai_hmi/text_hmi-1col.py b/src/rai_hmi/rai_hmi/text_hmi-1col.py index 1f1d4bff..fd026b8f 100644 --- a/src/rai_hmi/rai_hmi/text_hmi-1col.py +++ b/src/rai_hmi/rai_hmi/text_hmi-1col.py @@ -38,7 +38,7 @@ from rai.agents.conversational_agent import create_conversational_agent from rai.messages import HumanMultimodalMessage -from rai.node import RaiAsyncToolsNode +from rai.node import RaiBaseNode from rai.tools.ros.native_actions import GetCameraImage from rai.utils.artifacts import get_stored_artifacts from rai.utils.model_initialization import get_llm_model @@ -93,7 +93,7 @@ def initialize_agent(_node: BaseHMINode): model="gpt-4o", streaming=True, ) - rai_node = RaiAsyncToolsNode(node_name="rai_tool_node") + rai_node = RaiBaseNode(node_name="rai_tool_node") rclpy.spin_once(rai_node, timeout_sec=1.0) tools = [ get_ros2_interfaces_tool, diff --git a/src/rai_hmi/rai_hmi/text_hmi.py b/src/rai_hmi/rai_hmi/text_hmi.py index 1f1d4bff..fd026b8f 100644 --- a/src/rai_hmi/rai_hmi/text_hmi.py +++ b/src/rai_hmi/rai_hmi/text_hmi.py @@ -38,7 +38,7 @@ from rai.agents.conversational_agent import create_conversational_agent from rai.messages import HumanMultimodalMessage -from rai.node import RaiAsyncToolsNode +from rai.node import RaiBaseNode from rai.tools.ros.native_actions import GetCameraImage from rai.utils.artifacts import get_stored_artifacts from rai.utils.model_initialization import get_llm_model @@ -93,7 +93,7 @@ def initialize_agent(_node: BaseHMINode): model="gpt-4o", streaming=True, ) - rai_node = RaiAsyncToolsNode(node_name="rai_tool_node") + rai_node = RaiBaseNode(node_name="rai_tool_node") rclpy.spin_once(rai_node, timeout_sec=1.0) tools = [ get_ros2_interfaces_tool,