feat: husarion the descriptor wip

Author: maciejmajek

examples/explore_and_describe.py

@@ -79,18 +79,18 @@ def main():
         SystemMessage(
             content="You are an autonomous agent. Your main goal is to fulfill the user's requests. "
             "Do not make assumptions about the environment you are currently in. "
-            "Use the tooling provided to gather information about the environment."
+            "Use the tooling provided to gather information about the environment. Remember to list available topics. "
         ),
         HumanMultimodalMessage(
             content="Describe your surroundings and gather more information as needed. "
             "Move to explore further, aiming for clear areas near the robot (red arrow). Make sure to describe the area during movement."
             "Utilize available methods to obtain the map and identify relevant data streams. "
-            "Before starting the exploration, find out what kind of tooling is available and based on that plan your exploration."
+            "Before starting the exploration, find out what kind of tooling is available and based on that plan your exploration. For description, use the available tooling."
         ),
-        AgentLoop(stop_action=FinishTool().__class__.__name__, stop_iters=10),
+        AgentLoop(stop_action=FinishTool().__class__.__name__, stop_iters=50),
     ]
 
-    llm = ChatBedrock(model_id="anthropic.claude-3-5-sonnet-20240620-v1:0", region_name="us-west-2")  # type: ignore
+    llm = ChatBedrock(model_id="anthropic.claude-3-5-sonnet-20240620-v1:0", region_name="us-east-1")  # type: ignore
     runner = ScenarioRunner(scenario, llm=llm, tools=tools, llm_type="bedrock")
     runner.run()
     runner.save_to_html()

src/rai/communication/ros_communication.py

@@ -7,9 +7,16 @@
 import rclpy
 import rclpy.qos
 from cv_bridge import CvBridge
+from rclpy.duration import Duration
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 from rclpy.node import Node
-from rclpy.qos import QoSProfile
+from rclpy.qos import (
+    QoSDurabilityPolicy,
+    QoSHistoryPolicy,
+    QoSLivelinessPolicy,
+    QoSProfile,
+    QoSReliabilityPolicy,
+)
 from rclpy.signals import SignalHandlerGuardCondition
 from rclpy.utilities import timeout_sec_to_nsec
 from sensor_msgs.msg import Image
@@ -92,7 +99,16 @@ def grab_message(self) -> Any:
 
         node = rclpy.create_node(self.__class__.__name__ + "_node")  # type: ignore
         qos_profile = rclpy.qos.qos_profile_sensor_data
-
+        if self.topic == "/map":
+            qos_profile = QoSProfile(
+                reliability=QoSReliabilityPolicy.RELIABLE,
+                history=QoSHistoryPolicy.KEEP_ALL,
+                durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+                lifespan=Duration(seconds=0),
+                deadline=Duration(seconds=0),
+                liveliness=QoSLivelinessPolicy.AUTOMATIC,
+                liveliness_lease_duration=Duration(seconds=0),
+            )
         success, msg = wait_for_message(
             self.message_type,
             node,

src/rai/scenario_engine/scenario_engine.py

@@ -151,6 +151,12 @@ def _run(self, scenario: ScenarioType):
                     f"Looping agent actions until {msg.stop_action}. Max {msg.stop_iters} loops."
                 )
                 for _ in range(msg.stop_iters):
+                    if self.history[-1].type == "ai":
+                        self.history.append(
+                            HumanMessage(
+                                content="Thank you. Please continue your mision with tools."
+                            )
+                        )
                     ai_msg = cast(
                         AIMessage,
                         self.llm_with_tools.invoke(

src/rai/tools/ros/cli_tools.py

@@ -43,7 +43,7 @@ class Ros2TopicTool(BaseTool):
     def _run(self, command: str):
         """Executes the specified ROS2 topic command."""
         result = subprocess.run(
-            f"ros2 topic {command}", shell=True, capture_output=True
+            f"ros2 topic {command} -v", shell=True, capture_output=True
         )
         return result
 
@@ -139,9 +139,9 @@ class SetGoalPoseTool(BaseTool):
     def _run(self, topic: str, x: float, y: float):
         """Sets the goal pose for the robot."""
 
-        cmd = (
+        _ = (
             f"ros2 topic pub {topic} geometry_msgs/PoseStamped "
             f'\'{{header: {{stamp: {{sec: 0, nanosec: 0}}, frame_id: "map"}}, '
             f"pose: {{position: {{x: {x}, y: {y}, z: {0.0}}}}}}}' --once"
         )
-        return subprocess.run(cmd, shell=True)
+        return "done"  # subprocess.run(cmd, shell=True)

src/rai/tools/ros/tools.py

@@ -103,7 +103,7 @@ class GetOccupancyGridTool(BaseTool):
 
     name: str = "GetOccupancyGridTool"
     description: str = (
-        "A tool for getting the current map as an image with the robot's position marked on it."
+        "A tool for getting the current map as an image with the robot's position marked on it. The coordinates are written at the edges of the image. Make sure to read them correctly if you are planning on using the coordinates to set a new goal pose."
     )
 
     args_schema: Type[GetOccupancyGridToolInput] = GetOccupancyGridToolInput
@@ -119,7 +119,7 @@ def _postprocess_msg(self, map_msg: OccupancyGrid, transform: TransformStamped):
 
         # Convert the OccupancyGrid values to grayscale image (0-255)
         # the final image shape should be at most (1000x1000), scale to fit
-        scale = 1000 / max(width, height)
+        scale = 1500 / max(width, height)
         width = int(width * scale)
         height = int(height * scale)
         data = cv2.resize(data, (width, height), interpolation=cv2.INTER_NEAREST)
@@ -164,7 +164,7 @@ def _postprocess_msg(self, map_msg: OccupancyGrid, transform: TransformStamped):
         )
 
         robot_y = cast(
-            float, (transform.transform.translation.y - origin_position.y / resolution)
+            float, (transform.transform.translation.y - origin_position.y) / resolution
         )
 
         _, _, yaw = euler_from_quaternion(
@@ -193,7 +193,7 @@ def _postprocess_msg(self, map_msg: OccupancyGrid, transform: TransformStamped):
                     transform.transform.rotation.w,  # type: ignore
                 ]
             )
-            arrow_length = 10
+            arrow_length = 20
             arrow_end_x = int(robot_x + arrow_length * np.cos(yaw))
             arrow_end_y = int(robot_y + arrow_length * np.sin(yaw))
             cv2.arrowedLine(