update x3 0.0.11

Makefile

@@ -1,8 +1,8 @@
 # limo-dockerx-build-push:
 # 	docker buildx build --platform linux/arm64 -t huajuan6848/limo-54demo-ros-foxy:0.0.9-5 -f docker/limo/Dockerfile --push .
 x3-pi-dockerx-build-push:
-	docker buildx build --platform linux/arm64 -t huajuan6848/x3-54demo-ros-foxy:0.0.10-pi -f docker/x3/Dockerfile.pi --push .
+	docker buildx build --platform linux/arm64 -t huajuan6848/x3-54demo-ros-foxy:0.0.11-pi -f docker/x3/Dockerfile.pi --push .
 x3-jetson-dockerx-build-push:
-	docker buildx build --platform linux/arm64 -t huajuan6848/x3-54demo-ros-foxy:0.0.10-jetson -f docker/x3/Dockerfile.jetson --push .
+	docker buildx build --platform linux/arm64 -t huajuan6848/x3-54demo-ros-foxy:0.0.11-jetson -f docker/x3/Dockerfile.jetson --push .
 x3-base-dockerx-build-push:
-	docker buildx build --platform linux/arm64 -t huajuan6848/x3_ros2:0.0.1 -f docker/x3/Dockerfile.base --push .
+	docker buildx build --platform linux/arm64 -t huajuan6848/x3_ros2:0.0.1-orig-nav -f docker/x3/Dockerfile.base --push .

README.md

@@ -176,6 +176,12 @@ POST数据：json格式，例如
 }
 ```
 
+### 导航小车到指定位置和朝向（同步）
+
+URL: `http://<小车ip>:5000/nav_sync`
+
+其余同上。同步的意思是，这个请求会一直等到小车到达目标位置才返回。
+
 ### 错误处理
 
 如果以上的请求返回的状态码不是200，说明请求出现了错误。错误的信息会在返回的json中给出。

docker/limo/navigation2.yaml

@@ -61,7 +61,6 @@ bt_navigator:
     groot_zmq_server_port: 1667
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
-    - nav2_compute_path_through_poses_action_bt_node
     - nav2_follow_path_action_bt_node
     - nav2_back_up_action_bt_node
     - nav2_spin_action_bt_node
@@ -83,14 +82,6 @@ bt_navigator:
     - nav2_transform_available_condition_bt_node
     - nav2_time_expired_condition_bt_node
     - nav2_distance_traveled_condition_bt_node
-    - nav2_single_trigger_bt_node
-    - nav2_is_battery_low_condition_bt_node
-    - nav2_navigate_through_poses_action_bt_node
-    - nav2_navigate_to_pose_action_bt_node
-    - nav2_remove_passed_goals_action_bt_node
-    - nav2_planner_selector_bt_node
-    - nav2_controller_selector_bt_node
-    - nav2_goal_checker_selector_bt_node
 
 bt_navigator_rclcpp_node:
   ros__parameters:
@@ -124,13 +115,13 @@ controller_server:
     FollowPath:
       plugin: "dwb_core::DWBLocalPlanner"
       debug_trajectory_details: True
-      min_vel_x: 0.0
+      min_vel_x: -0.3
       min_vel_y: 0.0
-      max_vel_x: 0.22
+      max_vel_x: 0.3
       max_vel_y: 0.0
       max_vel_theta: 1.0
       min_speed_xy: 0.0
-      max_speed_xy: 0.22
+      max_speed_xy: 0.3
       min_speed_theta: 0.0
       # Add high threshold velocity for turtlebot 3 issue.
       # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
@@ -147,7 +138,8 @@ controller_server:
       linear_granularity: 0.05
       angular_granularity: 0.025
       transform_tolerance: 0.2
-      xy_goal_tolerance: 0.25
+      xy_goal_tolerance: 0.15
+      yaw_goal_tolerance: 0.10
       trans_stopped_velocity: 0.25
       short_circuit_trajectory_evaluation: True
       stateful: True

docker/x3/README.md

@@ -9,4 +9,5 @@
 - 0.0.8: 在`red_detector`启动5秒后才启动`red_obj_server`，防止出现race condition；加入vim
 - 0.0.9: 在`nav_http_server`中添加了`/initial_pose`接口，用于设置小车的初始位置和朝向(amcl)
 - 0.0.9-7: 添加中途取消导航的功能
-- 0.0.10: 将红色物体的检测方式改为apriltag
+- 0.0.10: 将红色物体的检测方式改为apriltag
+- 0.0.11: 1.添加同步导航接口 2.添加测试导航接口

nav_http_server/nav_http_server/nav_http_server.py

@@ -17,6 +17,11 @@
 from flask_cors import CORS
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from action_msgs.msg import GoalStatus
+import tf2_ros
+try:
+    from .utils import quaternion_to_eular  # for ros2 run
+except:
+    from utils import quaternion_to_eular  # for python3 nav_http_server.py
 
 # 设置日志级别和格式
 logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(name)s - %(levelname)s - %(message)s')
@@ -25,9 +30,11 @@
 parser = ArgumentParser()
 parser.add_argument('--map-frame', default='map')
 parser.add_argument('--navigate-to-pose-action', default='/navigate_to_pose')
+parser.add_argument('--robot-frame', default='base_link')
 args, unknown = parser.parse_known_args()
 
 MAP_FRAME = args.map_frame
+ROBOT_FRAME = args.robot_frame
 NAVIGATE_TO_POSE_ACTION = args.navigate_to_pose_action
 
 DIST_THRESHOLD = 0.01
