def odom_callback(self, msg):
'''
Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
'''
# Lock thread
self.lock.acquire()
# Save time
self.time = msg.header.stamp
# Translation
trans = (msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=msg.header.stamp,
child='/base_footprint',
parent='/world')
# Incremental odometry
if self.last_odom is not None:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# self.uk = np.array([delta_x,delta_y, angle_wrap(yaw)])
# Odometry seen from vehicle frame
self.uk = np.array([
delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
-delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
angle_wrap(yaw - lyaw)
])
# Flag available
self.cur_odom = msg
self.new_odom = True
# Save initial odometry for increment
else:
self.last_odom = msg
# Unlock thread
self.lock.release()
def odom_callback(self, msg):
'''
Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
'''
# Lock thread
self.lock.acquire()
# Save time
self.time = msg.header.stamp
# Translation
trans = (msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = msg.header.stamp,
child = '/base_footprint',
parent = '/world')
# Incremental odometry
if self.last_odom is not None:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# Odometry seen from vehicle frame
self.uk = np.array([delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
-delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
angle_wrap(yaw - lyaw)])
# Flag available
self.cur_odom = msg
self.new_odom = True
# Save initial odometry for increment
else:
self.last_odom = msg
# Unlock thread
self.lock.release()
def odom_callback(self, msg):
'''
Calculates the incremental odometry as seen from the vehicle frame.
'''
# Check if first data
if self.last_odom is None:
# Save
self.last_odom = msg
# Translation
self.trans = (msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
self.rot = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
# Calculate and publish increment
else:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# Publish
odom = IncrementalOdometry2D()
odom.header.stamp = msg.header.stamp
odom.header.frame_id = '/estimated_position'
odom.delta_x = +delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw)
odom.delta_y = -delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw)
odom.delta_a = angle_wrap(yaw - lyaw)
self.pub_odom.publish(odom)
# For next loop
self.last_odom = msg
# World transform
self.tf_broad.sendTransform(translation=self.trans,
rotation=self.rot,
time=msg.header.stamp,
parent='/odom',
child='/world')
def odom_callback(self, msg):
'''
Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
'''
# Save time
self.time = msg.header.stamp
# Translation
trans = (msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation=self.robot2sensor,
rotation=(0, 0, 0, 1),
time=msg.header.stamp,
child='/sensor',
parent='/robot')
self.tfBroad.sendTransform(translation=self.robot2sensor,
rotation=(0, 0, 0, 1),
time=msg.header.stamp,
child='/camera_depth_frame',
parent='/base_footprint')
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=msg.header.stamp,
child='/base_footprint',
parent='/odom')
self.tfBroad.sendTransform(
translation=(self.x0[0], self.x0[1], 0),
rotation=tf.transformations.quaternion_from_euler(
0, 0, self.x0[2]),
time=msg.header.stamp,
child='/odom',
parent='/world')
# Incremental odometry
if self.last_odom is not None and not self.new_odom:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# Odometry seen from vehicle frame
self.uk = np.array([
delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
-delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
angle_wrap(yaw - lyaw)
])
# Flag
self.new_odom = True
# For next loop
self.last_odom = msg
if self.last_odom is None:
self.last_odom = msg
def cbk_odom(self, msg):
"""Publish tf and calculate incremental odometry."""
# Save time
self.odomtime = msg.header.stamp
self.odom = msg
# Translation
trans = (msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation=self.robot2sensor,
rotation=(0, 0, 0, 1),
time=msg.header.stamp,
child='sensor',
parent='robot')
self.tfBroad.sendTransform(translation=self.robot2sensor,
rotation=(0, 0, 0, 1),
time=msg.header.stamp,
child='camera_depth_frame',
parent='base_footprint')
self.tfBroad.sendTransform(translation=trans,
rotation=rot,
time=msg.header.stamp,
child='base_footprint',
parent='world')
self.tfBroad.sendTransform(translation=(0, 0, 0),
rotation=(0, 0, 0, 1),
time=msg.header.stamp,
child='odom',
parent='world')
# Incremental odometry
if self.last_odom is not None:
# Increment computation
delta_x = msg.pose.pose.position.x - \
self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - \
self.last_odom.pose.pose.position.y
yaw = funcs.yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = funcs.yaw_from_quaternion(
self.last_odom.pose.pose.orientation)
# Odometry seen from vehicle frame
self.uk = np.array([delta_x*np.cos(lyaw) + delta_y*np.sin(lyaw),
-delta_x*np.sin(lyaw) + delta_y*np.cos(lyaw),
funcs.angle_wrap(yaw - lyaw)])
# Flag available
self.new_odom = True
# Save initial odometry for increment
else:
self.last_odom = msg
def odom_callback(self, msg):
'''
Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame.
'''
# Save time
self.time = msg.header.stamp
# Translation
trans = (msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z)
# Rotation
rot = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
# Publish transform
self.tfBroad.sendTransform(translation = self.robot2sensor,
rotation = (0, 0, 0, 1),
time = msg.header.stamp,
child = '/sensor',
parent = '/robot')
self.tfBroad.sendTransform(translation = self.robot2sensor,
rotation = (0, 0, 0, 1),
time = msg.header.stamp,
child = '/camera_depth_frame',
parent = '/base_footprint')
self.tfBroad.sendTransform(translation = trans,
rotation = rot,
time = msg.header.stamp,
child = '/base_footprint',
parent = '/odom')
self.tfBroad.sendTransform(translation = (self.x0[0],self.x0[1],0),
rotation = tf.transformations.quaternion_from_euler(0,0,self.x0[2]),
time = msg.header.stamp,
child = '/odom',
parent = '/world')
# Incremental odometry
if self.last_odom is not None and not self.new_odom:
# Increment computation
delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x
delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y
yaw = yaw_from_quaternion(msg.pose.pose.orientation)
lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation)
# Odometry seen from vehicle frame
self.uk = np.array([delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw),
- delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw),
angle_wrap(yaw - lyaw)])
# Flag
self.new_odom = True
# For next loop
self.last_odom = msg
if self.last_odom is None:
self.last_odom = msg