Devel/add nav2 tools

.gitignore

@@ -2,6 +2,7 @@ turtlebo3_agent/.env
 __pycache__/
 turtlebo3_agent/tools/navigation_tools.py
 turtlebo3_agent/tools/api_tools.py
+command.txt
 
 devel/
 logs/

launch/tb3_agent.launch.py

@@ -14,7 +14,7 @@ def generate_launch_description():
     )
     gazebo_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(gazebo_launch_dir, "empty_world.launch.py")
+            os.path.join(gazebo_launch_dir, "turtlebot3_world.launch.py")
         )
     )
 

turtlebot3_agent/tb3_node.py

@@ -10,12 +10,17 @@
 import time
 
 import rclpy
+import tf2_geometry_msgs
+import tf2_ros
 from cv_bridge import CvBridge
-from geometry_msgs.msg import Twist
+from geometry_msgs.msg import PointStamped, PoseStamped, TransformStamped, Twist
+from nav2_msgs.action import NavigateToPose
 from nav_msgs.msg import Odometry
+from rclpy.action import ActionClient
 from rclpy.node import Node
 from sensor_msgs.msg import Image, LaserScan
-from transforms3d.euler import quat2euler
+from tf2_ros import TransformException
+from transforms3d.euler import euler2quat, quat2euler
 
 from turtlebot3_agent.utils import normalize_angle
 
@@ -24,7 +29,7 @@
 ROTATION_ERROR_THRESHOLD = 0.017  # radians
 DISTANCE_ERROR_THRESHOLD = 0.02  # meters
 TOPIC_CMD_VEL = "/cmd_vel"
-TOPIC_ODOMETRY = "/odometry/filtered"
+TOPIC_ODOMETRY = "/odom"
 TOPIC_IMG = "/camera/image_raw"
 TOPIC_SCAN = "/scan"
 PUBLISH_RATE = 0.05
@@ -50,6 +55,13 @@ def __init__(self):
         self.create_subscription(LaserScan, TOPIC_SCAN, self.scan_callback, 10)
         self.create_subscription(Image, TOPIC_IMG, self.image_callback, 10)
 
+        # Nav2 Action Client
+        self._nav_client = ActionClient(self, NavigateToPose, "/navigate_to_pose")
+
+        # TF2 Buffer and Listener for coordinate transformations - この2行を追加
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+
         # Sensor data
         self.scan = None
         self.odom = None