@@ -41,9 +48,10 @@ def __init__(self):
         if not self.action_client.wait_for_server(timeout_sec=5.0):
             self.get_logger().error('NavigateToPose action server not available after waiting')
             exit(1)
-        self.last_goal = None
         self.initial_pose_publisher = self.create_publisher(PoseWithCovarianceStamped, '/initialpose', 10)
         self.goal_handles = []
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self, spin_thread=True)
 
     def send_initial_pose(self, x, y, theta):
         pose_msg = PoseWithCovarianceStamped()
@@ -60,17 +68,7 @@ def send_initial_pose(self, x, y, theta):
 
         self.initial_pose_publisher.publish(pose_msg)
 
-    def send_nav_goal(self, x, y, theta):
-        # 如果多次以相同的参数调用send_nav_goal，则不会发送新的目标
-        # 2024/06/26: 忽略这部分逻辑
-        if False:
-            x_last, y_last, theta_last = self.last_goal
-            dist = (x-x_last)**2 + (y-y_last)**2
-            rot = (theta-theta_last)**2
-            if dist < DIST_THRESHOLD and rot < ROT_THRESHOLD:
-                return True, 'goal not changed'
-
-        self.last_goal = (x, y, theta)
+    def __send_nav_goal_base(self, x, y, theta):
         q = eular_to_quaternion(0, 0, theta)
 
         goal_msg = NavigateToPose.Goal()
@@ -88,6 +86,10 @@ def send_nav_goal(self, x, y, theta):
         time.sleep(0.1)
         goal_handle = goal_handle_future.result()
 
+        return goal_handle
+
+    def send_nav_goal(self, x, y, theta):
+        goal_handle = self.__send_nav_goal_base(x, y, theta)
         if not goal_handle.accepted:
             self.get_logger().error('Goal rejected')
             return False, 'goal rejected'
@@ -97,6 +99,32 @@ def send_nav_goal(self, x, y, theta):
 
         return True, 'goal accepted'
 
+    def send_nav_goal_sync(self, x, y, theta):
+        goal_handle = self.__send_nav_goal_base(x, y, theta)
+        if not goal_handle.accepted:
+            self.get_logger().error('Goal rejected')
+            return False, 'goal rejected'
+        self.get_logger().info('Move started')
+        self.goal_handles.append(goal_handle)
+
+        # wait for the goal handle to finish
+        not_move_statuses = [GoalStatus.STATUS_SUCCEEDED, GoalStatus.STATUS_CANCELED, GoalStatus.STATUS_ABORTED]
+        import time
+        while goal_handle.status not in not_move_statuses:
+            time.sleep(0.1)
+
+        result_status = goal_handle.status
+        result_status_str = ''
+        if result_status == GoalStatus.STATUS_SUCCEEDED:
+            result_status_str = 'SUCCEEDED'
+        elif result_status == GoalStatus.STATUS_CANCELED:
+            result_status_str = 'CANCELED'
+        elif result_status == GoalStatus.STATUS_ABORTED:
+            result_status_str = 'ABORTED'
+
+        self.get_logger().info(f'Move finished with status: {result_status_str}')
+        return True, f'move finished with status: {result_status_str}'
+
     def is_moving(self):
         if len(self.goal_handles) == 0:
             return False
@@ -110,6 +138,19 @@ def cancel_nav(self):
         gh = self.goal_handles[-1]
         gh.cancel_goal()
         return True, 'goal canceled'
+
+    def robot_loc(self):
+        # 获得ROBOT_FRAME相对于MAP_FRAME的坐标变换（最新可用的）
+        transform = self.tf_buffer.lookup_transform(MAP_FRAME, ROBOT_FRAME, rclpy.time.Time(), rclpy.time.Duration(seconds=5))
+
+        x = transform.transform.translation.x
+        y = transform.transform.translation.y
+
+        q = transform.transform.rotation
+        e = quaternion_to_eular(q.x, q.y, q.z, q.w)
+        theta = e[2]
+
+        return x, y, theta
 
 rclpy.init()
 node = SendNavGoalNode()
@@ -148,6 +189,44 @@ def nav():
         err = RuntimeError(message)
         err.code = 500
         raise err
+
+@app.route('/nav_sync', methods=['POST'])
+def nav_sync():
+    try:
+        req : NavRequest = NavRequest(
+            x = request.json.get('x'),
+            y = request.json.get('y'),
+            theta = request.json.get('theta'))
+    except ValueError as ve:
+        msg = ve.errors()[0]['msg']
+        err = RuntimeError(msg)
+        err.code = 400
+        raise err
+    x = req.x; y = req.y; theta = req.theta
+
+    # check if the input is valid
+    if x is None or y is None or theta is None:
+        err = RuntimeError('invalid input: x, y, theta must be provided')
+        err.code = 400
+        raise err
+    success, message = node.send_nav_goal_sync(x, y, theta)
+    if success:
+        return jsonify({'status': 'success', 'message': message})
+    else:
+        err = RuntimeError(message)
+        err.code = 500
+        raise err
+
+
+@app.route("/nav_test", methods=['POST'])
+def nav_test():
+    import math
+    import time
+    curX, curY, curTheta = node.robot_loc()
+    node.send_nav_goal_sync(curX, curY, curTheta + math.pi) # 旋转180度
+    time.sleep(1)
+    node.send_nav_goal_sync(curX, curY, curTheta) # 旋转回来
+    return jsonify({'status': 'success', 'message': 'Nav test finished. Please check whether the robot has rotated 180 degrees and then rotated back.'})
 
 @app.route('/initial_pose', methods=['POST'])
 def initial_pose():