From dca3a825456ebc7a13dac035498ee19c584f620f Mon Sep 17 00:00:00 2001 From: Yutarop Date: Fri, 27 Jun 2025 07:11:39 +0000 Subject: [PATCH 1/7] change default world --- launch/tb3_agent.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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") ) ) From 8a60b73fc3cb8b1ab7a2db1b240f73325c9217b8 Mon Sep 17 00:00:00 2001 From: Yutarop Date: Fri, 27 Jun 2025 09:25:41 +0000 Subject: [PATCH 2/7] update gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) 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/ From 1066acce5f1407d2981befe7757c1214e48ef6e6 Mon Sep 17 00:00:00 2001 From: Yutarop Date: Fri, 27 Jun 2025 09:26:01 +0000 Subject: [PATCH 3/7] change topic name for odometry --- turtlebot3_agent/tb3_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/turtlebot3_agent/tb3_node.py b/turtlebot3_agent/tb3_node.py index 8827e82..d1bfd4c 100755 --- a/turtlebot3_agent/tb3_node.py +++ b/turtlebot3_agent/tb3_node.py @@ -24,7 +24,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 From 73735f5958ff6ae3d23aed2815262fecd1023ce4 Mon Sep 17 00:00:00 2001 From: Yutarop Date: Fri, 27 Jun 2025 12:09:34 +0000 Subject: [PATCH 4/7] updates for 6/27 --- turtlebot3_agent/tb3_node.py | 155 ++++++++++++++++++++++++- turtlebot3_agent/tools/all_tools.py | 4 + turtlebot3_agent/tools/motion_tools.py | 98 ++++++++++++++++ 3 files changed, 255 insertions(+), 2 deletions(-) diff --git a/turtlebot3_agent/tb3_node.py b/turtlebot3_agent/tb3_node.py index d1bfd4c..25868c2 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, 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 @@ -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..6df0cae 100644 --- a/turtlebot3_agent/tools/all_tools.py +++ b/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 @@ -39,9 +41,11 @@ def make_all_tools(node) -> list: 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_navigate_to_pose_tool(node), make_rotate_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..4bdf96a 100644 --- a/turtlebot3_agent/tools/motion_tools.py +++ b/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,98 @@ 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." + return ( + f"Navigation completed successfully! " + f"You are now at position ({round(node.x, 2)}, {round(node.y, 2)}) " + f"with a yaw angle of {round(node.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 odom frame (meters)") + odom_y: float = Field(description="Y coordinate in odom frame (meters)") + odom_yaw: float = Field(description="Yaw angle in odom 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 odom to map frame:\n" + f"Odom: ({round(inputs.odom_x, 3)}, {round(inputs.odom_y, 3)}, {round(inputs.odom_yaw, 3)} rad)\n" + f"Map: ({round(map_x, 3)}, {round(map_y, 3)}, {round(map_yaw, 3)} 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 odom 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. + """, + ) From b37462ecc2f7cefa1f45fd774f57c97695f8ded2 Mon Sep 17 00:00:00 2001 From: Yutarop Date: Sat, 28 Jun 2025 03:02:50 +0000 Subject: [PATCH 5/7] update tools --- turtlebot3_agent/tools/all_tools.py | 10 --------- turtlebot3_agent/tools/motion_tools.py | 28 +++++++++++++++----------- 2 files changed, 16 insertions(+), 22 deletions(-) diff --git a/turtlebot3_agent/tools/all_tools.py b/turtlebot3_agent/tools/all_tools.py index 6df0cae..e47fb7b 100644 --- a/turtlebot3_agent/tools/all_tools.py +++ b/turtlebot3_agent/tools/all_tools.py @@ -34,19 +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_navigate_to_pose_tool(node), - make_rotate_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 4bdf96a..b1f64ca 100644 --- a/turtlebot3_agent/tools/motion_tools.py +++ b/turtlebot3_agent/tools/motion_tools.py @@ -157,11 +157,15 @@ def inner(inputs: NavigateInput) -> str: if success: if not node.wait_for_pose(timeout=0.5): return "Navigation completed, but pose data not received yet." - return ( - f"Navigation completed successfully! " - f"You are now at position ({round(node.x, 2)}, {round(node.y, 2)}) " - f"with a yaw angle of {round(node.yaw, 3)} radians." - ) + 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." @@ -195,9 +199,9 @@ def make_transform_odom_to_map_tool(node): """ class TransformInput(BaseModel): - odom_x: float = Field(description="X coordinate in odom frame (meters)") - odom_y: float = Field(description="Y coordinate in odom frame (meters)") - odom_yaw: float = Field(description="Yaw angle in odom frame (radians)") + 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: @@ -205,9 +209,9 @@ def inner(inputs: TransformInput) -> str: odom_x=inputs.odom_x, odom_y=inputs.odom_y, odom_yaw=inputs.odom_yaw ) return ( - f"Transformed coordinates from odom to map frame:\n" - f"Odom: ({round(inputs.odom_x, 3)}, {round(inputs.odom_y, 3)}, {round(inputs.odom_yaw, 3)} rad)\n" - f"Map: ({round(map_x, 3)}, {round(map_y, 3)}, {round(map_yaw, 3)} rad)" + 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)}" @@ -216,7 +220,7 @@ def inner(inputs: TransformInput) -> str: func=inner, name="transform_odom_to_map", description=""" - Transform coordinates from odom frame to map frame using TF2. + Transform coordinates from pose frame to map frame using TF2. Args: - odom_x (float): X coordinate in robot's current frame (meters) From 0af913d229ad08256461354212958e5c821987fb Mon Sep 17 00:00:00 2001 From: Yutarop Date: Mon, 21 Jul 2025 10:46:13 +0000 Subject: [PATCH 6/7] Apply formatting to PR files --- turtlebot3_agent/tb3_node.py | 2 +- turtlebot3_agent/tools/all_tools.py | 11 ----------- turtlebot3_agent/tools/motion_tools.py | 2 -- 3 files changed, 1 insertion(+), 14 deletions(-) diff --git a/turtlebot3_agent/tb3_node.py b/turtlebot3_agent/tb3_node.py index 25868c2..7b37709 100755 --- a/turtlebot3_agent/tb3_node.py +++ b/turtlebot3_agent/tb3_node.py @@ -13,7 +13,7 @@ import tf2_geometry_msgs import tf2_ros from cv_bridge import CvBridge -from geometry_msgs.msg import PointStamped, PoseStamped, TransformStamped, 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 diff --git a/turtlebot3_agent/tools/all_tools.py b/turtlebot3_agent/tools/all_tools.py index e47fb7b..51f20df 100644 --- a/turtlebot3_agent/tools/all_tools.py +++ b/turtlebot3_agent/tools/all_tools.py @@ -1,17 +1,6 @@ # 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_navigate_to_pose_tool, - make_rotate_tool, make_transform_odom_to_map_tool, ) diff --git a/turtlebot3_agent/tools/motion_tools.py b/turtlebot3_agent/tools/motion_tools.py index b1f64ca..25f77bc 100644 --- a/turtlebot3_agent/tools/motion_tools.py +++ b/turtlebot3_agent/tools/motion_tools.py @@ -1,11 +1,9 @@ #!/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): From fad43a086018b00bc9c0408313237bbff66a967a Mon Sep 17 00:00:00 2001 From: Yuta <120556494+Yutarop@users.noreply.github.com> Date: Mon, 21 Jul 2025 19:52:26 +0900 Subject: [PATCH 7/7] Update turtlebot3_agent/tools/motion_tools.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- turtlebot3_agent/tools/motion_tools.py | 1 - 1 file changed, 1 deletion(-) diff --git a/turtlebot3_agent/tools/motion_tools.py b/turtlebot3_agent/tools/motion_tools.py index 25f77bc..f5bdca7 100644 --- a/turtlebot3_agent/tools/motion_tools.py +++ b/turtlebot3_agent/tools/motion_tools.py @@ -2,7 +2,6 @@ from langchain.tools import StructuredTool -from pydantic import BaseModel, Field from pydantic.v1 import BaseModel, Field