forked from zbw/yiliao2026
目前实现较快速从原点到达某个点，但是然后回到原点时有较大问题
This commit is contained in:
249
src/planner/scripts/odom_map_tf.py
Executable file
249
src/planner/scripts/odom_map_tf.py
Executable file
@@ -0,0 +1,249 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Align map to wheel odom using a user-provided initial pose.
|
||||
|
||||
This node replaces AMCL for controlled short-range tests. It publishes a fixed
|
||||
map->odom transform computed from:
|
||||
|
||||
known current map pose * inverse(current odom pose)
|
||||
|
||||
After initialization, pose updates come only from /odom.
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import rclpy
|
||||
from geometry_msgs.msg import PoseWithCovarianceStamped, TransformStamped
|
||||
from nav_msgs.msg import Odometry
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
from tf2_ros import TransformBroadcaster
|
||||
|
||||
|
||||
def yaw_from_quaternion(q):
|
||||
return math.atan2(
|
||||
2.0 * (q.w * q.z + q.x * q.y),
|
||||
1.0 - 2.0 * (q.y * q.y + q.z * q.z),
|
||||
)
|
||||
|
||||
|
||||
def normalize_angle(angle):
|
||||
return math.atan2(math.sin(angle), math.cos(angle))
|
||||
|
||||
|
||||
def fill_yaw_quaternion(q, yaw):
|
||||
q.x = 0.0
|
||||
q.y = 0.0
|
||||
q.z = math.sin(0.5 * yaw)
|
||||
q.w = math.cos(0.5 * yaw)
|
||||
|
||||
|
||||
class OdomMapTf(Node):
|
||||
def __init__(self):
|
||||
super().__init__("planner_odom_map_tf")
|
||||
self.declare_parameter("map_frame", "map")
|
||||
self.declare_parameter("odom_frame", "odom")
|
||||
self.declare_parameter("odom_topic", "/odom")
|
||||
self.declare_parameter("initial_pose_topic", "/initialpose")
|
||||
self.declare_parameter("status_topic", "/odom_localization_status")
|
||||
self.declare_parameter("initial_x", 0.0)
|
||||
self.declare_parameter("initial_y", 0.0)
|
||||
self.declare_parameter("initial_yaw", 0.0)
|
||||
self.declare_parameter("initialize_on_first_odom", True)
|
||||
self.declare_parameter("publish_rate", 30.0)
|
||||
self.declare_parameter("state_log_period", 1.0)
|
||||
|
||||
self.map_frame = str(self.get_parameter("map_frame").value)
|
||||
self.default_odom_frame = str(self.get_parameter("odom_frame").value)
|
||||
self.active_odom_frame = self.default_odom_frame
|
||||
self.initialize_on_first_odom = bool(
|
||||
self.get_parameter("initialize_on_first_odom").value
|
||||
)
|
||||
self.initial_pose = (
|
||||
float(self.get_parameter("initial_x").value),
|
||||
float(self.get_parameter("initial_y").value),
|
||||
float(self.get_parameter("initial_yaw").value),
|
||||
)
|
||||
self.state_log_period = max(
|
||||
0.0, float(self.get_parameter("state_log_period").value)
|
||||
)
|
||||
|
||||
self.latest_odom_pose = None
|
||||
self.pending_initial_pose = None
|
||||
self.have_transform = False
|
||||
self.last_state_log_ns = 0
|
||||
self.map_to_odom_x = 0.0
|
||||
self.map_to_odom_y = 0.0
|
||||
self.map_to_odom_yaw = 0.0
|
||||
|
||||
self.tf_broadcaster = TransformBroadcaster(self)
|
||||
self.status_pub = self.create_publisher(
|
||||
String, str(self.get_parameter("status_topic").value), 10
|
||||
)
|
||||
self.odom_sub = self.create_subscription(
|
||||
Odometry,
|
||||
str(self.get_parameter("odom_topic").value),
|
||||
self._odom_callback,
|
||||
20,
|
||||
)
|
||||
self.initial_pose_sub = self.create_subscription(
|
||||
PoseWithCovarianceStamped,
|
||||
str(self.get_parameter("initial_pose_topic").value),
|
||||
self._initial_pose_callback,
|
||||
10,
|
||||
)
|
||||
|
||||
publish_rate = max(1.0, float(self.get_parameter("publish_rate").value))
|
||||
self.timer = self.create_timer(1.0 / publish_rate, self._publish_transform)
|
||||
self._publish_status(
|
||||
"waiting_for_odom initial_pose=(%.3f, %.3f, %.3f)"
|
||||
% self.initial_pose
|
||||
)
|
||||
|
||||
def _odom_callback(self, msg):
|
||||
source_frame = msg.header.frame_id or self.default_odom_frame
|
||||
self.active_odom_frame = source_frame
|
||||
self.latest_odom_pose = (
|
||||
msg.pose.pose.position.x,
|
||||
msg.pose.pose.position.y,
|
||||
yaw_from_quaternion(msg.pose.pose.orientation),
|
||||
)
|
||||
|
||||
if self.pending_initial_pose is not None:
|
||||
pending = self.pending_initial_pose
|
||||
self.pending_initial_pose = None
|
||||
self._initialize_from_pose(*pending, reason="initialpose")
|
||||
elif not self.have_transform and self.initialize_on_first_odom:
|
||||
self._initialize_from_pose(*self.initial_pose, reason="launch_initial")
|
||||
|
||||
def _initial_pose_callback(self, msg):
|
||||
frame_id = msg.header.frame_id or self.map_frame
|
||||
if frame_id != self.map_frame:
|
||||
self._publish_status(
|
||||
"initialpose_rejected frame=%s expected=%s"
|
||||
% (frame_id, self.map_frame)
|
||||
)
|
||||
return
|
||||
|
||||
pose = msg.pose.pose
|
||||
initial_pose = (
|
||||
pose.position.x,
|
||||
pose.position.y,
|
||||
yaw_from_quaternion(pose.orientation),
|
||||
)
|
||||
if self.latest_odom_pose is None:
|
||||
self.pending_initial_pose = initial_pose
|
||||
self._publish_status("initialpose_queued waiting_for_odom")
|
||||
return
|
||||
self._initialize_from_pose(*initial_pose, reason="initialpose")
|
||||
|
||||
def _initialize_from_pose(self, map_x, map_y, map_yaw, reason):
|
||||
if self.latest_odom_pose is None:
|
||||
self.pending_initial_pose = (map_x, map_y, map_yaw)
|
||||
return
|
||||
|
||||
odom_x, odom_y, odom_yaw = self.latest_odom_pose
|
||||
delta_yaw = normalize_angle(map_yaw - odom_yaw)
|
||||
cos_yaw = math.cos(delta_yaw)
|
||||
sin_yaw = math.sin(delta_yaw)
|
||||
self.map_to_odom_x = map_x - (cos_yaw * odom_x - sin_yaw * odom_y)
|
||||
self.map_to_odom_y = map_y - (sin_yaw * odom_x + cos_yaw * odom_y)
|
||||
self.map_to_odom_yaw = delta_yaw
|
||||
self.have_transform = True
|
||||
self._publish_status(
|
||||
"%s_aligned %s->%s x=%.3f y=%.3f yaw=%.3f"
|
||||
% (
|
||||
reason,
|
||||
self.map_frame,
|
||||
self.active_odom_frame,
|
||||
self.map_to_odom_x,
|
||||
self.map_to_odom_y,
|
||||
self.map_to_odom_yaw,
|
||||
)
|
||||
)
|
||||
|
||||
def _publish_transform(self):
|
||||
if not self.have_transform:
|
||||
self._maybe_log_state()
|
||||
return
|
||||
msg = TransformStamped()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = self.map_frame
|
||||
msg.child_frame_id = self.active_odom_frame
|
||||
msg.transform.translation.x = self.map_to_odom_x
|
||||
msg.transform.translation.y = self.map_to_odom_y
|
||||
msg.transform.translation.z = 0.0
|
||||
fill_yaw_quaternion(msg.transform.rotation, self.map_to_odom_yaw)
|
||||
self.tf_broadcaster.sendTransform(msg)
|
||||
self._maybe_log_state()
|
||||
|
||||
def _maybe_log_state(self):
|
||||
if self.state_log_period <= 0.0:
|
||||
return
|
||||
now_ns = self.get_clock().now().nanoseconds
|
||||
min_period_ns = int(self.state_log_period * 1_000_000_000)
|
||||
if now_ns - self.last_state_log_ns < min_period_ns:
|
||||
return
|
||||
self.last_state_log_ns = now_ns
|
||||
|
||||
if self.latest_odom_pose is None:
|
||||
self.get_logger().info(
|
||||
"odom_tf_state waiting_for_odom "
|
||||
"initial_pose=(%.3f,%.3f,%.3f)"
|
||||
% self.initial_pose
|
||||
)
|
||||
return
|
||||
|
||||
odom_x, odom_y, odom_yaw = self.latest_odom_pose
|
||||
if not self.have_transform:
|
||||
self.get_logger().info(
|
||||
"odom_tf_state waiting_for_alignment "
|
||||
"odom=(%.3f,%.3f,%.3f) pending_initialpose=%s"
|
||||
% (odom_x, odom_y, odom_yaw, self.pending_initial_pose is not None)
|
||||
)
|
||||
return
|
||||
|
||||
cos_yaw = math.cos(self.map_to_odom_yaw)
|
||||
sin_yaw = math.sin(self.map_to_odom_yaw)
|
||||
map_x = self.map_to_odom_x + cos_yaw * odom_x - sin_yaw * odom_y
|
||||
map_y = self.map_to_odom_y + sin_yaw * odom_x + cos_yaw * odom_y
|
||||
map_yaw = normalize_angle(self.map_to_odom_yaw + odom_yaw)
|
||||
self.get_logger().info(
|
||||
"odom_tf_state aligned=%s odom=(%.3f,%.3f,%.3f) "
|
||||
"map_pose=(%.3f,%.3f,%.3f) map_to_odom=(%.3f,%.3f,%.3f)"
|
||||
% (
|
||||
self.have_transform,
|
||||
odom_x,
|
||||
odom_y,
|
||||
odom_yaw,
|
||||
map_x,
|
||||
map_y,
|
||||
map_yaw,
|
||||
self.map_to_odom_x,
|
||||
self.map_to_odom_y,
|
||||
self.map_to_odom_yaw,
|
||||
)
|
||||
)
|
||||
|
||||
def _publish_status(self, text):
|
||||
msg = String()
|
||||
msg.data = text
|
||||
self.status_pub.publish(msg)
|
||||
self.get_logger().info(text)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = None
|
||||
try:
|
||||
node = OdomMapTf()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user