def init_node(self, ns="~"):
self.imu_pose = rospy.get_param(ns + "imu_pose")
self.imu_pose = n2g(self.imu_pose, "Pose3")
self.imu_rot = self.imu_pose.rotation()
# Parameters for Node
self.dvl_max_velocity = rospy.get_param(ns + "dvl_max_velocity")
self.keyframe_duration = rospy.get_param(ns + "keyframe_duration")
self.keyframe_translation = rospy.get_param(ns +
"keyframe_translation")
self.keyframe_rotation = rospy.get_param(ns + "keyframe_rotation")
# Subscribers and caches
self.imu_sub = Subscriber(IMU_TOPIC, Imu)
self.dvl_sub = Subscriber(DVL_TOPIC, DVL)
self.depth_sub = Subscriber(DEPTH_TOPIC, Depth)
self.depth_cache = Cache(self.depth_sub, 1)
# Use point cloud for visualization
self.traj_pub = rospy.Publisher(LOCALIZATION_TRAJ_TOPIC,
PointCloud2,
queue_size=10)
self.odom_pub = rospy.Publisher(LOCALIZATION_ODOM_TOPIC,
Odometry,
queue_size=10)
# Sync
self.ts = ApproximateTimeSynchronizer([self.imu_sub, self.dvl_sub],
200, 0.1)
self.ts.registerCallback(self.callback)
self.tf = tf.TransformBroadcaster()
loginfo("Localization node is initialized")
def initROS(self):
# publishers
self.vel_costmap_topic_pub = rospy.Publisher(
self.vel_costmap_topic_name,
OccupancyGrid,
queue_size=10,
latch=True)
self.origin_topic_pub = rospy.Publisher(self.origin_topic_name,
PoseStamped,
queue_size=10)
self.vis_marker_topic_pub = rospy.Publisher(self.vis_marker_topic_name,
Marker,
queue_size=1)
self.vel_constraints_topic_pub = rospy.Publisher(
self.vel_constraints_topic_name, Float64MultiArray, queue_size=1)
# service clients
# none here
# subscribers and listeners
self.qtc_state_topic_sub = Subscriber(self.qtc_state_topic_name,
String)
self.qtc_cache = Cache(self.qtc_state_topic_sub,
10,
allow_headerless=True)
self.human_tracking_topic_sub = Subscriber(
self.human_tracking_topic_name, PeopleTracker)
subs = [self.qtc_state_topic_sub, self.human_tracking_topic_sub]
self.ts = ApproximateTimeSynchronizer(subs,
60,
slop=0.1,
allow_headerless=True)
self.ts.registerCallback(self.callback)
self.listenerBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.listenerBuffer)
def test_all_funcs(self):
sub = Subscriber("/empty", String)
cache = Cache(sub, 5)
msg = AnonymMsg()
msg.header.stamp = rospy.Time(0)
cache.add(msg)
msg = AnonymMsg()
msg.header.stamp = rospy.Time(1)
cache.add(msg)
msg = AnonymMsg()
msg.header.stamp = rospy.Time(2)
cache.add(msg)
msg = AnonymMsg()
msg.header.stamp = rospy.Time(3)
cache.add(msg)
msg = AnonymMsg()
msg.header.stamp = rospy.Time(4)
cache.add(msg)
l = len(cache.getInterval(rospy.Time(2.5), rospy.Time(3.5)))
self.assertEquals(l, 1, "invalid number of messages" + " returned in getInterval 1")
l = len(cache.getInterval(rospy.Time(2), rospy.Time(3)))
self.assertEquals(l, 2, "invalid number of messages" + " returned in getInterval 2")
l = len(cache.getInterval(rospy.Time(0), rospy.Time(4)))
self.assertEquals(l, 5, "invalid number of messages" + " returned in getInterval 3")
s = cache.getElemAfterTime(rospy.Time(0)).header.stamp
self.assertEqual(s, rospy.Time(0), "invalid msg return by getElemAfterTime")
s = cache.getElemBeforeTime(rospy.Time(3.5)).header.stamp
self.assertEqual(s, rospy.Time(3), "invalid msg return by getElemBeforeTime")
s = cache.getLastestTime()
self.assertEqual(s, rospy.Time(4), "invalid stamp return by getLastestTime")
s = cache.getOldestTime()
self.assertEqual(s, rospy.Time(0), "invalid stamp return by getOldestTime")
# Add another msg to fill the buffer
msg = AnonymMsg()
msg.header.stamp = rospy.Time(5)
cache.add(msg)
# Test that it discarded the right one
s = cache.getOldestTime()
self.assertEqual(s, rospy.Time(1), "wrong message discarded")
class LocalizationNode(object):
def __init__(self):
self.pose = None
self.prev_time = None
self.prev_vel = None
self.keyframes = []
# Force yaw at origin to be aligned with x axis
self.imu_yaw0 = None
self.imu_pose = [0, 0, 0, -np.pi / 2, 0, 0]
self.imu_rot = None
self.dvl_max_velocity = 0.3
# Create a new key pose when
# - |ti - tj| > min_duration and
# - |xi - xj| > max_translation or
# - |ri - rj| > max_rotation
self.keyframe_duration = None
self.keyframe_translation = None
self.keyframe_rotation = None
self.dvl_error_timer = 0.0
def init_node(self, ns="~"):
self.imu_pose = rospy.get_param(ns + "imu_pose")
self.imu_pose = n2g(self.imu_pose, "Pose3")
self.imu_rot = self.imu_pose.rotation()
# Parameters for Node
self.dvl_max_velocity = rospy.get_param(ns + "dvl_max_velocity")
self.keyframe_duration = rospy.get_param(ns + "keyframe_duration")
self.keyframe_translation = rospy.get_param(ns +
"keyframe_translation")
self.keyframe_rotation = rospy.get_param(ns + "keyframe_rotation")
# Subscribers and caches
self.imu_sub = Subscriber(IMU_TOPIC, Imu)
self.dvl_sub = Subscriber(DVL_TOPIC, DVL)
self.depth_sub = Subscriber(DEPTH_TOPIC, Depth)
self.depth_cache = Cache(self.depth_sub, 1)
# Use point cloud for visualization
self.traj_pub = rospy.Publisher(LOCALIZATION_TRAJ_TOPIC,
PointCloud2,
queue_size=10)
self.odom_pub = rospy.Publisher(LOCALIZATION_ODOM_TOPIC,
Odometry,
queue_size=10)
# Sync
self.ts = ApproximateTimeSynchronizer([self.imu_sub, self.dvl_sub],
200, 0.1)
self.ts.registerCallback(self.callback)
self.tf = tf.TransformBroadcaster()
loginfo("Localization node is initialized")
def callback(self, imu_msg, dvl_msg):
depth_msg = self.depth_cache.getLast()
if depth_msg is None:
return
dd_delay = (depth_msg.header.stamp - dvl_msg.header.stamp).to_sec()
if abs(dd_delay) > 1.0:
logdebug("Missing depth message for {}".format(dd_delay))
rot = r2g(imu_msg.orientation)
# nRb = nRs * bRs^-1
rot = rot.compose(self.imu_rot.inverse())
if self.imu_yaw0 is None:
self.imu_yaw0 = rot.yaw()
rot = gtsam.Rot3.Ypr(rot.yaw() - self.imu_yaw0, rot.pitch(),
rot.roll())
vel = np.array(
[dvl_msg.velocity.x, dvl_msg.velocity.y, dvl_msg.velocity.z])
if np.any(np.abs(vel) > self.dvl_max_velocity):
if self.pose:
self.dvl_error_timer += (dvl_msg.header.stamp -
self.prev_time).to_sec()
if self.dvl_error_timer > 5.0:
logwarn(
"DVL velocity ({:.1f}, {:.1f}, {:.1f}) exceeds max velocity {:.1f} for {:.1f} secs."
.format(
vel[0],
vel[1],
vel[2],
self.dvl_max_velocity,
self.dvl_error_timer,
))
vel = self.prev_vel
else:
return
else:
self.dvl_error_timer = 0.0
if self.pose:
dt = (dvl_msg.header.stamp - self.prev_time).to_sec()
dv = (vel + self.prev_vel) * 0.5
trans = dv * dt
local_point = gtsam.Point2(trans[0], trans[1])
pose2 = gtsam.Pose2(self.pose.x(), self.pose.y(),
self.pose.rotation().yaw())
point = pose2.transform_from(local_point)
self.pose = gtsam.Pose3(
rot, gtsam.Point3(point.x(), point.y(), depth_msg.depth))
else:
self.pose = gtsam.Pose3(rot, gtsam.Point3(0, 0, depth_msg.depth))
self.prev_time = dvl_msg.header.stamp
self.prev_vel = vel
omega = imu_msg.angular_velocity
omega = np.array([omega.x, omega.y, omega.z])
self.prev_omega = self.imu_rot.matrix().dot(omega)
new_keyframe = False
if not self.keyframes:
new_keyframe = True
else:
duration = self.prev_time.to_sec() - self.keyframes[-1][0]
if duration > self.keyframe_duration:
odom = self.keyframes[-1][1].between(self.pose)
odom = g2n(odom)
translation = np.linalg.norm(odom[:3])
rotation = abs(odom[-1])
if (translation > self.keyframe_translation
or rotation > self.keyframe_rotation):
new_keyframe = True
if new_keyframe:
self.keyframes.append((self.prev_time.to_sec(), self.pose))
self.publish_pose(new_keyframe)
def publish_pose(self, publish_traj=False):
if self.pose is None:
return
header = rospy.Header()
header.stamp = self.prev_time
header.frame_id = "odom"
odom_msg = Odometry()
odom_msg.header = header
# pose in odom frame
odom_msg.pose.pose = g2r(self.pose)
# twist in local frame
odom_msg.child_frame_id = "base_link"
# Local planer behaves worse
# odom_msg.twist.twist.linear.x = self.prev_vel[0]
# odom_msg.twist.twist.linear.y = self.prev_vel[1]
# odom_msg.twist.twist.linear.z = self.prev_vel[2]
# odom_msg.twist.twist.angular.x = self.prev_omega[0]
# odom_msg.twist.twist.angular.y = self.prev_omega[1]
# odom_msg.twist.twist.angular.z = self.prev_omega[2]
odom_msg.twist.twist.linear.x = 0
odom_msg.twist.twist.linear.y = 0
odom_msg.twist.twist.linear.z = 0
odom_msg.twist.twist.angular.x = 0
odom_msg.twist.twist.angular.y = 0
odom_msg.twist.twist.angular.z = 0
self.odom_pub.publish(odom_msg)
p = odom_msg.pose.pose.position
q = odom_msg.pose.pose.orientation
self.tf.sendTransform((p.x, p.y, p.z), (q.x, q.y, q.z, q.w),
header.stamp, "base_link", "odom")
if publish_traj:
traj = np.array([g2n(pose) for _, pose in self.keyframes])
traj_msg = ros_colorline_trajectory(traj)
traj_msg.header = header
self.traj_pub.publish(traj_msg)
def test_all_funcs(self):
sub = Subscriber("/empty", String)
cache = Cache(sub, 5)
msg = AnonymMsg()
msg.header.stamp = rospy.Time(0)
cache.add(msg)
msg = AnonymMsg()
msg.header.stamp = rospy.Time(1)
cache.add(msg)
msg = AnonymMsg()
msg.header.stamp = rospy.Time(2)
cache.add(msg)
msg = AnonymMsg()
msg.header.stamp = rospy.Time(3)
cache.add(msg)
msg = AnonymMsg()
msg.header.stamp = rospy.Time(4)
cache.add(msg)
l = len(cache.getInterval(rospy.Time(2.5), rospy.Time(3.5)))
self.assertEquals(
l, 1, "invalid number of messages" + " returned in getInterval 1")
l = len(cache.getInterval(rospy.Time(2), rospy.Time(3)))
self.assertEquals(
l, 2, "invalid number of messages" + " returned in getInterval 2")
l = len(cache.getInterval(rospy.Time(0), rospy.Time(4)))
self.assertEquals(
l, 5, "invalid number of messages" + " returned in getInterval 3")
s = cache.getElemAfterTime(rospy.Time(0)).header.stamp
self.assertEqual(s, rospy.Time(0),
"invalid msg return by getElemAfterTime")
s = cache.getElemBeforeTime(rospy.Time(3.5)).header.stamp
self.assertEqual(s, rospy.Time(3),
"invalid msg return by getElemBeforeTime")
s = cache.getLatestTime()
self.assertEqual(s, rospy.Time(4),
"invalid stamp return by getLatestTime")
self.assertEqual(
s, cache.getLastestTime(),
"stamps returned by getLatestTime and getLastestTime don't match")
s = cache.getOldestTime()
self.assertEqual(s, rospy.Time(0),
"invalid stamp return by getOldestTime")
# Add another msg to fill the buffer
msg = AnonymMsg()
msg.header.stamp = rospy.Time(5)
cache.add(msg)
# Test that it discarded the right one
s = cache.getOldestTime()
self.assertEqual(s, rospy.Time(1), "wrong message discarded")
def test_headerless(self):
sub = Subscriber("/empty", String)
cache = Cache(sub, 5, allow_headerless=False)
msg = String()
cache.add(msg)
self.assertIsNone(cache.getElemAfterTime(rospy.Time(0)),
"Headerless message invalidly added.")
cache = Cache(sub, 5, allow_headerless=True)
rospy.rostime.set_rostime_initialized(True)
rospy.rostime._set_rostime(rospy.Time(0))
cache.add(msg)
s = cache.getElemAfterTime(rospy.Time(0))
self.assertEqual(s, msg, "invalid msg returned in headerless scenario")
s = cache.getElemAfterTime(rospy.Time(1))
self.assertIsNone(s, "invalid msg returned in headerless scenario")
rospy.rostime._set_rostime(rospy.Time(2))
cache.add(msg)
s = cache.getInterval(rospy.Time(0), rospy.Time(1))
self.assertEqual(s, [msg],
"invalid msg returned in headerless scenario")
s = cache.getInterval(rospy.Time(0), rospy.Time(2))
self.assertEqual(s, [msg, msg],
"invalid msg returned in headerless scenario")
def test_headerless(self):
sub = Subscriber("/empty", String)
cache = Cache(sub, 5, allow_headerless=False)
msg = String()
cache.add(msg)
self.assertIsNone(cache.getElemAfterTime(rospy.Time(0)),
"Headerless message invalidly added.")
cache = Cache(sub, 5, allow_headerless=True)
rospy.rostime.set_rostime_initialized(True)
rospy.rostime._set_rostime(rospy.Time(0))
cache.add(msg)
s = cache.getElemAfterTime(rospy.Time(0))
self.assertEqual(s, msg,
"invalid msg returned in headerless scenario")
s = cache.getElemAfterTime(rospy.Time(1))
self.assertIsNone(s, "invalid msg returned in headerless scenario")
rospy.rostime._set_rostime(rospy.Time(2))
cache.add(msg)
s = cache.getInterval(rospy.Time(0), rospy.Time(1))
self.assertEqual(s, [msg],
"invalid msg returned in headerless scenario")
s = cache.getInterval(rospy.Time(0), rospy.Time(2))
self.assertEqual(s, [msg, msg],
"invalid msg returned in headerless scenario")
def test_all_funcs(self):
sub = Subscriber(self.node, String, "/empty")
cache = Cache(sub, 5)
msg = AnonymMsg()
msg.header.stamp = Time(seconds=0)
cache.add(msg)
msg = AnonymMsg()
msg.header.stamp = Time(seconds=1)
cache.add(msg)
msg = AnonymMsg()
msg.header.stamp = Time(seconds=2)
cache.add(msg)
msg = AnonymMsg()
msg.header.stamp = Time(seconds=3)
cache.add(msg)
msg = AnonymMsg()
msg.header.stamp = Time(seconds=4)
cache.add(msg)
l = len(cache.getInterval(Time(seconds=2.5), Time(seconds=3.5)))
self.assertEqual(
l, 1, "invalid number of messages" + " returned in getInterval 1")
l = len(cache.getInterval(Time(seconds=2), Time(seconds=3)))
self.assertEqual(
l, 2, "invalid number of messages" + " returned in getInterval 2")
l = len(cache.getInterval(Time(), Time(seconds=4)))
self.assertEqual(
l, 5, "invalid number of messages" + " returned in getInterval 5")
s = cache.getElemAfterTime(Time()).header.stamp
self.assertEqual(s, Time(), "invalid msg return by getElemAfterTime")
s = cache.getElemBeforeTime(Time(seconds=3.5)).header.stamp
self.assertEqual(s, Time(seconds=3),
"invalid msg return by getElemBeforeTime")
s = cache.getLastestTime()
self.assertEqual(s, Time(seconds=4),
"invalid stamp return by getLastestTime")
s = cache.getOldestTime()
self.assertEqual(s, Time(), "invalid stamp return by getOldestTime")
# Add another msg to fill the buffer
msg = AnonymMsg()
msg.header.stamp = Time(seconds=5)
cache.add(msg)
# Test that it discarded the right one
s = cache.getOldestTime()
self.assertEqual(s, Time(seconds=1), "wrong message discarded")
def test_headerless(self):
sub = Subscriber(self.node, String, "/empty")
cache = Cache(sub, 5, allow_headerless=False)
msg = String()
cache.add(msg)
self.assertIsNone(
cache.getElemAfterTime(Time(clock_type=ClockType.ROS_TIME)),
"Headerless message invalidly added.")
cache = Cache(sub, 5, allow_headerless=True)
cache.add(msg)
s = cache.getElemAfterTime(Time(clock_type=ClockType.ROS_TIME))
self.assertEqual(s, msg, "invalid msg returned in headerless scenario")
currentRosTime = ROSClock().now()
s = cache.getElemAfterTime(currentRosTime)
self.assertIsNone(s, "invalid msg returned in headerless scenario")
cache.add(msg)
s = cache.getInterval(Time(clock_type=ClockType.ROS_TIME),
currentRosTime)
self.assertEqual(s, [msg],
"invalid msg returned in headerless scenario")
s = cache.getInterval(Time(clock_type=ClockType.ROS_TIME),
(currentRosTime + Duration(seconds=2)))
self.assertEqual(s, [msg, msg],
"invalid msg returned in headerless scenario")