diff --git a/resources/slam-ros-bridge/bridge.py b/resources/slam-ros-bridge/bridge.py index 0da8279..05cdb2e 100644 --- a/resources/slam-ros-bridge/bridge.py +++ b/resources/slam-ros-bridge/bridge.py @@ -31,7 +31,8 @@ from rclpy.node import Node from rclpy.parameter import Parameter from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy, qos_profile_sensor_data from rclpy.time import Time -from sensor_msgs.msg import LaserScan, PointCloud2 +from sensor_msgs.msg import JointState, LaserScan, PointCloud2 +from std_msgs.msg import Float64 from tf2_msgs.msg import TFMessage import uvicorn @@ -129,6 +130,10 @@ class VelocityPayload(BaseModel): angular: float = 0.0 +class SteeringPayload(BaseModel): + angle: float + + class NavigationGoalPayload(BaseModel): x: float y: float @@ -158,6 +163,9 @@ class SlamNode(Node): self.cmd_vel_topic = os.environ.get("CMD_VEL_TOPIC", "/cmd_vel") self.map_topic = os.environ.get("MAP_TOPIC", "/map") self.plan_topic = os.environ.get("PLAN_TOPIC", "/plan") + self.joint_states_topic = os.environ.get("JOINT_STATES_TOPIC", "/joint_states") + self.steering_topic = os.environ.get("STEERING_TOPIC", "/mkmini/steering_command") + self.steering_feedback_topic = os.environ.get("STEERING_FEEDBACK_TOPIC", "/mkmini/steering_angle") self.navigate_to_pose_action = os.environ.get("NAVIGATE_TO_POSE_ACTION", "/navigate_to_pose") self.read_only = os.environ.get("READ_ONLY", "false").lower() in ("1", "true", "yes") @@ -190,6 +198,9 @@ class SlamNode(Node): self._icp_odom: Optional[Odometry] = None self._map: Optional[OccupancyGrid] = None self._plan: Optional[NavPath] = None + self._joint_positions = {} + self._steering_feedback = None + self._simulator_steering_angle = None self._tf = {} self._odom_count = 0 self._scan_count = 0 @@ -212,6 +223,7 @@ class SlamNode(Node): self._topic_status_last_refresh = 0.0 self._navigation = {"status": "idle", "goal_id": None} self._active_goal_handle = None + self._requested_initial_pose: Optional[dict] = None self._lock = threading.Lock() self.create_subscription(Odometry, self.odom_topic, self._on_odom, reliable_qos) @@ -228,9 +240,12 @@ class SlamNode(Node): self.create_subscription(OccupancyGrid, self.map_topic, self._on_map, map_qos) self.create_subscription(OccupancyGrid, self.map_topic, self._on_map, best_effort_qos) self.create_subscription(NavPath, self.plan_topic, self._on_plan, reliable_qos) + self.create_subscription(JointState, self.joint_states_topic, self._on_joint_states, reliable_qos) + self.create_subscription(Float64, self.steering_feedback_topic, self._on_steering_feedback, reliable_qos) self.create_subscription(TFMessage, "/tf", self._on_tf, qos_profile_sensor_data) self.create_subscription(TFMessage, "/tf_static", self._on_tf, tf_static_qos) self._cmd_vel_pub = self.create_publisher(Twist, self.cmd_vel_topic, 10) + self._steering_pub = self.create_publisher(Float64, self.steering_topic, 10) self._initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 10) self._navigate_to_pose_client = ActionClient(self, NavigateToPose, self.navigate_to_pose_action) @@ -269,6 +284,18 @@ class SlamNode(Node): message.pose.covariance[0] = 0.25 message.pose.covariance[7] = 0.25 message.pose.covariance[35] = 0.0685 + with self._lock: + # Use this only until AMCL starts publishing map -> odom. It is a + # short-lived viewer fallback, not a replacement for live TF. + self._requested_initial_pose = { + "x": float(x), + "y": float(y), + "z": 0.0, + "yaw": float(yaw), + "frame_id": frame_id or "map", + "t": time.time(), + "expires_at": time.monotonic() + 2.0, + } self._initial_pose_pub.publish(message) def save_map(self, name: Optional[str] = None) -> dict: @@ -425,6 +452,22 @@ class SlamNode(Node): self._plan_count += 1 self._last_plan_wall_time = time.time() + def _clear_plan_locked(self): + """Discard a path that belongs to a completed or replaced navigation goal.""" + self._plan = None + self._last_plan_wall_time = None + + def _on_joint_states(self, msg: JointState): + with self._lock: + self._joint_positions = { + name: position + for name, position in zip(msg.name, msg.position) + } + + def _on_steering_feedback(self, msg: Float64): + with self._lock: + self._steering_feedback = float(msg.data) + def topic_status(self) -> dict: now = time.time() with self._lock: @@ -563,6 +606,21 @@ class SlamNode(Node): msg.angular.z = float(angular) self._cmd_vel_pub.publish(msg) + def set_simulator_steering(self, angle: float) -> None: + if self.read_only: + raise RuntimeError("읽기 전용 3D SLAM 화면에서는 차량 제어를 지원하지 않습니다.") + if not math.isfinite(angle): + raise RuntimeError("조향각이 올바르지 않습니다.") + steering_angle = max(-0.5, min(0.5, float(angle))) + command = Float64() + command.data = steering_angle + with self._lock: + self._simulator_steering_angle = steering_angle + self._steering_pub.publish(command) + drive_command = Twist() + drive_command.angular.z = steering_angle + self._cmd_vel_pub.publish(drive_command) + def send_navigation_goal(self, x: float, y: float, yaw: float = 0.0, frame_id: str = "map") -> dict: if self.read_only: raise RuntimeError("읽기 전용 3D SLAM 화면에서는 목표점을 설정할 수 없습니다.") @@ -596,6 +654,7 @@ class SlamNode(Node): goal_id = bytes(goal_handle.goal_id.uuid).hex() with self._lock: + self._clear_plan_locked() self._navigation = {"status": "running", "goal_id": goal_id} self._active_goal_handle = goal_handle goal_handle.get_result_async().add_done_callback( @@ -631,11 +690,13 @@ class SlamNode(Node): return self._navigation = {**self._navigation, "status": result_status} self._active_goal_handle = None + self._clear_plan_locked() def cancel_navigation(self, wait: bool = False) -> bool: with self._lock: goal_handle = self._active_goal_handle self._navigation = {**self._navigation, "status": "canceled"} + self._clear_plan_locked() if goal_handle is None: return True @@ -662,6 +723,10 @@ class SlamNode(Node): scan = self._scan grid = self._map plan = self._plan + amcl_pose = self._amcl_pose + requested_initial_pose = dict(self._requested_initial_pose) if self._requested_initial_pose else None + joint_positions = dict(self._joint_positions) + steering_feedback = self._steering_feedback tf_snapshot = dict(self._tf) navigation = dict(self._navigation) effective_map = self._effective_map(grid, scan, odom) @@ -671,6 +736,7 @@ class SlamNode(Node): pose_map = None scan_pose = None scan_pose_map = None + vehicle = {} if odom is not None: pos = odom.pose.pose.position ori = odom.pose.pose.orientation @@ -703,6 +769,10 @@ class SlamNode(Node): "source_frame_id": scan.header.frame_id, "t": scan.header.stamp.sec + scan.header.stamp.nanosec * 1e-9, } + # map -> base is the live localization transform and must win over + # /amcl_pose. AMCL may publish a pose only when it changes, while + # TF continues updating with odometry; preferring the message made + # the viewer appear to stop between AMCL updates. map_to_base = resolve_2d_transform(tf_snapshot, "map", odom.child_frame_id) if map_to_base is not None: pose_map = { @@ -712,10 +782,79 @@ class SlamNode(Node): "source_frame_id": odom.header.frame_id, "t": pose["t"], } + elif amcl_pose is not None and amcl_pose.header.frame_id.lstrip("/") == "map": + amcl_position = amcl_pose.pose.pose.position + pose_map = { + "x": amcl_position.x, + "y": amcl_position.y, + "z": amcl_position.z, + "yaw": yaw_from_quaternion(amcl_pose.pose.pose.orientation), + "frame_id": "map", + "child_frame_id": odom.child_frame_id, + "source_frame_id": odom.header.frame_id, + "t": amcl_pose.header.stamp.sec + amcl_pose.header.stamp.nanosec * 1e-9, + } + elif ( + requested_initial_pose + and requested_initial_pose["frame_id"].lstrip("/") == "map" + and time.monotonic() <= requested_initial_pose.get("expires_at", 0.0) + ): + pose_map = { + **requested_initial_pose, + "child_frame_id": odom.child_frame_id, + "source_frame_id": odom.header.frame_id, + } + + # The Gazebo MK-mini uses independent front steering joints. Keep + # both values so the viewer renders the Ackermann inner/outer turn. + steering_left_angle = next( + ( + position + for name, position in joint_positions.items() + if name == "front_left_visual_steering_joint" or name.endswith("/front_left_visual_steering_joint") + ), + None, + ) + steering_right_angle = next( + ( + position + for name, position in joint_positions.items() + if name == "front_right_visual_steering_joint" or name.endswith("/front_right_visual_steering_joint") + ), + None, + ) + steering_angle = None + if steering_left_angle is not None and steering_right_angle is not None: + vehicle["steering_left_angle"] = steering_left_angle + vehicle["steering_right_angle"] = steering_right_angle + steering_angle = (steering_left_angle + steering_right_angle) * 0.5 + else: + steering_angle = next( + ( + position + for name, position in joint_positions.items() + if name == "front_steering_joint" or name.endswith("/front_steering_joint") + ), + None, + ) + if steering_angle is None and steering_feedback is not None: + steering_angle = steering_feedback + if steering_angle is None: + front_fork = resolve_2d_transform( + tf_snapshot, + odom.child_frame_id or "base_link", + "front_fork", + ) + steering_angle = front_fork["yaw"] if front_fork is not None else None + if steering_angle is None: + steering_angle = self._simulator_steering_angle + if steering_angle is not None: + vehicle["steering_angle"] = steering_angle if scan is not None and pose_map is not None: - # Resolve map -> sensor directly through the published TF graph so - # the scan uses the same localization correction as the map. + # Prefer direct map -> sensor TF for the current scan. It carries + # the current odometry timestamp and keeps the laser synchronized + # with the moving robot. map_to_scan = resolve_2d_transform(tf_snapshot, "map", scan.header.frame_id) if map_to_scan is not None: scan_pose_map = { @@ -726,6 +865,21 @@ class SlamNode(Node): "source_frame_id": scan.header.frame_id, "t": scan.header.stamp.sec + scan.header.stamp.nanosec * 1e-9, } + else: + base_to_scan = resolve_2d_transform( + tf_snapshot, + odom.child_frame_id if odom is not None else "base_link", + scan.header.frame_id, + ) + if base_to_scan is not None: + scan_pose_map = { + **compose_2d(pose_map, base_to_scan), + "roll": 0.0, + "pitch": 0.0, + "frame_id": "map", + "source_frame_id": scan.header.frame_id, + "t": pose_map["t"], + } pose_3d = None if icp_odom is not None: @@ -805,6 +959,7 @@ class SlamNode(Node): "scan_pose": scan_pose, "scan_pose_map": scan_pose_map, "pose_3d": pose_3d, + "vehicle": vehicle, "scan": scan_payload, "point_cloud": point_cloud_payload, "map": map_payload, @@ -1036,6 +1191,17 @@ def cmd_vel(payload: VelocityPayload): return {"ok": True} +@app.post("/steering") +def steering(payload: SteeringPayload): + if not slam_node: + raise HTTPException(status_code=503, detail="ROS node is not ready") + try: + slam_node.set_simulator_steering(payload.angle) + except RuntimeError as error: + raise HTTPException(status_code=409, detail=str(error)) from error + return {"ok": True} + + @app.post("/stop") def stop(): if not slam_node: