def cb(self, msg): odom = Odometry() odom.header = Header() odom.header = msg.header odom.pose.pose = msg.pose self.pubodom.publish(odom)
def odom_callback(self, msg):
odom_rotated = Odometry()
odom = msg
pose = odom.pose
twist = odom.twist
#tranform twist into odom frame
linear_twist = twist.twist.linear
angular_twist = twist.twist.angular
twist_rotated = TwistWithCovariance()
transformed_linear = self.transform_meas_to_world(
tl(linear_twist), odom.child_frame_id, odom.header.frame_id,
rospy.Time(0), False)
transformed_angular = self.transform_meas_to_world(
tl(angular_twist), odom.child_frame_id, odom.header.frame_id,
rospy.Time(0), False)
twist_rotated.twist.linear = tm(transformed_linear, Vector3)
twist_rotated.twist.angular = tm(transformed_angular, Vector3)
twist_rotated.covariance = twist.covariance #may need to be transformed
odom_rotated.header = odom.header
odom_rotated.child_frame_id = odom.child_frame_id
odom_rotated.pose = pose
odom_rotated.twist = twist_rotated
self.rotated_twist_pub.publish(odom_rotated)
def agentCallback(self, data):
# msg = self.transform(data)
pose = PoseStamped()
pose.header = data.header
pose.pose = data.pose.pose
vector = Vector3Stamped()
vector.header = data.header
vector.vector = data.twist.twist.linear
try:
pose = self.tl.transformPose('world', pose)
vector = self.tl.transformVector3('world', vector)
except:
print 'no transform to world frame'
return
msg = Odometry()
msg.header = data.header
msg.header.frame_id = 'world'
msg.child_frame_id = data.child_frame_id
msg.pose.pose = pose.pose
msg.twist.twist.linear = vector.vector
self.agentPublisher.publish(msg)
def pub_ekf_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.child_frame_id = 'base_footprint'
odom.pose = msg.pose
self.ekf_pub.publish(odom)
def publish_ground_truth():
rospy.init_node('pose_ground_truth')
odom_pub = rospy.Publisher('/pose_ground_truth', Odometry, queue_size=10)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
odom = Odometry()
header = Header()
header.frame_id = 'odom'
child_frame = 'base_link'
model = GetModelStateRequest()
model.model_name = 'rrbot'
r = rospy.Rate(20)
while not rospy.is_shutdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
header.stamp = rospy.Time.now()
odom.header = header
odom.child_frame_id = child_frame
odom_pub.publish(odom)
r.sleep()
def odom_transform_2d(self, data, new_frame, trans, rot):
# NOTES: only in 2d rotation
# also, it removes covariance, etc information
odom_new = Odometry()
odom_new.header = data.header
odom_new.header.frame_id = new_frame
odom_new.pose.pose.position.x = data.pose.pose.position.x + trans[0]
odom_new.pose.pose.position.y = data.pose.pose.position.y + trans[1]
odom_new.pose.pose.position.z = data.pose.pose.position.z + trans[2]
# rospy.loginfo('td: %s tr: %s' % (str(type(data)), str(type(rot))))
q = data.pose.pose.orientation
q_tuple = (q.x, q.y, q.z, q.w,)
# Quaternion(*(tft quaternion)) converts a tf-style quaternion to a ROS msg quaternion
odom_new.pose.pose.orientation = Quaternion(*tft.quaternion_multiply(q_tuple, rot))
heading_change = quaternion_to_heading(rot)
odom_new.twist.twist.linear.x = data.twist.twist.linear.x*math.cos(heading_change) - data.twist.twist.linear.y*math.sin(heading_change)
odom_new.twist.twist.linear.y = data.twist.twist.linear.y*math.cos(heading_change) + data.twist.twist.linear.x*math.sin(heading_change)
odom_new.twist.twist.linear.z = 0
odom_new.twist.twist.angular.x = 0
odom_new.twist.twist.angular.y = 0
odom_new.twist.twist.angular.z = data.twist.twist.angular.z
return odom_new
def publish_pose(self):
"""
Append dead reckoning from Localization to SLAM estimate to achieve realtime TF.
"""
pose_msg = PoseWithCovarianceStamped()
pose_msg.header.stamp = self.current_frame.time
pose_msg.header.frame_id = "map"
pose_msg.pose.pose = g2r(self.current_frame.pose3)
cov = 1e-4 * np.identity(6, np.float32)
# FIXME Use cov in current_frame
cov[np.ix_((0, 1, 5), (0, 1, 5))] = self.current_keyframe.transf_cov
pose_msg.pose.covariance = cov.ravel().tolist()
self.pose_pub.publish(pose_msg)
o2m = self.current_frame.pose3.compose(
self.current_frame.dr_pose3.inverse())
o2m = g2r(o2m)
p = o2m.position
q = o2m.orientation
self.tf.sendTransform(
(p.x, p.y, p.z),
[q.x, q.y, q.z, q.w],
self.current_frame.time,
"odom",
"map",
)
odom_msg = Odometry()
odom_msg.header = pose_msg.header
odom_msg.pose.pose = pose_msg.pose.pose
odom_msg.child_frame_id = "base_link"
odom_msg.twist.twist = self.current_frame.twist
self.odom_pub.publish(odom_msg)
def visualize(self):
#print 'Visualizing...'
self.state_lock.acquire()
self.inferred_pose = self.expected_pose()
if isinstance(self.inferred_pose, np.ndarray):
self.publish_tf(self.inferred_pose)
ps = PoseStamped()
ps.header = Utils.make_header("map")
ps.pose.position.x = self.inferred_pose[0]
ps.pose.position.y = self.inferred_pose[1]
ps.pose.orientation = Utils.angle_to_quaternion(self.inferred_pose[2])
if(self.pose_pub.get_num_connections() > 0):
self.pose_pub.publish(ps)
if(self.pub_odom.get_num_connections() > 0):
odom = Odometry()
odom.header = ps.header
odom.pose.pose = ps.pose
self.pub_odom.publish(odom)
if self.particle_pub.get_num_connections() > 0:
if self.particles.shape[0] > self.MAX_VIZ_PARTICLES:
# randomly downsample particles
proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES, p=self.weights)
# proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES)
self.publish_particles(self.particles[proposal_indices,:])
else:
self.publish_particles(self.particles)
if self.pub_laser.get_num_connections() > 0 and isinstance(self.sensor_model.last_laser, LaserScan):
self.sensor_model.last_laser.header.frame_id = "/laser"
self.sensor_model.last_laser.header.stamp = rospy.Time.now()
self.pub_laser.publish(self.sensor_model.last_laser)
self.state_lock.release()
def waypoints():
pub = rospy.Publisher('/odom', Odometry, queue_size=1)
rospy.init_node('odomer', anonymous=True)
rate = rospy.Rate(1) # 10hz
odom = Odometry()
covarian = [
9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0
]
odom.child_frame_id = "odom"
odom.pose.pose.position.x = 1.0
odom.pose.pose.position.y = 0.0
odom.pose.pose.orientation.x = 0.0
odom.pose.pose.orientation.y = 0.0
odom.pose.pose.orientation.z = 0.0
odom.pose.pose.orientation.w = 1.0
odom.pose.covariance = covarian
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
odom.header = h
odom.twist.twist.linear.x = 0.0
odom.twist.twist.linear.y = 0.0
odom.twist.twist.linear.z = 0.0
odom.twist.twist.angular.x = 0.0
odom.twist.twist.angular.y = 0.0
odom.twist.twist.angular.z = 0.0
odom.twist.covariance = covarian
while not rospy.is_shutdown():
pub.publish(odom)
rate.sleep()
def pub_ekf_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.child_frame_id = 'base_footprint'
odom.pose = msg.pose
self.ekf_pub.publish(odom)
def process(bag_file):
bag_path, bag_name = os.path.split(bag_file)
bag_out = os.path.join(bag_path, os.path.splitext(bag_name)[0] + "_p.bag")
print "Opening bag ", bag_file, "."
bag_in = rosbag.Bag(bag_file, 'r')
num_messages = bag_in.get_message_count()
print "Done. Num messages: ", num_messages, "."
bag_out = rosbag.Bag(bag_out, 'w')
i = 0
for topic, msg, t in bag_in.read_messages():
if topic == '/pose_imu':
m = Odometry()
m.header = msg.header
m.child_frame_id = "IMU/GPS"
m.pose.pose = msg.pose
msg = m
bag_out.write(topic, msg, t)
i += 1
if i % 1000 == 0:
print "Processed ", i, "/", num_messages, " messages."
bag_out.close()
def main(model_name):
rospy.init_node('odom_pub')
print(model_name)
odom_pub = rospy.Publisher("/position_" + model_name,
Odometry,
queue_size=10)
rospy.wait_for_service('/gazebo/get_model_state')
get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
odom = Odometry()
header = Header()
header.frame_id = '/odom'
model = GetModelStateRequest()
model.model_name = model_name
r = rospy.Rate(100)
while not rospy.is_shutdown():
result = get_model_srv(model)
odom.pose.pose = result.pose
odom.twist.twist = result.twist
header.stamp = rospy.Time.now()
odom.header = header
odom_pub.publish(odom)
print(model_name)
r.sleep()
def publish(self, x, y):
msg = Odometry()
msg.header = Header()
msg.header.frame_id = 'target_pose'
msg.pose.pose.position.x = x
msg.pose.pose.position.y = y
self.target_pose_pub.publish(msg)
def callback_state(data):
global state
state = Pose2D()
state.x = data.x
state.y = data.y
state.theta = data.yaw
msg = OdometryState()
msg_ros = Odometry()
msg.header = data.header
msg_ros.header = data.header
msg.state.pose.pose.position.x = data.x
msg.state.pose.pose.position.y = data.y
[x, y, z, w] = quaternion_from_euler(state.theta, 0, 0)
msg.state.pose.pose.orientation.x = x
msg.state.pose.pose.orientation.y = y
msg.state.pose.pose.orientation.z = z
msg.state.pose.pose.orientation.w = w
msg_ros.pose.pose.position.x = data.x
msg_ros.pose.pose.position.y = data.y
[x, y, z, w] = quaternion_from_euler(state.theta, 0, 0)
msg_ros.pose.pose.orientation.x = x
msg_ros.pose.pose.orientation.y = y
msg_ros.pose.pose.orientation.z = z
msg_ros.pose.pose.orientation.w = w
v = np.array([data.vx, data.vy])
msg.velocity = np.sqrt(np.sum(np.square(v)))
pub_state.publish(msg)
pub_state_ros.publish(msg_ros)
def puhlish_odom_data_to_f110(self):
'''publish odometry data '''
loop_rate = rospy.Rate(self.PUBLISH_RATE)
while not rospy.is_shutdown():
if self.car_state is not None:
current_state = self.car_state
#localized state
posedata = PoseStamped()
posedata.header = self.create_header('map')
posedata.pose.position.x = current_state.x
posedata.pose.position.y = current_state.y
quat_data = quaternion_from_euler(0, 0, current_state.yaw)
posedata.pose.orientation.z = quat_data[2]
posedata.pose.orientation.w = quat_data[3]
self.pose_pub.publish(posedata)
#odometry message
cmd = Odometry()
cmd.header = self.create_header('map')
cmd.child_frame_id = 'base_link'
cmd.twist.twist.linear.x = np.linalg.norm(
np.array([current_state.vx, current_state.vy]))
self.odom_pub.publish(cmd)
loop_rate.sleep()
def computeOdom(self):
while True:
self.computeSpeed()
xdot = (1 / 2) * math.cos(self.theta) * self.speedRight + (
1 / 2) * math.cos(self.theta) * self.speedLeft
ydot = (1 / 2) * math.sin(self.theta) * self.speedRight + (
1 / 2) * math.sin(self.theta) * self.speedLeft
thetadot = (1.0 / WHEEL_BASE) * self.speedRight - (
1.0 / WHEEL_BASE) * self.speedLeft
self.x = self.x + 0.05 * xdot
self.y = self.y + 0.05 * ydot
self.theta = self.theta + 0.05 * thetadot
q = euler2quat(0.0, 0.0, self.theta)
odom = Odometry()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = 'odom'
odom.header = h
odom.child_frame_id = 'base_link'
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.orientation.x = q[1]
odom.pose.pose.orientation.y = q[2]
odom.pose.pose.orientation.z = q[3]
odom.pose.pose.orientation.w = q[0]
self.odomPub.publish(odom)
time.sleep(0.05)
def pose_cb(data):
global path
global count
global path_len, x_prev, y_prev, z_prev
x = data.pose.position.x
y = data.pose.position.y
z = data.pose.position.z
dist = ((x - x_prev)**2 + (y - y_prev)**2 + (z - z_prev)**2)**0.5
#Temporary value of 0.5 used for eliminating jumps. CHANGE THIS LATER
if dist < 0.5:
path_len = path_len + dist
else:
path_len = 0
x_prev, y_prev, z_prev = x, y, z
#print("Path Length = "+str(path_len))
path.header = data.header
path.poses.append(data)
path_pub.publish(path)
count = (count + 1) % 20
if not count:
odom = Odometry()
odom.header = data.header
odom.pose.pose = data.pose
odom_pub.publish(odom)
print("Path Length = " + str(path_len))
def pub_ekf_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.child_frame_id = 'angelbot_base'
odom.pose = msg.pose
self.ekf_pub.publish(odom)
def vioCallback(self, uav_odom):
tf_odom = Odometry()
tf_odom.header = uav_odom.header
x = uav_odom.pose.pose.position.x
y = uav_odom.pose.pose.position.y
z = uav_odom.pose.pose.position.z
pos = (x, y, z)
x = uav_odom.pose.pose.orientation.x
y = uav_odom.pose.pose.orientation.y
z = uav_odom.pose.pose.orientation.z
w = uav_odom.pose.pose.orientation.w
qua = (x, y, z, w)
trans = tf.transformations.translation_matrix(pos)
rot = tf.transformations.quaternion_matrix(qua)
uav_pose_r = np.matmul(trans, rot) # uav's pose in robot coordinates
vio_pose_r = np.matmul(self.T_robot2vio,
uav_pose_r) # vio's pose in robot coordinates
vio_pose_c = np.matmul(vio_pose_r, self.T_vio2robot)
tf_pos = tf.transformations.translation_from_matrix(vio_pose_c)
tf_qua = tf.transformations.quaternion_from_matrix(vio_pose_c)
tf_odom.pose.pose.position.x = tf_pos[0]
tf_odom.pose.pose.position.y = tf_pos[1]
tf_odom.pose.pose.position.z = tf_pos[2]
tf_odom.pose.pose.orientation.x = tf_qua[0]
tf_odom.pose.pose.orientation.y = tf_qua[1]
tf_odom.pose.pose.orientation.z = tf_qua[2]
tf_odom.pose.pose.orientation.w = tf_qua[3]
self.pub_vio.publish(tf_odom)
def start(self):
rospy.init_node(self.vehicle_type + '_' + self.vehicle_id +
"_communication")
rate = rospy.Rate(100)
'''
main ROS thread
'''
while not rospy.is_shutdown():
self.target_motion_pub.publish(self.target_motion)
try:
response = self.gazeboModelstate(
self.vehicle_type + '_' + self.vehicle_id, 'ground_plane')
except rospy.ServiceException, e:
print "Gazebo model state service call failed: %s" % e
odom = Odometry()
odom.header = response.header
odom.pose.pose = response.pose
odom.twist.twist = response.twist
self.odom_groundtruth_pub.publish(odom)
if (self.flight_mode is
"LAND") and (self.local_pose.pose.position.z < 0.15):
if (self.disarm()):
self.flight_mode = "DISARMED"
rate.sleep()
def pub_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.child_frame_id = self.base_frame
odom.pose = msg.pose
self.odom_pub.publish(odom)
def pose_update_cb(pose_data):
"""
Publishes odometry information
:param pose_data: geometry_msgs/PoseWithCovarianceStamped, current pose data from hector_mapping package.
:return:
"""
# Broadcast transform base_link -> odom
odom_broadcaster.sendTransform(
(pose_data.pose.pose.position.x, pose_data.pose.pose.position.y,
pose_data.pose.pose.position.z),
(pose_data.pose.pose.orientation.x, pose_data.pose.pose.orientation.y,
pose_data.pose.pose.orientation.z, pose_data.pose.pose.orientation.w),
pose_data.header.stamp, "base_link", "odom")
# Next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header = pose_data.header
odom.header.frame_id = "odom"
# Set the position
odom.pose = pose_data.pose
# Set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(vel_x, 0, 0), Vector3(0, 0, vel_yaw))
# Publish the message
odom_pub.publish(odom)
def callback_odom(data):
# get current transorm
if tlisten.frameExists(_base_frame_name) and tlisten.frameExists(
_map_frame_name):
_t = tlisten.getLatestCommonTime(_base_frame_name, _map_frame_name)
try:
position, quaternion = tlisten.lookupTransform(
_map_frame_name, _base_frame_name, rospy.Time())
except tf.ExtrapolationException as e:
rospy.logwarn('Extrapolation Warning')
return None
# publish odom
_msg = Odometry()
_msg.header = data.header
_msg.header.frame_id = _map_frame_name
_msg.child_frame_id = _base_frame_name
_msg.pose.pose.orientation.x = quaternion[0]
_msg.pose.pose.orientation.y = quaternion[1]
_msg.pose.pose.orientation.z = quaternion[2]
_msg.pose.pose.orientation.w = quaternion[3]
_msg.pose.pose.position.x = position[0]
_msg.pose.pose.position.y = position[1]
_msg.pose.pose.position.z = position[2]
_msg.twist.twist.linear.x = data.twist.twist.linear.x
_msg.twist.twist.angular.z = data.twist.twist.linear.x
pub.publish(_msg)
else:
rospy.logwarn('Frame does not exist : %s, %s', _base_frame_name,
_map_frame_name)
def pose_callback(self, pose_msg):
'''
Uses a pose message to generate an odometry and state message.
:param pose_msg: (geometry_msgs/PoseStamped) pose message
'''
new_pose_msg, trans, rot = self.tf_to_pose("LO01_base_link", "map",
pose_msg.header)
if not self.use_amcl:
pose_msg = new_pose_msg
self.new_pose_pub.publish(
pose_msg
) # if using AMCL, this will be the same as the original pose message
if trans and rot: # if not getting tf, trans and rot will be None
footprint = PolygonStamped()
footprint.header = pose_msg.header # has same frame_id (map) and time stamp
loomo_points = np.array([[0.16, -0.31], [0.16, 0.31],
[-0.16, 0.31], [-0.16, -0.31]])
roll, pitch, yaw = tf.transformations.euler_from_quaternion(rot)
rot = np.array([[np.cos(yaw), -np.sin(yaw)],
[np.sin(yaw), np.cos(yaw)]])
rotated_points = np.matmul(rot, loomo_points.T) # 2x4 array
rot_and_trans = rotated_points + np.array([[trans[0]], [trans[1]]])
polygon = Polygon(
points=[Point32(x=x, y=y, z=0) for x, y in rot_and_trans.T])
footprint.polygon = polygon
self.footprint_pub.publish(footprint)
odom_msg = Odometry()
odom_msg.pose.pose = pose_msg.pose
odom_msg.header = pose_msg.header
self.odom_pub.publish(odom_msg)
state_msg = State()
state_msg.header = pose_msg.header
state_msg.state_stamp = pose_msg.header.stamp
state_msg.pos.x = pose_msg.pose.position.x
state_msg.pos.y = pose_msg.pose.position.y
state_msg.pos.z = pose_msg.pose.position.z
state_msg.quat.x = pose_msg.pose.orientation.x
state_msg.quat.y = pose_msg.pose.orientation.y
state_msg.quat.z = pose_msg.pose.orientation.z
state_msg.quat.w = pose_msg.pose.orientation.w
if self.last_state_time and self.last_state:
dt = pose_msg.header.stamp.nsecs - self.last_state_time
state_msg.vel.x = (state_msg.pos.x -
self.last_state.pos.x) / (float(dt) / 10**9)
state_msg.vel.y = (state_msg.pos.y -
self.last_state.pos.y) / (float(dt) / 10**9)
state_msg.vel.z = 0 # ground robot not traveling in z direction
self.last_state_time = pose_msg.header.stamp.nsecs
self.last_state = state_msg
self.state_pub.publish(state_msg)
def pub_ekf_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.header.frame_id = '/odom'
odom.child_frame_id = 'base_link'
odom.pose = msg.pose
self.ekf_pub.publish(odom)
def publish_tf(self, pose, stamp=None):
""" Publish a tf for the car. This tells ROS where the car is with respect to the map. """
if stamp == None:
stamp = rospy.Time.now()
# this may cause issues with the TF tree. If so, see the below code.
# self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]),
# stamp , "/laser", "/map")
# also publish odometry to facilitate getting the localization pose
if self.PUBLISH_ODOM:
odom = Odometry()
odom.header = Utils.make_header("/map", stamp)
odom.pose.pose.position.x = pose[0]
odom.pose.pose.position.y = pose[1]
odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2])
self.odom_pub.publish(odom)
# return # below this line is disabled
"""
Our particle filter provides estimates for the "laser" frame
since that is where our laser range estimates are measure from. Thus,
We want to publish a "map" -> "laser" transform.
However, the car's position is measured with respect to the "base_link"
frame (it is the root of the TF tree). Thus, we should actually define
a "map" -> "base_link" transform as to not break the TF tree.
"""
# Get map -> laser transform.
map_laser_pos = np.array((pose[0], pose[1], 0))
map_laser_rotation = np.array(
tf.transformations.quaternion_from_euler(0, 0, pose[2]))
# Apply laser -> base_link transform to map -> laser transform
# This gives a map -> base_link transform
laser_base_link_offset = (0.265, 0, 0)
map_laser_pos -= np.dot(
tf.transformations.quaternion_matrix(
tf.transformations.unit_vector(map_laser_rotation))[:3, :3],
laser_base_link_offset).T
map_laser_pos[0] -= self.position[0]
map_laser_pos[1] -= self.position[1]
orientation_list = [
self.odom_orientation.x, self.odom_orientation.y,
self.odom_orientation.z, self.odom_orientation.w
]
(roll, pitch, odom_base_link_yaw
) = tf.transformations.euler_from_quaternion(orientation_list)
map_odom_yaw = pose[2] - odom_base_link_yaw
map_odom_rotation = np.array(
tf.transformations.quaternion_from_euler(0, 0, map_odom_yaw))
# Publish transform
self.pub_tf.sendTransform(map_laser_pos, map_odom_rotation, stamp,
"/odom", "/map")
def publish_tf(self, pose, stamp=None):
""" Publish a tf for the car. This tells ROS where the car is with respect to the map. """
if stamp == None:
stamp = rospy.Time.now()
# this may cause issues with the TF tree. If so, see the below code.
# self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]),
# stamp , "/laser", "/map")
# also publish odometry to facilitate getting the localization pose
if self.PUBLISH_ODOM:
odom = Odometry()
odom.header = Utils.make_header("/map", stamp)
odom.pose.pose.position.x = pose[0]
odom.pose.pose.position.y = pose[1]
odom.pose.pose.orientation = Utils.yaw_to_quaternion(pose[2])
self.odom_pub.publish(odom)
# return # below this line is disabled
"""
Our particle filter provides estimates for the "laser" frame
since that is where our laser range estimates are measure from. Thus,
We want to publish a "map" -> "laser" transform.
However, the car's position is measured with respect to the "base_link"
frame (it is the root of the TF tree). And "odom" to "base_link" transform is published
by vesc_to_odom node. Thus, we should actually define
a "map" -> "odom" transform as to not break the TF tree.
"""
# Get map -> laser transform.
# map_laser_pos = np.array( (pose[0],pose[1],0) )
# map_laser_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2]) )
# Apply laser -> base_link transform to map -> laser transform
# This gives a map -> base_link transform
# laser_base_link_offset = (0.06, 0, 0)
# map_laser_pos -= np.dot(tf.transformations.quaternion_matrix(tf.transformations.unit_vector(map_laser_rotation))[:3,:3], laser_base_link_offset).T
# Publish transform
# self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp , "/base_link", "/map")
# Assuming only translational offset, no rotation
base_link_laser_delta = 0.06
map_base_link_pos_x = pose[0] - base_link_laser_delta * math.cos(
pose[2])
map_base_link_pos_y = pose[1] - base_link_laser_delta * math.sin(
pose[2])
# This assumes the map and odom frames are initially aligned
map_odom_delta = np.array(
(map_base_link_pos_x, map_base_link_pos_y, 0))
map_odom_orientation = np.array(
tf.transformations.quaternion_from_euler(0, 0, pose[2]))
self.pub_tf.sendTransform(map_odom_delta, map_odom_orientation, stamp,
"/base_link", "/map")
def callback(self, data):
lat = data.latitude
lon = data.longitude
alt = data.altitude
e, n, self.zone_number, self.zone_letter = utm.from_latlon(lat, lon)
if not self.start_pos_was_set:
self.start_e = e
self.start_n = n
"""
if self.offset:
self.start_e -= self.offset.translation.x
self.start_n -= self.offset.translation.y
"""
if self.offset:
self.start_e -= 0.4
self.start_n -= 0.4
self.start_pos_was_set = True
self.start_alt = data.altitude
position = Point()
position.x = e - self.start_e
position.y = n - self.start_n
position.z = 0.0
erel = e - self.start_e # relative position to start position in meters
nrel = n - self.start_n
alt_rel = alt - self.start_alt
# empty as the gps data doesnt include orientation
orientation = Quaternion()
orientation.w = 1.0
mypose = Pose()
mypose.orientation = orientation
mypose.position = position
if self.fix_covariance:
covariance = np.array([1.5, 0, 0, 0, 1.5, 0, 0, 0, 3])
else:
covariance = data.position_covariance
covariance = np.reshape(covariance, (3, 3))
cov6x6 = np.zeros((6, 6))
cov6x6[:3, :3] = covariance
cov6x6_flat = np.reshape(cov6x6, (36, )).astype(np.float64)
pose_with_cov = PoseWithCovariance()
pose_with_cov.covariance = cov6x6_flat
pose_with_cov.pose = mypose
odom = Odometry()
odom.pose = pose_with_cov
odom.header = data.header
odom.header.stamp = data.header.stamp
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "map"
# odom.twist stays empty as gps doesn't have this data
self.publisher.publish(odom)
def callback(pose):
# cov_east = navsatfix.position_covariance[0]
# cov_north = navsatfix.position_covariance[4]
# if cov_east>cov_thresh or cov_north>cov_thresh:
# return
odom = Odometry()
odom.pose.pose = pose.pose
odom.header = pose.header
pub.publish(odom)
def transition(self, state, twist, dt):
desired = state[1]
current = state[0]
# do something
n = Odometry()
n.header = current.header
n.twist.twist = self.update_twist(current.twist.twist, twist)
n.pose.pose = self.update_pose(current.pose.pose, current.twist.twist, n.twist.twist, dt)
return (n, desired, )
def _on_receive_twist(self, twist_stamped_msg):
"""Receive TwistStamped message from simulation and assign to odom"""
odom = Odometry()
odom.twist.twist = twist_stamped_msg.twist
odom.header = twist_stamped_msg.header
odom.header.frame_id = 'odom'
odom.child_frame_id = "dvl_link"
odom.header.stamp.secs = rospy.get_rostime().secs
odom.header.stamp.nsecs = rospy.get_rostime().nsecs
self.odom_pub.publish(odom)
def publish_pose_estimate(self, pose):
"""
Converts from pose estimate to odometry messsage
"""
odom = Odometry()
odom.header = make_header("map")
odom.pose.pose.position.x = pose[0]
odom.pose.pose.position.y = pose[1]
odom.pose.pose.orientation = angle_to_quaternion(pose[2])
self.odom_pub.publish(odom)
def handle_img(self, msg):
starttime = time.time()
image_np = self.cv_bridge.imgmsg_to_cv2(msg, "bgr8")
if image_np.shape[0] != self.intrinsic.shape[0] or image_np.shape[
1] != self.intrinsic.shape[1]:
print(
'The intrinsic parameter does not match the image parameter!')
return
if self.last_img is not None:
pose_msg = PoseStamped()
pose_msg.header.stamp = msg.header.stamp
pose_msg.header.frame_id = 'map'
sample = {
'img1': self.last_img,
'img2': image_np,
'intrinsic': self.intrinsic
}
sample = self.transform(sample)
sample['img1'] = sample['img1'][None] # increase the dimension
sample['img2'] = sample['img2'][None]
sample['intrinsic'] = sample['intrinsic'][None]
motion, _ = self.tartanvo.test_batch(sample)
motion = motion[0]
# adjust the scale if available
# if self.scale!=1:
# trans = motion[:3]
# trans = trans / np.linalg.norm(trans) * self.scale
# motion[:3] = trans
print(self.scale)
motion_mat = se2SE(motion)
self.pose = self.pose * motion_mat
quat = SO2quat(self.pose[0:3, 0:3])
pose_msg.pose.position.x = self.scale * self.pose[0, 3]
pose_msg.pose.position.y = self.scale * self.pose[1, 3]
pose_msg.pose.position.z = self.scale * self.pose[2, 3]
pose_msg.pose.orientation.x = quat[0]
pose_msg.pose.orientation.y = quat[1]
pose_msg.pose.orientation.z = quat[2]
pose_msg.pose.orientation.w = quat[3]
self.pose_pub.publish(pose_msg)
odom_msg = Odometry()
odom_msg.header = pose_msg.header
odom_msg.pose.pose = pose_msg.pose
self.odom_pub.publish(odom_msg)
self.last_img = image_np.copy()
print(" call back time: {}:".format(time.time() - starttime))
def callbackOdom(msg):
global have_first
global first_odom
if not have_first:
first_odom = msg
have_first = True
print "first_odom: " + str(first_odom.pose.x)
odom_msg = Odometry()
odom_msg.header = msg.header
odom_msg.header.frame_id = 'odom'
odom_msg.child_frame_id = msg.child_frame_id
odom_msg.child_frame_id = 'base_link'
odom_msg.pose.pose.position.x = msg.pose.x - first_odom.pose.x
odom_msg.pose.pose.position.y = msg.pose.y - first_odom.pose.y
# Wheel radius.
# TODO(gbrooks): Parameterize.
odom_msg.pose.pose.position.z = 0.127
q = tf.transformations.quaternion_from_euler(
0, 0, msg.pose.theta - first_odom.pose.theta)
odom_msg.pose.pose.orientation.x = q[0]
odom_msg.pose.pose.orientation.y = q[1]
odom_msg.pose.pose.orientation.z = q[2]
odom_msg.pose.pose.orientation.w = q[3]
odom_msg.pose.covariance = [4, 0, 0, 0, 0, 0, \
0, 4, 0, 0, 0, 0, \
0, 0, 4, 0, 0, 0, \
0, 0, 0, 0.1, 0, 0, \
0, 0, 0, 0, 0.1, 0, \
0, 0, 0, 0, 0, 0.1]
odom_msg.twist.twist.linear.x = msg.twist.vx
odom_msg.twist.twist.linear.y = msg.twist.vy
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 = msg.twist.vtheta
odom_msg.twist.covariance = [0.1, 0, 0, 0, 0, 0, \
0, 0.1, 0, 0, 0, 0, \
0, 0, 0.1, 0, 0, 0, \
0, 0, 0, 0.03, 0, 0, \
0, 0, 0, 0, 0.03, 0, \
0, 0, 0, 0, 0, 0.03]
odom_pub.publish(odom_msg)
odom_br = tf.TransformBroadcaster()
odom_br.sendTransform(
(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, 0),
tf.transformations.quaternion_from_euler(
0, 0, msg.pose.theta - first_odom.pose.theta), rospy.Time.now(),
"base_link_odom_wheel", "odom")
def __init__(self, rand_size):
self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=2)
self.odom = None
self.carrot_sub = rospy.Subscriber("/lqrrt/ref", Odometry, self.set_odom)
fprint("Shaking hands and taking names.")
rospy.sleep(1)
# We need to publish an inital odom message for lqrrt
start_ori = trns.quaternion_from_euler(0, 0, np.random.normal() * 3.14)
start_pos = np.append(np.random.uniform(rand_size, size=(2)), 1)
start_pose = navigator_tools.numpy_quat_pair_to_pose(start_pos, start_ori)
start_odom = Odometry()
start_odom.header = navigator_tools.make_header(frame='enu')
start_odom.child_frame_id = 'base_link'
start_odom.pose.pose = start_pose
self.odom_pub.publish(start_odom)
def default(self, ci='unused'):
odometry = Odometry()
odometry.header = self.get_ros_header()
odometry.child_frame_id = self.child_frame_id
# fill pose and twist
odometry.pose.pose = self.get_pose()
odometry.twist.twist = self.get_twist()
self.publish(odometry)
# send current odometry transform
self.sendTransform(self.get_position(),
self.get_orientation(),
odometry.header.stamp,
odometry.child_frame_id,
odometry.header.frame_id)
def unpackOdom(self, data):
odom_msg = Odometry()
# Unpack Header
odom_msg.header = self.unpackOdomHeader(data[0:20])
# Unpack Child_frame_id
# Is constant and "base_link"
cfid = unpack("xxxxccccccccc", data[20:33])
# odom_msg.child_frame_id = (cfid[0] + cfid[1] + cfid[2] +
# cfid[3] + cfid[4] + cfid[5] + cfid[6] + cfid[7] + cfid[8])
odom_msg.child_frame_id = "base_footprint"
# Unpack Pose
pose = self.bytestr_to_array(data[33:89])
odom_msg.pose.pose.position.x = pose[0]
odom_msg.pose.pose.position.y = pose[1]
odom_msg.pose.pose.position.z = pose[2]
odom_msg.pose.pose.orientation.x = pose[3]
odom_msg.pose.pose.orientation.y = pose[4]
odom_msg.pose.pose.orientation.z = pose[5]
odom_msg.pose.pose.orientation.w = pose[6]
# Skip Unpack Pose Covariance of Pose
# The values are all zeros since we did not know how to properly
# calculate them. We are not properly sending the correct covarinace
# maxtrix. Ours is 3x3 where it should be 6x6. To skip the covaraince
# we will skip 4+ (9*8) = 76 bytes
odom_msg.pose.covariance = covariance
# Unpack Twist
twist = self.bytestr_to_array(data[165:213])
odom_msg.twist.twist.linear.x = twist[0]
odom_msg.twist.twist.linear.y = twist[1]
odom_msg.twist.twist.linear.z = twist[2]
odom_msg.twist.twist.angular.x = twist[3]
odom_msg.twist.twist.angular.y = twist[4]
odom_msg.twist.twist.angular.z = twist[5]
# Skip Unpack Twist Covariance of Pose
# The values are all zeros since we did not know how to properly
# calculate them. We are not properly sending the correct covarinace
# maxtrix. Ours is 3x3 where it should be 6x6. To skip the covaraince
# we will skip 4+ (9*8) = 76 bytes
odom_msg.twist.covariance = covariance
return odom_msg
def subCB(msg_in):
global pub
msg_out = Odometry()
msg_out.header = msg_in.header
msg_out.header.frame_id = "Pioneer3AT/odom"
msg_out.child_frame_id = "imu"
cart = LLA2Naive(msg_in.LLA.x, msg_in.LLA.y, msg_in.LLA.z)
msg_out.pose.pose.position.x = cart[0]
msg_out.pose.pose.position.y = cart[1]
msg_out.pose.pose.position.z = cart[2]
q = tf.transformations.quaternion_from_euler(pi * msg_in.RPY.x / 180.0,
pi * msg_in.RPY.y / 180.0,
pi * msg_in.RPY.z / -180.0 )
msg_out.pose.pose.orientation = Quaternion(*q)
pub.publish(msg_out)
def publish_tf(self,pose, stamp=None):
""" Publish a tf for the car. This tells ROS where the car is with respect to the map. """
if stamp == None:
stamp = rospy.Time.now()
# this may cause issues with the TF tree. If so, see the below code.
self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]),
stamp , "/laser", "/map")
# also publish odometry to facilitate getting the localization pose
if self.PUBLISH_ODOM:
odom = Odometry()
odom.header = Utils.make_header("/map", stamp)
odom.pose.pose.position.x = pose[0]
odom.pose.pose.position.y = pose[1]
odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2])
self.odom_pub.publish(odom)
return # below this line is disabled
"""
Our particle filter provides estimates for the "laser" frame
since that is where our laser range estimates are measure from. Thus,
We want to publish a "map" -> "laser" transform.
However, the car's position is measured with respect to the "base_link"
frame (it is the root of the TF tree). Thus, we should actually define
a "map" -> "base_link" transform as to not break the TF tree.
"""
# Get map -> laser transform.
map_laser_pos = np.array( (pose[0],pose[1],0) )
map_laser_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2]) )
# Apply laser -> base_link transform to map -> laser transform
# This gives a map -> base_link transform
laser_base_link_offset = (0.265, 0, 0)
map_laser_pos -= np.dot(tf.transformations.quaternion_matrix(tf.transformations.unit_vector(map_laser_rotation))[:3,:3], laser_base_link_offset).T
# Publish transform
self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp , "/base_link", "/map")
def main():
parser = argparse.ArgumentParser(description="Parse the images.txt file to "
"create a rosbag with several "
"PoseStamped messages.")
parser.add_argument('-i', '--input',
help='file with poses (images.txt)',
metavar='poses_filename',
default='./images.txt',
type=str,
required=True
)
parser.add_argument('-o', '--output',
help='output bag file (with location)',
metavar='bag_filename',
type=str,
required=True
)
parser.add_argument('-y',
help='if images_adjusted.txt is found in the same folder'
'as the supplied filename, then use it without'
'asking',
metavar='True/False',
type=bool,
default=False,
choices=[True, False]
)
arguments = parser.parse_args()
images_folder, _ = os.path.split(arguments.input)
alt_file = os.path.join(images_folder, 'images_adjusted.txt')
if os.path.exists(alt_file):
if arguments.y:
images_file = open(alt_file)
else:
reply = None
while reply not in ['y', 'n']:
print("The images_adjusted.txt file is in the folder {}, do you"
" want to use that instead? [y/n]".format(images_folder))
reply = raw_input()
if reply == 'y':
images_file = open(alt_file)
else:
images_file = open(arguments.input)
else:
images_file = open(arguments.input)
print("Processing data from {}...".format(images_file.name))
with rosbag.Bag(arguments.output, 'w') as outputbag:
for lineno, line in enumerate(images_file):
#lines must be of the form:
#image_basename,t_android,t_movidius,r11,r12,r12,t1,r21,r22,r23,t2,r31,r32,r33,t3
tokens = line.strip('\n').split(',')
if (len(tokens)) != 15:
print("Line {0} appears to be wrong: \"{1}\" ".format(lineno, tokens))
continue
#first elements before transformation
image_name = tokens[0]
ts1 = float(tokens[1])
ts2 = float(tokens[2])
#getting the quaterion out the rotation matrix
rotation_matrix = np.array(map(float, tokens[3:])).reshape(3, 4)
rotation_matrix = np.vstack((rotation_matrix, [0, 0, 0, 1]))
quaternion = transformations.quaternion_from_matrix(rotation_matrix)
#creating the pose
p = PoseStamped()
p.header.frame_id = "/tango/world"
p.header.stamp = rospy.Time.from_seconds(ts1)
p.header.seq = lineno
p.pose.position.x = rotation_matrix[0, 3]
p.pose.position.y = rotation_matrix[1, 3]
p.pose.position.z = rotation_matrix[2, 3]
p.pose.orientation.x = quaternion[0]
p.pose.orientation.y = quaternion[1]
p.pose.orientation.z = quaternion[2]
p.pose.orientation.w = quaternion[3]
outputbag.write("tango_imu_pose", p, rospy.Time.from_seconds(ts1))
#creating the odometry entry
#TODO get covariances from the covariances.txt file
o = Odometry()
o.header = p.header
o.child_frame_id = o.header.frame_id
o.pose.pose = p.pose
outputbag.write("tango_imu_odom", o, rospy.Time.from_seconds(ts1))
#creating the tf messages
tfmsg = tfMessage()
tf_stamped = TransformStamped()
tf_stamped.header.frame_id = "/tango/world"
tf_stamped.header.seq = lineno
tf_stamped.header.stamp = rospy.Time.from_seconds(ts1)
tf_stamped.child_frame_id = "/tango/imu"
tf_stamped.transform.translation.x = rotation_matrix[0, 3]
tf_stamped.transform.translation.y = rotation_matrix[1, 3]
tf_stamped.transform.translation.z = rotation_matrix[2, 3]
tf_stamped.transform.rotation = p.pose.orientation
tfmsg.transforms.append(tf_stamped)
outputbag.write("tf", tfmsg, rospy.Time.from_seconds(ts1))
print("Bag creation complete, bagfile: {}".format(arguments.output))
def callback(data):
odom = Odometry()
odom.header = data.header
odom.child_frame_id = child_frame_id
odom.pose = data.pose
pub.publish(odom)
def odom_remap(odom_msg):
global namespace
global previous_noiseless_pose
global previous_noiseless_odom
global previous_noiseless_time
# Make our received odom reading more palatable
current_odom = Pose2D()
current_odom.x = odom_msg.pose.pose.position.x
current_odom.y = odom_msg.pose.pose.position.y
current_odom.theta = convert_quaternion_to_yaw(odom_msg.pose.pose.orientation)
current_time = odom_msg.header.stamp
if previous_noiseless_time is None:
previous_noiseless_time = copy.deepcopy(current_time)
# If we have no previous_pose, set to (0, 0, 0)
if previous_noiseless_pose is None:
previous_noiseless_pose = Pose2D()
previous_noiseless_pose.x = 0
previous_noiseless_pose.y = 0
previous_noiseless_pose.theta = 0
# Create the previous_odom Pose2D if it doesn't exist, also (0, 0, 0)
if previous_noiseless_odom is None:
previous_noiseless_odom = Pose2D()
previous_noiseless_odom.x = 0
previous_noiseless_odom.y = 0
previous_noiseless_odom.theta = 0
noiseless_odom = Odometry()
noiseless_odom.header = odom_msg.header
noiseless_odom.child_frame_id = namespace + 'base_footprint_filter'
# Split movement into three distinct actions, rotate -> translate -> rotate
delta_rotation_1 = helpers.correct_angle(math.atan2(current_odom.y - previous_noiseless_odom.y, current_odom.x - previous_noiseless_odom.x) - previous_noiseless_odom.theta)
delta_translation = math.sqrt((previous_noiseless_odom.x - current_odom.x) ** 2 + (previous_noiseless_odom.y - current_odom.y) ** 2)
delta_rotation_2 = helpers.correct_angle(current_odom.theta - previous_noiseless_odom.theta - delta_rotation_1)
noiseless_odom.pose.pose.position.x = previous_noiseless_pose.x + delta_translation * math.cos(previous_noiseless_pose.theta + delta_rotation_1)
noiseless_odom.pose.pose.position.y = previous_noiseless_pose.y + delta_translation * math.sin(previous_noiseless_pose.theta + delta_rotation_1)
noiseless_odom_yaw = previous_noiseless_pose.theta + delta_rotation_1 + delta_rotation_2
noiseless_odom.pose.pose.orientation = convert_yaw_to_quaternion(noiseless_odom_yaw)
delta_time = current_time - previous_noiseless_time
delta_time_seconds = delta_time.to_sec()
assert delta_time_seconds >= 0
# We will assume that our robot moves perfectly with only linear x velocity and angular z velocity
# This is an okay assumption because we ignore other data anyways because all readings are noise and not valid
noiseless_odom.twist.twist.linear.y = 0
noiseless_odom.twist.twist.linear.z = 0
noiseless_odom.twist.twist.angular.x = 0
noiseless_odom.twist.twist.angular.y = 0
if delta_time_seconds > 0:
noiseless_odom.twist.twist.linear.x = delta_translation / delta_time_seconds
noiseless_odom.twist.twist.angular.z = (noiseless_odom_yaw - previous_noiseless_pose.theta) / delta_time_seconds
else:
noiseless_odom.twist.twist.linear.x = 0
noiseless_odom.twist.twist.angular.z = 0
# From documentation on nav_msgs/Odometry:
# Row-major representation of the 6x6 covariance matrix
# The orientation parameters use a fixed-axis representation.
# In order, the parameters are:
# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
noiseless_odom.pose.covariance = odom_msg.pose.covariance
# Twist covariance copied from position covariance TODO this definitely wrong
noiseless_odom.twist.covariance = odom_msg.pose.covariance
# Publish odom message
try:
odom_publisher.publish(noiseless_odom)
except rospy.ROSException as e:
rospy.logwarn(e.message)
# Increment previous pose for use with next message
previous_noiseless_pose.x = noiseless_odom.pose.pose.position.x
previous_noiseless_pose.y = noiseless_odom.pose.pose.position.y
previous_noiseless_pose.theta = convert_quaternion_to_yaw(noiseless_odom.pose.pose.orientation)
# Increment previous odom for use with next message
previous_noiseless_odom.x = current_odom.x
previous_noiseless_odom.y = current_odom.y
previous_noiseless_odom.theta = current_odom.theta
# Increment time
previous_noiseless_time = current_time
def noisy_odom_remap(odom_msg):
global namespace
global previous_noisy_pose
global previous_noisy_odom
global previous_noisy_time
# Odometry model taken from Probabilistic Robotics by Thrun et al.
# Algorithm used is sample_motion_model_odometry from Table 5.6
# Robot specific noise parameters
# Default value = 0.02
alpha_1 = 0.02
alpha_2 = 0.02
alpha_3 = 0.02
alpha_4 = 0.02
# Make our received odom reading more palatable
current_odom = Pose2D()
current_odom.x = odom_msg.pose.pose.position.x
current_odom.y = odom_msg.pose.pose.position.y
current_odom.theta = convert_quaternion_to_yaw(odom_msg.pose.pose.orientation)
current_time = odom_msg.header.stamp
if previous_noisy_time is None:
previous_noisy_time = copy.deepcopy(current_time)
# If we have no previous_pose, set to (0, 0, 0)
if previous_noisy_pose is None:
previous_noisy_pose = Pose2D()
previous_noisy_pose.x = 0
previous_noisy_pose.y = 0
previous_noisy_pose.theta = 0
# Create the previous_odom Pose2D if it doesn't exist, also (0, 0, 0)
if previous_noisy_odom is None:
previous_noisy_odom = Pose2D()
previous_noisy_odom.x = 0
previous_noisy_odom.y = 0
previous_noisy_odom.theta = 0
noisy_odom_x_list = []
noisy_odom_y_list = []
noisy_odom_yaw_list = []
noisy_odom_x_vel_list = []
noisy_odom_y_vel_list = []
noisy_odom_yaw_vel_list = []
noisy_odom = Odometry()
noisy_odom.header = odom_msg.header
noisy_odom.child_frame_id = namespace + 'base_footprint_filter'
for i in range(1, 1000):
# Split movement into three distinct actions, rotate -> translate -> rotate
delta_rotation_1 = helpers.correct_angle(math.atan2(current_odom.y - previous_noisy_odom.y, current_odom.x - previous_noisy_odom.x) - previous_noisy_odom.theta)
delta_translation = math.sqrt((previous_noisy_odom.x - current_odom.x) ** 2 + (previous_noisy_odom.y - current_odom.y) ** 2)
delta_rotation_2 = helpers.correct_angle(current_odom.theta - previous_noisy_odom.theta - delta_rotation_1)
# Create random pose from given movements by adding noise
std_dev_1 = alpha_1 * abs(delta_rotation_1) + alpha_2 * delta_translation
delta_rotation_1_hat = delta_rotation_1 - normal(scale=std_dev_1)
std_dev_2 = alpha_3 * delta_translation + alpha_4 * (abs(delta_rotation_1) + abs(delta_rotation_2))
delta_translation_hat = delta_translation - normal(scale=std_dev_2)
std_dev_3 = alpha_1 * abs(delta_rotation_2) + alpha_2 * delta_translation
delta_rotation_2_hat = delta_rotation_2 - normal(scale=std_dev_3)
noisy_odom.pose.pose.position.x = previous_noisy_pose.x + delta_translation_hat * math.cos(previous_noisy_pose.theta + delta_rotation_1_hat)
noisy_odom.pose.pose.position.y = previous_noisy_pose.y + delta_translation_hat * math.sin(previous_noisy_pose.theta + delta_rotation_1_hat)
noisy_odom_yaw = previous_noisy_pose.theta + delta_rotation_1_hat + delta_rotation_2_hat
noisy_odom.pose.pose.orientation = convert_yaw_to_quaternion(noisy_odom_yaw)
delta_time = current_time - previous_noisy_time
delta_time_seconds = delta_time.to_sec()
assert delta_time_seconds >= 0
# We will assume that our robot moves perfectly with only linear x velocity and angular z velocity
# This is an okay assumption because we ignore other data anyways because all readings are noise and not valid
noisy_odom.twist.twist.linear.y = 0
noisy_odom.twist.twist.linear.z = 0
noisy_odom.twist.twist.angular.x = 0
noisy_odom.twist.twist.angular.y = 0
if delta_time_seconds > 0:
noisy_odom.twist.twist.linear.x = delta_translation_hat / delta_time_seconds
noisy_odom.twist.twist.angular.z = (noisy_odom_yaw - previous_noisy_pose.theta) / delta_time_seconds
else:
noisy_odom.twist.twist.linear.x = 0
noisy_odom.twist.twist.angular.z = 0
noisy_odom_x_list.append(noisy_odom.pose.pose.position.x)
noisy_odom_y_list.append(noisy_odom.pose.pose.position.y)
noisy_odom_yaw_list.append(noisy_odom_yaw)
noisy_odom_x_vel_list.append(noisy_odom.twist.twist.linear.x)
noisy_odom_y_vel_list.append(noisy_odom.twist.twist.linear.y)
noisy_odom_yaw_vel_list.append(noisy_odom.twist.twist.angular.z)
# From documentation on nav_msgs/Odometry:
# Row-major representation of the 6x6 covariance matrix
# The orientation parameters use a fixed-axis representation.
# In order, the parameters are:
# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
noisy_odom.pose.covariance = [numpy.var(noisy_odom_x_list), 0, 0, 0, 0, 0, # x variance taken from Gazebo odometry
0, numpy.var(noisy_odom_y_list), 0, 0, 0, 0, # y variance taken from Gazebo odometry
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, numpy.var(noisy_odom_yaw_list)] # yaw variance taken from Gazebo odometry
# Twist covariance copied from position covariance TODO this definitely wrong
noisy_odom.twist.covariance = [numpy.var(noisy_odom_x_vel_list), 0, 0, 0, 0, 0, # x_vel variance
0, numpy.var(noisy_odom_y_vel_list), 0, 0, 0, 0, # y_vel variance
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, numpy.var(noisy_odom_yaw_vel_list)] # yaw_vel variance
# Publish noisy odom message
try:
rospy.logdebug('Publishing noisy odom message')
noisy_odom_publisher.publish(noisy_odom)
except rospy.ROSException as e:
rospy.logwarn(e.message)
rospy.logdebug('Calculated new noisy odom pose. Previous pose was ' + str(previous_noisy_pose) + ',\n previous odom was ' +
str(previous_noisy_odom) + ',\n current odom was ' + str(current_odom) + ',\n and new noisy pose is ' +
str(noisy_odom))
rospy.logdebug('d_rot_1 noise was ' + str(delta_rotation_1_hat - delta_rotation_1) + '\n d_trans noise was ' +
str(delta_translation_hat - delta_translation) + '\n d_rot_2 noise was ' +
str(delta_rotation_2_hat - delta_rotation_2))
# Increment previous pose for use with next message
previous_noisy_pose.x = noisy_odom.pose.pose.position.x
previous_noisy_pose.y = noisy_odom.pose.pose.position.y
previous_noisy_pose.theta = convert_quaternion_to_yaw(noisy_odom.pose.pose.orientation)
# Increment previous odom for use with next message
previous_noisy_odom.x = current_odom.x
previous_noisy_odom.y = current_odom.y
previous_noisy_odom.theta = current_odom.theta
# Increment time
previous_noisy_time = current_time
while not rospy.is_shutdown(): if opts.useoriginref: file_string = 'scan'+format(counter_index,'05d')+'.pcd' else: file_string = 'scanorg'+format(counter_index,'05d')+'.pcd' file_string = os.path.join(opts.dir_prefix, file_string) pcl_cloud = pcl.load(file_string) pcloud = PointCloud2() header = Header() header.stamp = rospy.Time.now() if opts.useoriginref: header.frame_id = 'map' else: header.frame_id = 'velodyne' #'odom' #'pose' #'frame' #'velodyne' pcloud = pc2.create_cloud_xyz32(header, pcl_cloud.to_array()) #pose = graph[str(counter_index)][0] index_graph = graph_node_names.index(str(counter_index)) pose = graph.nodes[index_graph].pose print pose odom = Odometry() odom.header = Header() odom.header.frame_id = 'map' odom.header.stamp = rospy.Time.now() odom.child_frame_id = 'velodyne' odom.pose.pose = pose pub_frame.publish(pcloud) pub_odom.publish(odom) pub_tf.sendTransform((pose.position.x, pose.position.y, pose.position.z), (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w), rospy.Time.now(),'velodyne','map') counter_index = counter_index + 1 rate.sleep()
def callback(input):
output = Odometry()
output.header = input.header
output.pose.pose = input.pose
pub.publish(output)