@@ -242,3 +254,142 @@ def move_non_linear(
 
         self._stop_robot()
         return True
+
+    def navigate_to_pose(self, x, y, yaw, timeout_sec=30.0):
+        """
+        Navigate to a specific pose using Nav2 NavigateToPose action.
+
+        Args:
+            x (float): Goal X coordinate in meters (map frame)
+            y (float): Goal Y coordinate in meters (map frame)
+            yaw (float): Goal yaw angle in radians
+            timeout_sec (float): Maximum time to wait for navigation completion
+
+        Returns:
+            bool: True if navigation completed successfully, False otherwise
+        """
+        # Wait for action server
+        if not self._nav_client.wait_for_server(timeout_sec=5.0):
+            self.get_logger().error("NavigateToPose action server not available.")
+            return False
+
+        # Create goal message
+        goal_msg = NavigateToPose.Goal()
+        goal_msg.pose = PoseStamped()
+        goal_msg.pose.header.frame_id = "map"
+        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
+
+        # Set position
+        goal_msg.pose.pose.position.x = float(x)
+        goal_msg.pose.pose.position.y = float(y)
+        goal_msg.pose.pose.position.z = 0.0
+
+        # Convert yaw to quaternion
+        quat = euler2quat(0, 0, yaw)  # (roll, pitch, yaw)
+        goal_msg.pose.pose.orientation.w = float(quat[0])
+        goal_msg.pose.pose.orientation.x = float(quat[1])
+        goal_msg.pose.pose.orientation.y = float(quat[2])
+        goal_msg.pose.pose.orientation.z = float(quat[3])
+
+        self.get_logger().info(f"Sending navigation goal: x={x}, y={y}, yaw={yaw}")
+
+        # Send goal
+        send_goal_future = self._nav_client.send_goal_async(goal_msg)
+
+        # Wait for goal acceptance
+        rclpy.spin_until_future_complete(self, send_goal_future, timeout_sec=5.0)
+
+        if not send_goal_future.done():
+            self.get_logger().error("Failed to send goal within timeout")
+            return False
+
+        goal_handle = send_goal_future.result()
+        if not goal_handle.accepted:
+            self.get_logger().error("Goal rejected by NavigateToPose server")
+            return False
+
+        self.get_logger().info("Goal accepted, waiting for result...")
+
+        # Wait for result
+        result_future = goal_handle.get_result_async()
+        rclpy.spin_until_future_complete(self, result_future, timeout_sec=timeout_sec)
+
+        if not result_future.done():
+            self.get_logger().error("Navigation timed out")
+            # Cancel the goal
+            cancel_future = goal_handle.cancel_goal_async()
+            rclpy.spin_until_future_complete(self, cancel_future, timeout_sec=2.0)
+            return False
+
+        result = result_future.result()
+        if result.status == 4:  # SUCCEEDED
+            self.get_logger().info("Navigation completed successfully!")
+            return True
+        else:
+            self.get_logger().error(f"Navigation failed with status: {result.status}")
+            return False
+
+    def transform_odom_to_map(self, odom_x, odom_y, odom_yaw):
+        """
+        Transform coordinates from odom frame to map frame.
+
+        Args:
+            odom_x (float): X coordinate in odom frame (meters)
+            odom_y (float): Y coordinate in odom frame (meters)
+            odom_yaw (float): Yaw angle in odom frame (radians)
+
+        Returns:
+            tuple: (map_x, map_y, map_yaw) coordinates in map frame
+
+        Raises:
+            Exception: If transform lookup fails
+        """
+        try:
+            # Wait for transform to be available
+            self.tf_buffer.can_transform(
+                "map",
+                "odom",
+                rclpy.time.Time(),
+                timeout=rclpy.duration.Duration(seconds=1.0),
+            )
+
+            # Get the transform from odom to map
+            transform = self.tf_buffer.lookup_transform(
+                "map", "odom", rclpy.time.Time()
+            )
+
+            # Transform the position
+            point_odom = PointStamped()
+            point_odom.header.frame_id = "odom"
+            point_odom.header.stamp = self.get_clock().now().to_msg()
+            point_odom.point.x = odom_x
+            point_odom.point.y = odom_y
+            point_odom.point.z = 0.0
+
+            point_map = tf2_geometry_msgs.do_transform_point(point_odom, transform)
+
+            # Transform the orientation
+            # Extract rotation from transform
+            trans_quat = transform.transform.rotation
+            trans_euler = quat2euler(
+                [trans_quat.w, trans_quat.x, trans_quat.y, trans_quat.z]
+            )
+            trans_yaw = trans_euler[2]  # Z-axis rotation (yaw)
+
+            # Add the yaw angles
+            map_yaw = odom_yaw + trans_yaw
+
+            # Normalize the angle to [-pi, pi]
+            while map_yaw > math.pi:
+                map_yaw -= 2 * math.pi
+            while map_yaw < -math.pi:
+                map_yaw += 2 * math.pi
+
+            return point_map.point.x, point_map.point.y, map_yaw
+
+        except TransformException as ex:
+            self.get_logger().error(f"Could not transform from odom to map: {ex}")
+            raise Exception(f"Transform failed: {ex}")
+        except Exception as ex:
+            self.get_logger().error(f"Unexpected error during transformation: {ex}")
+            raise Exception(f"Transform failed: {ex}")

turtlebot3_agent/tools/all_tools.py

@@ -10,7 +10,9 @@
 from turtlebot3_agent.tools.motion_tools import (
     make_move_linear_tool,
     make_move_non_linear_tool,
+    make_navigate_to_pose_tool,
     make_rotate_tool,
+    make_transform_odom_to_map_tool,
 )
 
 # from turtlebot3_agent.tools.navigation_tools import make_navigate_to_goal_tool
@@ -32,17 +34,9 @@ def make_all_tools(node) -> list:
         list: List of tools available to the agent
     """
     return [
-        calculate_absolute_angle,
-        calculate_distance_and_relative_angle,
-        calculate_euclidean_distance,
-        calculate_relative_angle,
-        calculate_relative_angle_from_yaw,
-        degrees_to_radians,
-        calculate_distance_and_relative_angle,
+        make_transform_odom_to_map_tool(node),
         make_start_camera_display_tool(node),
-        make_move_linear_tool(node),
-        make_move_non_linear_tool(node),
-        make_rotate_tool(node),
+        make_navigate_to_pose_tool(node),
         make_get_turtle_pose_tool(node),
         make_get_lidar_scan_tool(node),
     ]

turtlebot3_agent/tools/motion_tools.py

@@ -1,8 +1,11 @@
 #!/usr/bin/env python3
 
+import math
+
 from langchain.tools import StructuredTool
 from pydantic import BaseModel, Field
 from pydantic.v1 import BaseModel, Field
+from transforms3d.euler import euler2quat
 
 
 def make_move_linear_tool(node):
@@ -130,3 +133,102 @@ def inner(inputs: MoveCurveInput) -> str:
         Use this to make the robot move in arcs or spirals.
         """,
     )
+
+
+def make_navigate_to_pose_tool(node):
+    """
+    Create a tool that uses Nav2 NavigateToPose action to navigate to a goal position.
+
+    Args:
+        node: The TB3Agent node instance
+
+    Returns:
+        StructuredTool: A LangChain tool for navigation
+    """
+
+    class NavigateInput(BaseModel):
+        x: float = Field(description="Goal X coordinate in meters (map frame)")
+        y: float = Field(description="Goal Y coordinate in meters (map frame)")
+        yaw: float = Field(description="Goal yaw angle in radians")
+
+    def inner(inputs: NavigateInput) -> str:
+        success = node.navigate_to_pose(x=inputs.x, y=inputs.y, yaw=inputs.yaw)
+
+        if success:
+            if not node.wait_for_pose(timeout=0.5):
+                return "Navigation completed, but pose data not received yet."
+            else:
+                map_x, map_y, map_yaw = node.transform_odom_to_map(
+                    odom_x=node.x, odom_y=node.y, odom_yaw=node.yaw
+                )
+                return (
+                    f"Navigation completed successfully! "
+                    f"You are now at position ({round(map_x, 2)}, {round(map_y, 2)}) in the map frame"
+                    f"with a yaw angle of {round(map_yaw, 3)} radians."
+                )
+        else:
+            return "Navigation failed. The goal may be unreachable or the action server is not available."
+
+    return StructuredTool.from_function(
+        func=inner,
+        name="navigate_to_pose",
+        description="""
+        Navigate to a specific goal position using Nav2 path planning and obstacle avoidance.
+
+        Args:
+        - x (float): Goal X coordinate in meters (map frame)
+        - y (float): Goal Y coordinate in meters (map frame)  
+        - yaw (float): Goal yaw angle in radians
+
+        This tool uses the Nav2 navigation stack to plan a path and navigate to the goal
+        while avoiding obstacles. It's more robust than direct movement commands for
+        navigating in complex environments with obstacles.
+        """,
+    )
+
+
+def make_transform_odom_to_map_tool(node):
+    """
+    Create a tool that transforms coordinates from odom frame to map frame.
+
+    Args:
+        node: The TB3Agent node instance
+
+    Returns:
+        StructuredTool: A LangChain tool for coordinate transformation
+    """
+
+    class TransformInput(BaseModel):
+        odom_x: float = Field(description="X coordinate in pose frame (meters)")
+        odom_y: float = Field(description="Y coordinate in pose frame (meters)")
+        odom_yaw: float = Field(description="Yaw angle in pose frame (radians)")
+
+    def inner(inputs: TransformInput) -> str:
+        try:
+            map_x, map_y, map_yaw = node.transform_odom_to_map(
+                odom_x=inputs.odom_x, odom_y=inputs.odom_y, odom_yaw=inputs.odom_yaw
+            )
+            return (
+                f"Transformed coordinates from pose to map frame:\n"
+                f"Odom: ({round(inputs.odom_x, 2)}, {round(inputs.odom_y, 2)}, {round(inputs.odom_yaw, 2)} rad)\n"
+                f"Map:  ({round(map_x, 2)}, {round(map_y, 2)}, {round(map_yaw, 2)} rad)"
+            )
+        except Exception as e:
+            return f"Failed to transform coordinates: {str(e)}"
+
+    return StructuredTool.from_function(
+        func=inner,
+        name="transform_odom_to_map",
+        description="""
+        Transform coordinates from pose frame to map frame using TF2.
+
+        Args:
+        - odom_x (float): X coordinate in robot's current frame (meters)
+        - odom_y (float): Y coordinate in robot's current frame (meters)
+        - odom_yaw (float): Yaw angle in robot's current frame (radians)
+
+        Returns the equivalent coordinates in the map frame. This is useful for
+        converting robot's current position from odometry to map coordinates
+        for navigation planning.
+        """,
+    )