diff --git a/.gitignore b/.gitignore index b76c8ea..af4cc77 100644 --- a/.gitignore +++ b/.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/ diff --git a/launch/tb3_agent.launch.py b/launch/tb3_agent.launch.py index 873e506..2728974 100755 --- a/launch/tb3_agent.launch.py +++ b/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") ) ) diff --git a/turtlebot3_agent/tb3_node.py b/turtlebot3_agent/tb3_node.py index 8827e82..7b37709 100755 --- a/turtlebot3_agent/tb3_node.py +++ b/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, 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}") diff --git a/turtlebot3_agent/tools/all_tools.py b/turtlebot3_agent/tools/all_tools.py index d214aeb..51f20df 100644 --- a/turtlebot3_agent/tools/all_tools.py +++ b/turtlebot3_agent/tools/all_tools.py @@ -1,16 +1,7 @@ # from turtlebot3_agent.tools.api_tools import get_information_from_internet -from turtlebot3_agent.tools.math_tools import ( - calculate_absolute_angle, - calculate_distance_and_relative_angle, - calculate_euclidean_distance, - calculate_relative_angle, - calculate_relative_angle_from_yaw, - degrees_to_radians, -) from turtlebot3_agent.tools.motion_tools import ( - make_move_linear_tool, - make_move_non_linear_tool, - make_rotate_tool, + make_navigate_to_pose_tool, + make_transform_odom_to_map_tool, ) # from turtlebot3_agent.tools.navigation_tools import make_navigate_to_goal_tool @@ -32,17 +23,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), ] diff --git a/turtlebot3_agent/tools/motion_tools.py b/turtlebot3_agent/tools/motion_tools.py index df3248b..f5bdca7 100644 --- a/turtlebot3_agent/tools/motion_tools.py +++ b/turtlebot3_agent/tools/motion_tools.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 + from langchain.tools import StructuredTool -from pydantic import BaseModel, Field from pydantic.v1 import BaseModel, Field @@ -130,3 +130,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. + """, + )