feat: enable hybrid local and remote module execution
This commit is contained in:
@@ -25,10 +25,12 @@ from nav_msgs.msg import OccupancyGrid, Odometry, Path as NavPath
|
||||
from nav2_msgs.action import NavigateToPose
|
||||
from pydantic import BaseModel
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.duration import Duration
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
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 tf2_msgs.msg import TFMessage
|
||||
import uvicorn
|
||||
@@ -149,6 +151,7 @@ class SlamNode(Node):
|
||||
|
||||
self.odom_topic = os.environ.get("ODOM_TOPIC", "/odom")
|
||||
self.scan_topic = os.environ.get("SCAN_TOPIC", "/scan")
|
||||
self.amcl_pose_topic = os.environ.get("AMCL_POSE_TOPIC", "/amcl_pose")
|
||||
self.point_cloud_topic = os.environ.get("POINT_CLOUD_TOPIC", "/cloud_map")
|
||||
self.fallback_point_cloud_topic = os.environ.get("FALLBACK_POINT_CLOUD_TOPIC", "/lidar_filtered")
|
||||
self.icp_odom_topic = os.environ.get("ICP_ODOM_TOPIC", "/icp_odom")
|
||||
@@ -181,6 +184,7 @@ class SlamNode(Node):
|
||||
|
||||
self._odom: Optional[Odometry] = None
|
||||
self._scan: Optional[LaserScan] = None
|
||||
self._amcl_pose: Optional[PoseWithCovarianceStamped] = None
|
||||
self._point_cloud: Optional[PointCloud2] = None
|
||||
self._fallback_point_cloud: Optional[PointCloud2] = None
|
||||
self._icp_odom: Optional[Odometry] = None
|
||||
@@ -189,6 +193,7 @@ class SlamNode(Node):
|
||||
self._tf = {}
|
||||
self._odom_count = 0
|
||||
self._scan_count = 0
|
||||
self._amcl_pose_count = 0
|
||||
self._point_cloud_count = 0
|
||||
self._fallback_point_cloud_count = 0
|
||||
self._icp_odom_count = 0
|
||||
@@ -196,10 +201,15 @@ class SlamNode(Node):
|
||||
self._tf_count = 0
|
||||
self._last_odom_wall_time = None
|
||||
self._last_scan_wall_time = None
|
||||
self._last_amcl_pose_wall_time = None
|
||||
self._last_point_cloud_wall_time = None
|
||||
self._last_fallback_point_cloud_wall_time = None
|
||||
self._last_icp_odom_wall_time = None
|
||||
self._last_map_wall_time = None
|
||||
self._plan_count = 0
|
||||
self._last_plan_wall_time = None
|
||||
self._topic_status_cache = {}
|
||||
self._topic_status_last_refresh = 0.0
|
||||
self._navigation = {"status": "idle", "goal_id": None}
|
||||
self._active_goal_handle = None
|
||||
self._lock = threading.Lock()
|
||||
@@ -208,6 +218,8 @@ class SlamNode(Node):
|
||||
self.create_subscription(Odometry, self.odom_topic, self._on_odom, best_effort_qos)
|
||||
self.create_subscription(LaserScan, self.scan_topic, self._on_scan, qos_profile_sensor_data)
|
||||
self.create_subscription(LaserScan, self.scan_topic, self._on_scan, reliable_qos)
|
||||
self.create_subscription(PoseWithCovarianceStamped, self.amcl_pose_topic, self._on_amcl_pose, reliable_qos)
|
||||
self.create_subscription(PoseWithCovarianceStamped, self.amcl_pose_topic, self._on_amcl_pose, best_effort_qos)
|
||||
self.create_subscription(PointCloud2, self.point_cloud_topic, self._on_point_cloud, map_qos)
|
||||
if self.fallback_point_cloud_topic != self.point_cloud_topic:
|
||||
self.create_subscription(PointCloud2, self.fallback_point_cloud_topic, self._on_fallback_point_cloud, qos_profile_sensor_data)
|
||||
@@ -233,8 +245,19 @@ class SlamNode(Node):
|
||||
self._last_odom_wall_time = time.time()
|
||||
|
||||
def publish_initial_pose(self, x: float, y: float, yaw: float = 0.0, frame_id: str = "map") -> None:
|
||||
# AMCL transforms the initial pose at the message timestamp. On a real
|
||||
# platform the bridge clock can be a few milliseconds ahead of the
|
||||
# latest odom/TF sample, which makes AMCL reject the pose as a future
|
||||
# extrapolation. Anchor it to the latest received odometry instead.
|
||||
with self._lock:
|
||||
odom_stamp = self._odom.header.stamp if self._odom is not None else None
|
||||
message = PoseWithCovarianceStamped()
|
||||
message.header.stamp = self.get_clock().now().to_msg()
|
||||
if odom_stamp:
|
||||
# The odom callback can lead the TF buffer by a few milliseconds.
|
||||
# Keep a small margin so AMCL can always transform map -> odom.
|
||||
message.header.stamp = (Time.from_msg(odom_stamp) - Duration(seconds=0.1)).to_msg()
|
||||
else:
|
||||
message.header.stamp = self.get_clock().now().to_msg()
|
||||
message.header.frame_id = frame_id
|
||||
message.pose.pose.position.x = x
|
||||
message.pose.pose.position.y = y
|
||||
@@ -355,6 +378,12 @@ class SlamNode(Node):
|
||||
self._scan_count += 1
|
||||
self._last_scan_wall_time = time.time()
|
||||
|
||||
def _on_amcl_pose(self, msg: PoseWithCovarianceStamped):
|
||||
with self._lock:
|
||||
self._amcl_pose = msg
|
||||
self._amcl_pose_count += 1
|
||||
self._last_amcl_pose_wall_time = time.time()
|
||||
|
||||
def _on_point_cloud(self, msg: PointCloud2):
|
||||
with self._lock:
|
||||
self._point_cloud = msg
|
||||
@@ -393,6 +422,51 @@ class SlamNode(Node):
|
||||
def _on_plan(self, msg: NavPath):
|
||||
with self._lock:
|
||||
self._plan = msg
|
||||
self._plan_count += 1
|
||||
self._last_plan_wall_time = time.time()
|
||||
|
||||
def topic_status(self) -> dict:
|
||||
now = time.time()
|
||||
with self._lock:
|
||||
if now - self._topic_status_last_refresh < 1.0:
|
||||
return dict(self._topic_status_cache)
|
||||
scan_last = self._last_scan_wall_time
|
||||
odom_last = self._last_odom_wall_time
|
||||
amcl_pose_last = self._last_amcl_pose_wall_time
|
||||
point_cloud_last = self._last_point_cloud_wall_time
|
||||
fallback_point_cloud_last = self._last_fallback_point_cloud_wall_time
|
||||
map_count = self._map_count
|
||||
plan_count = self._plan_count
|
||||
|
||||
def received_recently(last_received: Optional[float]) -> bool:
|
||||
return last_received is not None and now - last_received <= 3.0
|
||||
|
||||
try:
|
||||
plan_available = plan_count > 0 or self.count_publishers(self.plan_topic) > 0
|
||||
cmd_vel_available = self.count_subscribers(self.cmd_vel_topic) > 0
|
||||
navigation_available = self._navigate_to_pose_client.server_is_ready()
|
||||
except Exception:
|
||||
plan_available = plan_count > 0
|
||||
cmd_vel_available = False
|
||||
navigation_available = False
|
||||
|
||||
status = {
|
||||
"scan": {"name": self.scan_topic, "available": received_recently(scan_last)},
|
||||
"odom": {"name": self.odom_topic, "available": received_recently(odom_last)},
|
||||
"map": {"name": self.map_topic, "available": map_count > 0},
|
||||
"amcl_pose": {"name": self.amcl_pose_topic, "available": received_recently(amcl_pose_last)},
|
||||
"point_cloud": {
|
||||
"name": self.point_cloud_topic,
|
||||
"available": received_recently(point_cloud_last) or received_recently(fallback_point_cloud_last),
|
||||
},
|
||||
"plan": {"name": self.plan_topic, "available": plan_available},
|
||||
"cmd_vel": {"name": self.cmd_vel_topic, "available": cmd_vel_available},
|
||||
"navigate_to_pose": {"name": self.navigate_to_pose_action, "available": navigation_available},
|
||||
}
|
||||
with self._lock:
|
||||
self._topic_status_cache = status
|
||||
self._topic_status_last_refresh = now
|
||||
return dict(status)
|
||||
|
||||
@staticmethod
|
||||
def _has_populated_map(grid: Optional[OccupancyGrid]) -> bool:
|
||||
@@ -738,6 +812,7 @@ class SlamNode(Node):
|
||||
"map_to_odom": resolve_2d_transform(tf_snapshot, "map", "odom"),
|
||||
},
|
||||
"navigation": navigation,
|
||||
"topic_status": self.topic_status(),
|
||||
"plan": {
|
||||
"frame_id": plan.header.frame_id,
|
||||
"poses": [
|
||||
@@ -904,6 +979,7 @@ def health():
|
||||
"has_point_cloud": bool(snap.get("point_cloud")),
|
||||
"has_map": bool(snap.get("map")),
|
||||
"topics": snap.get("topics", {}),
|
||||
"topic_status": snap.get("topic_status", {}),
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user