def execute(self):
while not rospy.is_shutdown():
with self.tf_lock:
if len(self.tf_buffer) > 0:
self.tf_buffer.sort(key = lambda msg: msg.header.stamp.to_sec()) # It seems that transformations in tfMessage needs to be sorted by timestamp
self.broadcaster.publish(tfMessage(self.tf_buffer))
self.tf_buffer = []
self.r.sleep()
def to_tf(estimate):
time = rospy.Time.now()
msg = tfMessage()
for camera in estimate.cameras:
msg.transforms.append(pose_to_transform(camera.pose, camera.camera_id, time))
for target, target_id in zip(estimate.targets, range(len(estimate.targets))):
msg.transforms.append(pose_to_transform(target, 'target_%d'%target_id, time))
return msg
def callback(msg, filt):
filt = tfMessage()
for transform in msg.transforms:
if 'map' in transform.header.frame_id:
filt.transforms.append(transform)
if 'odom' in transform.header.frame_id:
filt.transforms.append(transform)
if len(filt.transforms) > 0:
pub.publish(filt)
def read_nav_csv(fname, origin):
''' Reads the nav.csv , converts into a local coordinate frame and returns tf msgs '''
msgs = []
DEG2RAD = m.pi/180.0
with open(fname, 'r') as f:
# example data
# 1354679075.295000000,6248548.85357,332949.839026,-41.4911136152,-0.00740605127066,-0.0505019649863,1.95254564285
# a Nx7 matrix containing pose [time N E D R P Y] UTM cartesian coordinate system
for i, line in enumerate(f):
words = line.split(',')
time = words[0]
[secs, nsecs] = map(int, time.split('.'))
x = float(words[1])
y = float(words[2])
z = float(words[3])
R = float(words[4])
P = float(words[5])
Y = float(words[6])
# convert to local coordinate frame, located at the origin
x = x - origin[0]
y = y - origin[1]
z = z - origin[2]
# create TF msg
tf_msg = tfMessage()
geo_msg = TransformStamped()
geo_msg.header.stamp = Time(secs,nsecs)
geo_msg.header.seq = i
geo_msg.header.frame_id = "map"
geo_msg.child_frame_id = "body"
geo_msg.transform.translation.x = x
geo_msg.transform.translation.y = y
geo_msg.transform.translation.z = z
angles = tf.transformations.quaternion_from_euler(R,P,Y)
geo_msg.transform.rotation.x = angles[0]
geo_msg.transform.rotation.y = angles[1]
geo_msg.transform.rotation.z = angles[2]
geo_msg.transform.rotation.w = angles[3]
# rviz frame
geo_msg2 = TransformStamped()
geo_msg2.header.stamp = Time(secs,nsecs)
geo_msg2.header.seq = i
geo_msg2.header.frame_id = "map_rviz"
geo_msg2.child_frame_id = "map"
geo_msg2.transform.translation.x = 0
geo_msg2.transform.translation.y = 0
geo_msg2.transform.translation.z = 0
angles = tf.transformations.quaternion_from_euler(DEG2RAD*180, 0, 0) # ax, ay, az
geo_msg2.transform.rotation.x = angles[0]
geo_msg2.transform.rotation.y = angles[1]
geo_msg2.transform.rotation.z = angles[2]
geo_msg2.transform.rotation.w = angles[3]
# push all transformations
tf_msg.transforms.append(geo_msg)
tf_msg.transforms.append(geo_msg2)
msgs.append(tf_msg)
return msgs
def execute(self):
while not rospy.is_shutdown():
with self.tf_lock:
if len(self.tf_buffer) > 0:
self.tf_buffer.sort(
key=lambda msg: msg.header.stamp.to_sec()
) # It seems that transformations in tfMessage needs to be sorted by timestamp
self.broadcaster.publish(tfMessage(self.tf_buffer))
self.tf_buffer = []
self.r.sleep()
def ROSPublishTopics(params):
global messagePages
global messageSendingPage
global pageMutex
global publisherMap
global oldPI
startT = timer()
tfMessages = tfMessage()
seqTf = 0
for message in params:
topic = message['topic']
params = message['params']
if ((topic == '/tf') or (topic == 'tf')):
msg = geometry_msgs.msg.TransformStamped()
msg.header.seq = seqTf
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = params['frame_id']
msg.child_frame_id = params['child_frame_id']
msg.transform.translation.x = float(params['x'])
msg.transform.translation.y = float(params['y'])
msg.transform.translation.z = float(params['z'])
msg.transform.rotation.x = float(params['qx'])
msg.transform.rotation.y = float(params['qy'])
msg.transform.rotation.z = float(params['qz'])
msg.transform.rotation.w = float(params['qw'])
seqTf = seqTf + 1
tfMessages.transforms.append(msg)
elif topic in publisherMap:
publisherEntry = publisherMap[topic]
publisher = publisherEntry[0]
reader = publisherEntry[1]
publisher.publish(reader(params))
elif (topic == 'huh'):
print "Placeholder topic"
else:
print "Unrecognized topic " + topic
TFPublisher.publish(tfMessages)
pageMutex.acquire()
messagePages[messageSendingPage[0]].clear()
messageSendingPage[0] = messageSendingPage[0] ^ 1
pageMutex.release()
print "PI: " + str(
messageSendingPage[0]) + " " + str(oldPI ^ messageSendingPage[0])
if 0 == oldPI ^ messageSendingPage[0]:
print "!!!!!!! FLIP !!!!!!!"
oldPI = messageSendingPage[0]
if messagePages[messageSendingPage[0]] != {}:
print str({"messages": messagePages[messageSendingPage[0]]})
endT = timer()
print "T: " + str(endT - startT)
return {"messages": messagePages[messageSendingPage[0]]}
def __init__(self):
#POSE RECORDE
self.pre_st_x = 0
self.pre_st_y = 0
self.pre_st_w = 0
self._pre_pose = Pose()
self._pre_pose.position.x = 0
self._pre_pose.position.y = 0
self._pre_pose.position.z = 0
self._pre_pose.orientation.x = 0
self._pre_pose.orientation.y = 0
self._pre_pose.orientation.z = 0
self._pre_pose.orientation.w = 0
# Initialise fields
self.estimatedpose = PoseWithCovarianceStamped()
self.occupancy_map = OccupancyGrid()
self.particlecloud = PoseArray()
self.tf_message = tfMessage()
self._update_lock = Lock()
# Parameters
self.ODOM_ROTATION_NOISE = 0 # Odometry model rotation noise
self.ODOM_TRANSLATION_NOISE = 0 # Odometry x axis (forward) noise
self.ODOM_DRIFT_NOISE = 0 # Odometry y axis (side-side) noise
self.NUMBER_PREDICTED_READINGS = 20
# Set 'previous' translation to origin
# All Transforms are given relative to 0,0,0, not in absolute coords.
self.prev_odom_x = 0.0 # Previous odometry translation from origin
self.prev_odom_y = 0.0 # Previous odometry translation from origin
self.prev_odom_heading = 0.0 # Previous heading from odometry data
self.last_odom_pose = None
# Request robot's initial odometry values to be recorded in prev_odom
self.odom_initialised = False
self.sensor_model_initialised = False
# Set default initial pose to initial position and orientation.
self.estimatedpose.pose.pose.position.x = self.INIT_X
self.estimatedpose.pose.pose.position.y = self.INIT_Y
self.estimatedpose.pose.pose.position.z = self.INIT_Z
self.estimatedpose.pose.pose.orientation = rotateQuaternion(
Quaternion(w=1.0), self.INIT_HEADING)
# NOTE: Currently not making use of covariance matrix
self.estimatedpose.header.frame_id = "/map"
self.particlecloud.header.frame_id = "/map"
# Sensor model
self.sensor_model = sensor_model.SensorModel()
def __init__(self):
#POSE RECORDE
self.pre_st_x=0
self.pre_st_y=0
self.pre_st_w=0
self._pre_pose = Pose()
self._pre_pose.position.x = 0
self._pre_pose.position.y = 0
self._pre_pose.position.z = 0
self._pre_pose.orientation.x = 0
self._pre_pose.orientation.y = 0
self._pre_pose.orientation.z = 0
self._pre_pose.orientation.w = 0
# Initialise fields
self.estimatedpose = PoseWithCovarianceStamped()
self.occupancy_map = OccupancyGrid()
self.particlecloud = PoseArray()
self.tf_message = tfMessage()
self._update_lock = Lock()
# Parameters
self.ODOM_ROTATION_NOISE = 0 # Odometry model rotation noise
self.ODOM_TRANSLATION_NOISE = 0 # Odometry x axis (forward) noise
self.ODOM_DRIFT_NOISE = 0 # Odometry y axis (side-side) noise
self.NUMBER_PREDICTED_READINGS = 20
# Set 'previous' translation to origin
# All Transforms are given relative to 0,0,0, not in absolute coords.
self.prev_odom_x = 0.0 # Previous odometry translation from origin
self.prev_odom_y = 0.0 # Previous odometry translation from origin
self.prev_odom_heading = 0.0 # Previous heading from odometry data
self.last_odom_pose = None
# Request robot's initial odometry values to be recorded in prev_odom
self.odom_initialised = False
self.sensor_model_initialised = False
# Set default initial pose to initial position and orientation.
self.estimatedpose.pose.pose.position.x = self.INIT_X
self.estimatedpose.pose.pose.position.y = self.INIT_Y
self.estimatedpose.pose.pose.position.z = self.INIT_Z
self.estimatedpose.pose.pose.orientation = rotateQuaternion(Quaternion(w=1.0),
self.INIT_HEADING)
# NOTE: Currently not making use of covariance matrix
self.estimatedpose.header.frame_id = "/map"
self.particlecloud.header.frame_id = "/map"
# Sensor model
self.sensor_model = sensor_model.SensorModel()
def to_tf(estimate):
time = rospy.Time.now()
msg = tfMessage()
for camera in estimate.cameras:
msg.transforms.append(
pose_to_transform(camera.pose, camera.camera_id, time))
for target, target_id in zip(estimate.targets,
range(len(estimate.targets))):
msg.transforms.append(
pose_to_transform(target, 'target_%d' % target_id, time))
return msg
def run(self):
tf_publisher = rospy.Publisher('tf', tf_msgs.tfMessage)
status_publisher = rospy.Publisher('status', std_msgs.String)
rospy.Subscriber('tf', tf_msgs.tfMessage, self.callback)
while not rospy.is_shutdown():
tf_publisher.publish(tf_msgs.tfMessage())
if (rospy.Time.now() - self.time) > rospy.Duration(5):
status_publisher.publish(std_msgs.String('%.2f Hz' % (self.count.next() / 5.0)))
with self.lock:
self.count = itertools.count()
self.time = rospy.Time.now()
def run(self):
tf_publisher = rospy.Publisher('tf', tf_msgs.tfMessage)
status_publisher = rospy.Publisher('status', std_msgs.String)
rospy.Subscriber('tf', tf_msgs.tfMessage, self.callback)
while not rospy.is_shutdown():
tf_publisher.publish(tf_msgs.tfMessage())
if (rospy.Time.now() - self.time) > rospy.Duration(5):
status_publisher.publish(
std_msgs.String('%.2f Hz' % (next(self.count) / 5.0)))
with self.lock:
self.count = itertools.count()
self.time = rospy.Time.now()
def CreateBag(matfile, bagname, frame_id, navsat_topic="/fix"):
'''Creates the actual bag file by successively adding images'''
bag = rosbag.Bag(bagname, 'w', compression=rosbag.Compression.BZ2, chunk_threshold=32 * 1024 * 1024)
data = loadmat(matfile)['rnv']
rnv = {}
for i, col in enumerate(data.dtype.names):
rnv[col] = data.item()[i]
try:
ref = None
x, y = [], []
for i, t in enumerate(tqdm(rnv['t'])):
# print("Adding %s" % image_name)
stamp = rospy.Time.from_sec(t)
nav_msg = NavSatFix()
nav_msg.header.stamp = stamp
nav_msg.header.frame_id = "map"
nav_msg.header.seq = i
nav_msg.latitude = rnv['lat'][i][0]
nav_msg.longitude = rnv['lon'][i][0]
nav_msg.altitude = -rnv['depth'][i][0]
nav_msg.position_covariance_type = nav_msg.COVARIANCE_TYPE_UNKNOWN
transform_msg = TransformStamped()
transform_msg.header = copy(nav_msg.header)
utm = fromLatLong(nav_msg.latitude, nav_msg.longitude, nav_msg.altitude)
if ref is None:
ref = utm.toPoint()
x.append(utm.toPoint().x - ref.x)
y.append(utm.toPoint().y - ref.y)
dx = x[-1] - sum(x[-min(20, len(x)):]) / min(20, len(x))
dy = y[-1] - sum(y[-min(20, len(y)):]) / min(20, len(y))
transform_msg.transform.translation.x = x[-1]
transform_msg.transform.translation.y = y[-1]
transform_msg.transform.translation.z = nav_msg.altitude
transform_msg.transform.rotation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(0, 0,
math.atan2(dy, dx)))
transform_msg.child_frame_id = frame_id
tf_msg = tfMessage(transforms=[transform_msg])
odometry_msg = Odometry()
odometry_msg.header = copy(transform_msg.header)
odometry_msg.child_frame_id = frame_id
odometry_msg.pose.pose.position = transform_msg.transform.translation
odometry_msg.pose.pose.orientation = transform_msg.transform.rotation
bag.write(navsat_topic, nav_msg, stamp)
bag.write("/tf", tf_msg, stamp)
bag.write("/transform", transform_msg, stamp)
bag.write("/odom", odometry_msg, stamp)
finally:
bag.close()
def recalculate_transform(self, currentTime):
"""
Creates updated transform from /odom to /map given recent odometry and
laser data.
:Args:
| currentTime (rospy.Time()): Time stamp for this update
"""
transform = Transform()
T_est = transformations.quaternion_matrix([
self.estimatedpose.pose.pose.orientation.x,
self.estimatedpose.pose.pose.orientation.y,
self.estimatedpose.pose.pose.orientation.z,
self.estimatedpose.pose.pose.orientation.w
])
T_est[0, 3] = self.estimatedpose.pose.pose.position.x
T_est[1, 3] = self.estimatedpose.pose.pose.position.y
T_est[2, 3] = self.estimatedpose.pose.pose.position.z
T_odom = transformations.quaternion_matrix([
self.last_odom_pose.pose.pose.orientation.x,
self.last_odom_pose.pose.pose.orientation.y,
self.last_odom_pose.pose.pose.orientation.z,
self.last_odom_pose.pose.pose.orientation.w
])
T_odom[0, 3] = self.last_odom_pose.pose.pose.position.x
T_odom[1, 3] = self.last_odom_pose.pose.pose.position.y
T_odom[2, 3] = self.last_odom_pose.pose.pose.position.z
T = np.dot(T_est, np.linalg.inv(T_odom))
q = transformations.quaternion_from_matrix(T) #[:3, :3])
transform.translation.x = T[0, 3]
transform.translation.y = T[1, 3]
transform.translation.z = T[2, 3]
transform.rotation.x = q[0]
transform.rotation.y = q[1]
transform.rotation.z = q[2]
transform.rotation.w = q[3]
# ----- Insert new Transform into a TransformStamped object and add to the
# ----- tf tree
new_tfstamped = TransformStamped()
new_tfstamped.child_frame_id = "/odom"
new_tfstamped.header.frame_id = "/map"
new_tfstamped.header.stamp = currentTime
new_tfstamped.transform = transform
# ----- Add the transform to the list of all transforms
self.tf_message = tfMessage(transforms=[new_tfstamped])
def tfm(matrix, parent, child, stamp):
rotation = quaternion_from_matrix(matrix)
translation = translation_from_matrix(matrix)
t = TransformStamped()
t.header.frame_id = parent
t.header.stamp = stamp
t.child_frame_id = child
t.transform.translation.x = translation[0]
t.transform.translation.y = translation[1]
t.transform.translation.z = translation[2]
t.transform.rotation.x = rotation[0]
t.transform.rotation.y = rotation[1]
t.transform.rotation.z = rotation[2]
t.transform.rotation.w = rotation[3]
return tfMessage([t])
def tfm(matrix, parent, child, stamp):
rotation = quaternion_from_matrix(matrix)
translation = translation_from_matrix(matrix)
t = TransformStamped()
t.header.frame_id = parent
t.header.stamp = stamp
t.child_frame_id = child
t.transform.translation.x = translation[0]
t.transform.translation.y = translation[1]
t.transform.translation.z = translation[2]
t.transform.rotation.x = rotation[0]
t.transform.rotation.y = rotation[1]
t.transform.rotation.z = rotation[2]
t.transform.rotation.w = rotation[3]
return tfMessage([t])
def __init__(self):
# Perhaps publish the object on a separate topic?
# define object handle
self.item_list = [
"object_1", "object_2", "object_3", "object_4", "object_5",
"object_6", "object_7", "object_8", "object_9", "object_10",
"object_11", "object_12", "object_13", "object_14", "object_15",
"object_16", "object_17", "object_18"
]
# create a dict to lookup object class
self.item_dict = {
"apple": [
"object_1",
"object_2",
"object_3",
"object_4",
"object_5",
"object_6",
"object_7",
"object_8",
"object_9",
"object_10",
"object_11",
"object_12",
"object_13",
"object_14",
"object_15",
],
"mouse": [
"object_16", "object_17", "object_18", "object_19",
"object_20", "object_21", "object_22", "object_23",
"object_24", "object_25", "object_26", "object_27",
"object_28", "object_29", "object_30"
]
}
self.in_map_log = [] # ["mouse", "apple"]
self.tfMessage = tfMessage()
self.tf_listener = tf.TransformListener()
rospy.loginfo('listener made')
tf_subscriber = rospy.Subscriber("/tf",
tfMessage,
self._tf_callback,
queue_size=1)
self.tf_publisher = rospy.Publisher("/tf", tfMessage, queue_size=1)
self.or_pub = rospy.Publisher('/or', PoseStamped, queue_size=3)
self.or_map_pub = rospy.Publisher('/or_map', PoseStamped, queue_size=3)
def test_face_tf(self):
pub, msg_class = rostopic.create_publisher('/camera/face_locations',
'pi_face_tracker/Faces',
True)
msg = msg_class()
msg_str = """faces:
- id: 1
point:
x: 0.76837966452
y: -0.132697071241
z: 0.0561996095957
attention: 0.990000009537
- id: 2
point:
x: 0.807249662369
y: -0.00428759906469
z: 0.035407830253
attention: 0.990000009537"""
fill_message_args(msg, [yaml.load(msg_str)])
pub.publish(msg)
msg = wait_for_message('/tf', tfMessage, 5)
expect_str = """
transforms:
-
header:
seq: 0
stamp:
nsecs: 752310037
frame_id: camera
child_frame_id: face_base1
transform:
translation:
x: 0.76837966452
y: -0.132697071241
z: 0.0561996095957
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
"""
expect = tfMessage()
fill_message_args(expect, [yaml.load(expect_str)])
#self.assertEqual(msg.transforms[0].child_frame_id,
# expect.transforms[0].child_frame_id)
self.assertEqual(msg.transforms[0].transform,
expect.transforms[0].transform)
pub.unregister()
def recalculate_transform(self, currentTime):
"""
Creates updated transform from /odom to /map given recent odometry and
laser data.
:Args:
| currentTime (rospy.Time()): Time stamp for this update
"""
transform = Transform()
T_est = transformations.quaternion_matrix([self.estimatedpose.pose.pose.orientation.x,
self.estimatedpose.pose.pose.orientation.y,
self.estimatedpose.pose.pose.orientation.z,
self.estimatedpose.pose.pose.orientation.w])
T_est[0, 3] = self.estimatedpose.pose.pose.position.x
T_est[1, 3] = self.estimatedpose.pose.pose.position.y
T_est[2, 3] = self.estimatedpose.pose.pose.position.z
T_odom = transformations.quaternion_matrix([self.last_odom_pose.pose.pose.orientation.x,
self.last_odom_pose.pose.pose.orientation.y,
self.last_odom_pose.pose.pose.orientation.z,
self.last_odom_pose.pose.pose.orientation.w])
T_odom[0, 3] = self.last_odom_pose.pose.pose.position.x
T_odom[1, 3] = self.last_odom_pose.pose.pose.position.y
T_odom[2, 3] = self.last_odom_pose.pose.pose.position.z
T = np.dot(T_est, np.linalg.inv(T_odom))
q = transformations.quaternion_from_matrix(T) #[:3, :3])
transform.translation.x = T[0, 3]
transform.translation.y = T[1, 3]
transform.translation.z = T[2, 3]
transform.rotation.x = q[0]
transform.rotation.y = q[1]
transform.rotation.z = q[2]
transform.rotation.w = q[3]
# Insert new Transform into a TransformStamped object and add to the
# tf tree
new_tfstamped = TransformStamped()
new_tfstamped.child_frame_id = "/odom"
new_tfstamped.header.frame_id = "/map"
new_tfstamped.header.stamp = currentTime
new_tfstamped.transform = transform
# Add the transform to the list of all transforms
self.tf_message = tfMessage(transforms=[new_tfstamped])
def test_face_tf(self):
pub, msg_class = rostopic.create_publisher(
'/camera/face_locations', 'pi_face_tracker/Faces', True)
msg = msg_class()
msg_str = """faces:
- id: 1
point:
x: 0.76837966452
y: -0.132697071241
z: 0.0561996095957
attention: 0.990000009537
- id: 2
point:
x: 0.807249662369
y: -0.00428759906469
z: 0.035407830253
attention: 0.990000009537"""
fill_message_args(msg, [yaml.load(msg_str)])
pub.publish(msg)
msg = wait_for_message('/tf', tfMessage, 5)
expect_str = """
transforms:
-
header:
seq: 0
stamp:
nsecs: 752310037
frame_id: camera
child_frame_id: face_base1
transform:
translation:
x: 0.76837966452
y: -0.132697071241
z: 0.0561996095957
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
"""
expect = tfMessage()
fill_message_args(expect, [yaml.load(expect_str)])
#self.assertEqual(msg.transforms[0].child_frame_id,
# expect.transforms[0].child_frame_id)
self.assertEqual(msg.transforms[0].transform,
expect.transforms[0].transform)
pub.unregister()
def publish_filtered_tf(self, header):
yaw = np.median(self.buffer)
q = tfs.quaternion_from_euler(0, 0, yaw - math.pi)
ts = TransformStamped()
ts.header = header
ts.header.frame_id = self.source
ts.child_frame_id = self.goal
ts.transform.translation.x = self.position[0]
ts.transform.translation.y = self.position[1]
ts.transform.translation.z = 0
ts.transform.rotation.x = q[0]
ts.transform.rotation.y = q[1]
ts.transform.rotation.z = q[2]
ts.transform.rotation.w = q[3]
msg = tfMessage()
msg.transforms.append(ts)
self.tf_pub.publish(msg)
def object_to_map(self, pose, object_type, time):
obj_in_map = self.tf_listener.transformPose("/map", pose)
obj_in_map.header.frame_id = object_type + "_in_map"
transform = Transform()
transform.translation = obj_in_map.pose.position
transform.rotation = obj_in_map.pose.orientation
# Insert new Transform into a TransformStamped object and add to the tf tree
tf_to_map = TransformStamped()
tf_to_map.child_frame_id = object_type + "_in_map"
tf_to_map.header.frame_id = "/map"
tf_to_map.transform = transform
tf_to_map.header.stamp = time
#rospy.loginfo(new_tfstamped)
# add to tf list
self.or_map_pub.publish(obj_in_map)
self.tf_message = tfMessage(transforms=[tf_to_map])
self.tf_publisher.publish(self.tf_message)
def publish_filtered_tf(self, header):
m = np.median(self.buffer, axis=0)
if not self.filter_position:
m[0:3] = self.last_transform[0:3]
q = quaternion_from_euler(m[3], m[4], m[5])
ts = TransformStamped()
ts.header = header
ts.child_frame_id = self.frame_id + '_filtered'
ts.transform.translation.x = m[0]
ts.transform.translation.y = m[1]
ts.transform.translation.z = m[2]
ts.transform.rotation.x = q[0]
ts.transform.rotation.y = q[1]
ts.transform.rotation.z = q[2]
ts.transform.rotation.w = q[3]
msg = tfMessage()
msg.transforms.append(ts)
self.tf_pub.publish(msg)
def pose2tf(pose, timestamp, frame_id, child_frame_id):
tf_msg = tfMessage()
geo_msg = TransformStamped()
geo_msg.header.stamp = timestamp
geo_msg.header.frame_id = frame_id
geo_msg.child_frame_id = child_frame_id
geo_msg.transform.translation.x = pose[0]
geo_msg.transform.translation.y = pose[1]
geo_msg.transform.translation.z = pose[2]
angles = tf.transformations.quaternion_from_euler(pose[3], pose[4],
pose[5])
geo_msg.transform.rotation.x = angles[0]
geo_msg.transform.rotation.y = angles[1]
geo_msg.transform.rotation.z = angles[2]
geo_msg.transform.rotation.w = angles[3]
tf_msg.transforms.append(geo_msg)
return tf_msg
def publishTransform(transform, timestamp, frame_id, output_bag):
scale, shear, angles, transl, persp = tf.transformations.decompose_matrix(
transform)
rotation = tf.transformations.quaternion_from_euler(*angles)
trans = TransformStamped()
trans.header.stamp = timestamp
trans.header.frame_id = 'world'
trans.child_frame_id = frame_id
trans.transform.translation.x = transl[0]
trans.transform.translation.y = transl[1]
trans.transform.translation.z = transl[2]
trans.transform.rotation.x = rotation[0]
trans.transform.rotation.y = rotation[1]
trans.transform.rotation.z = rotation[2]
trans.transform.rotation.w = rotation[3]
msg = tfMessage()
msg.transforms.append(trans)
output_bag.write('/tf', msg, timestamp)
def write_transform(view_pose, timestamp, frame_id, output_bag, publishers,
publish):
scale, shear, angles, transl, persp = tf.transformations.decompose_matrix(
camera_to_world_with_pose(view_pose))
rotation = tf.transformations.quaternion_from_euler(*angles)
trans = TransformStamped()
trans.header.stamp = timestamp
trans.header.frame_id = 'world'
trans.child_frame_id = frame_id
trans.transform.translation.x = transl[0]
trans.transform.translation.y = transl[1]
trans.transform.translation.z = transl[2]
trans.transform.rotation.x = rotation[0]
trans.transform.rotation.y = rotation[1]
trans.transform.rotation.z = rotation[2]
trans.transform.rotation.w = rotation[3]
msg = tfMessage()
msg.transforms.append(trans)
write_msg("/tf", msg, output_bag, publishers, publish, timestamp)
def writeTransform(view, timestamp, frame_id, output_bag):
ground_truth_pose = interpolate_poses(view.shutter_open,
view.shutter_close, 0.5)
scale, shear, angles, transl, persp = tf.transformations.decompose_matrix(
camera_to_world_with_pose(ground_truth_pose))
rotation = tf.transformations.quaternion_from_euler(*angles)
trans = TransformStamped()
trans.header.stamp = timestamp
trans.header.frame_id = 'world'
trans.child_frame_id = frame_id
trans.transform.translation.x = transl[0]
trans.transform.translation.y = transl[1]
trans.transform.translation.z = transl[2]
trans.transform.rotation.x = rotation[0]
trans.transform.rotation.y = rotation[1]
trans.transform.rotation.z = rotation[2]
trans.transform.rotation.w = rotation[3]
msg = tfMessage()
msg.transforms.append(trans)
output_bag.write('/tf', msg, timestamp)
def tf_callbak(self, data=tfMessage()):
obstacle_msg = PolygonStamped()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map
for i in data.transforms:
if i.child_frame_id == "camera_link":
tempPose = PoseStamped()
tempPose.pose.position.x = -1*i.transform.translation.x #axis reverse
tempPose.pose.position.y = i.transform.translation.y
tempPose.pose.position.z = i.transform.translation.z
x = i.transform.rotation.x
y = i.transform.rotation.y
z = i.transform.rotation.z
w = i.transform.rotation.w
quaternion = (x,y,z,w)
self.initialAngle = tf.transformations.euler_from_quaternion(quaternion)[2] #euler to radians
self.camera_link = tempPose
try:
self.myCord = self.vectortoCord(self.camera_link, self.org.position.x, self.org.position.y, 0.05)
except AttributeError:
print "catch"
obstacle_msg.polygon.points = []
#painting the polygon
length = 50 #cm vehicle dim
width = 35 #cm vehicle dim
#the origin point is at the head of the car (y axis), and in the midlle (x axis)
for i in range (-length, 0):
for j in range (-width/2, width/2):
tempPointOrien = Point32()
degree = self.initialAngle
#orientation with multiplication by transform matrix
tempPointOrien.x = (i * 0.01 * cos(degree) - j * 0.01 * sin(degree)) - tempPose.pose.position.x
tempPointOrien.y = (i * 0.01 * sin(degree) + j * 0.01 * cos(degree)) + tempPose.pose.position.y
tempPointOrien.z = 0
obstacle_msg.polygon.points.append(tempPointOrien)
pub2 = rospy.Publisher('/geometry_msgs/Polygon', PolygonStamped, queue_size=10)
pub2.publish(obstacle_msg) #publish msg
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 __call__(self, channel, data):
lcm_msg = agile.state_t.decode(data)
# List of messages and topics to publish
topics_to_publish = []
msgs_to_publish = []
# Populate pose message
topics_to_publish.append(self.rostopic)
ros_msg = PoseStamped()
ros_msg.header = self.make_header(lcm_msg.utime)
ros_msg.pose.position.x = lcm_msg.position[0]
ros_msg.pose.position.y = lcm_msg.position[1]
ros_msg.pose.position.z = lcm_msg.position[2]
ros_msg.pose.orientation.w = lcm_msg.orient[0]
ros_msg.pose.orientation.x = lcm_msg.orient[1]
ros_msg.pose.orientation.y = lcm_msg.orient[2]
ros_msg.pose.orientation.z = lcm_msg.orient[3]
msgs_to_publish.append(ros_msg)
# Populate TF tree message
tf_packet = tfMessage()
# Mocap NED <> Mocap ENU
# topics_to_publish.append("/tf_static")
# tf_ned_2_enu_msg = TransformStamped()
# tf_ned_2_enu_msg.header = self.make_header(lcm_msg.utime)
# tf_ned_2_enu_msg.header.frame_id = "mocap_ENU"
# tf_ned_2_enu_msg.child_frame_id = "mocap_NED"
# tf_ned_2_enu_msg.transform.translation = ros_msg.pose.position
# tf_ned_2_enu_msg.transform.rotation = ros_msg.pose.orientation
# msgs_to_publish.append(tf_ned_2_enu_msg)
# [ 0, 0.7071068, 0.7071068, 0 ]
# Mocap NED <> drone_body message
tf_drone_msg = TransformStamped()
tf_drone_msg.header = self.make_header(lcm_msg.utime)
tf_drone_msg.header.frame_id = "mocap_NED"
tf_drone_msg.child_frame_id = "body_frame"
tf_drone_msg.transform.translation = ros_msg.pose.position
tf_drone_msg.transform.rotation = ros_msg.pose.orientation
# Append message to transform list
tf_packet.transforms.append(tf_drone_msg)
# Static TFs
# drone_body <> IMU message
# topics_to_publish.append("/tf_static")
# tf_imu_msg = TransformStamped()
# tf_imu_msg.header = self.make_header(lcm_msg.utime)
# tf_imu_msg.header.frame_id = "body_frame"
# tf_imu_msg.child_frame_id = "imu"
# tf_imu_msg.transform.translation.x = 0.0
# tf_imu_msg.transform.translation.y = 0.0
# tf_imu_msg.transform.translation.z = 0.0
# # [ 0.707479362748676 0.002029830683065 -0.007745228390866 0.706688646087723 ]
# tf_imu_msg.transform.rotation.w = 0.707479362748676
# tf_imu_msg.transform.rotation.x = 0.002029830683065
# tf_imu_msg.transform.rotation.y = -0.007745228390866
# tf_imu_msg.transform.rotation.z = 0.706688646087723
# msgs_to_publish.append(tf_imu_msg)
# # drone_body <> camera_l
# topics_to_publish.append("/tf_static")
# tf_cam_l_msg = TransformStamped()
# tf_cam_l_msg.header = self.make_header(lcm_msg.utime)
# tf_cam_l_msg.header.frame_id = "body_frame"
# tf_cam_l_msg.child_frame_id = "camera_l"
# tf_cam_l_msg.transform.translation.x = 0.0
# tf_cam_l_msg.transform.translation.y = -0.05
# tf_cam_l_msg.transform.translation.z = 0.0
# tf_cam_l_msg.transform.rotation.w = 1.0
# tf_cam_l_msg.transform.rotation.x = 0.0
# tf_cam_l_msg.transform.rotation.y = 0.0
# tf_cam_l_msg.transform.rotation.z = 0.0
# msgs_to_publish.append(tf_cam_l_msg)
# # drone_body <> camera_r
# topics_to_publish.append("/tf_static")
# tf_cam_r_msg = TransformStamped()
# tf_cam_r_msg.header = self.make_header(lcm_msg.utime)
# tf_cam_r_msg.header.frame_id = "body_frame"
# tf_cam_r_msg.child_frame_id = "camera_r"
# tf_cam_r_msg.transform.translation.x = 0.0
# tf_cam_r_msg.transform.translation.y = 0.05
# tf_cam_r_msg.transform.translation.z = 0.0
# tf_cam_r_msg.transform.rotation.w = 1.0
# tf_cam_r_msg.transform.rotation.x = 0.0
# tf_cam_r_msg.transform.rotation.y = 0.0
# tf_cam_r_msg.transform.rotation.z = 0.0
# msgs_to_publish.append(tf_cam_r_msg)
# # drone_body <> camera_d
# topics_to_publish.append("/tf_static")
# tf_cam_d_msg = TransformStamped()
# tf_cam_d_msg.header = self.make_header(lcm_msg.utime)
# tf_cam_d_msg.header.frame_id = "body_frame"
# tf_cam_d_msg.child_frame_id = "camera_d"
# tf_cam_d_msg.transform.translation.x = 0.0
# tf_cam_d_msg.transform.translation.y = 0.0
# tf_cam_d_msg.transform.translation.z = 0.0
# # 0.707,0,-0.707,0
# tf_cam_d_msg.transform.rotation.w = 0.707
# tf_cam_d_msg.transform.rotation.x = 0.0
# tf_cam_d_msg.transform.rotation.y = -0.707
# tf_cam_d_msg.transform.rotation.z = 0.0
# msgs_to_publish.append(tf_cam_d_msg)
# Queue TF tree message for publication
topics_to_publish.append("/tf")
msgs_to_publish.append(tf_packet)
# Populate camera info message (if needed)
if (lcm_msg.utime in self.image_timestamps):
cam_info = CameraInfo()
cam_info.header = self.make_header(lcm_msg.utime)
cam_info.height = 768
cam_info.width = 1024
cam_info.distortion_model = 'plumb_bob'
cam_info.K = [
665.107510106, 0., 511.5, 0., 665.107510106, 383.5, 0., 0., 1.
]
cam_info.P = [
665.107510106, 0., 511.5, 0., 0., 665.107510106, 383.5, 0., 0.,
0., 1., 0
]
for cam_topic in [
"/camera_l/camera_info", "/camera_r/camera_info",
"/camera_d/camera_info"
]:
topics_to_publish.append(cam_topic)
msgs_to_publish.append(cam_info)
return topics_to_publish, msgs_to_publish
def go():
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((host, port))
first = True
buf = ''
count = 0
while True:
d = s.recv(2**12)
if not d:
break
buf += d
lines = buf.split('\n')
buf = lines[-1]
for line in lines[:-1]:
if first:
first = False
continue
if count % decimation == 0:
d = json.loads(line)
ecef_cov = numpy.array(d['X_position_relative_position_orientation_ecef_covariance'])
absodom_pub.publish(Odometry(
header=Header(
stamp=rospy.Time.from_sec(d['timestamp']*1e-9),
frame_id='/ecef',
),
child_frame_id=child_frame_id,
pose=PoseWithCovariance(
pose=Pose(
position=Point(*d['position_ecef']),
orientation=Quaternion(**d['orientation_ecef']),
),
covariance=numpy.vstack((
numpy.hstack((ecef_cov[0:3, 0:3], ecef_cov[0:3, 6:9])),
numpy.hstack((ecef_cov[6:9, 0:3], ecef_cov[6:9, 6:9])),
)).flatten(),
),
twist=TwistWithCovariance(
twist=Twist(
linear=Vector3(*d['velocity_body']),
angular=Vector3(*d['angular_velocity_body']),
),
covariance=numpy.vstack((
numpy.hstack((d['X_velocity_body_covariance'], numpy.zeros((3, 3)))),
numpy.hstack((numpy.zeros((3, 3)), d['X_angular_velocity_body_covariance'])),
)).flatten(),
),
))
odom_pub.publish(Odometry(
header=Header(
stamp=rospy.Time.from_sec(d['timestamp']*1e-9),
frame_id='/enu',
),
child_frame_id=child_frame_id,
pose=PoseWithCovariance(
pose=Pose(
position=Point(*d['relative_position_enu']),
orientation=Quaternion(**d['orientation_enu']),
),
covariance=numpy.array(d['X_relative_position_orientation_enu_covariance']).flatten(),
),
twist=TwistWithCovariance(
twist=Twist(
linear=Vector3(*d['velocity_body']),
angular=Vector3(*d['angular_velocity_body']),
),
covariance=numpy.vstack((
numpy.hstack((d['X_velocity_body_covariance'], numpy.zeros((3, 3)))),
numpy.hstack((numpy.zeros((3, 3)), d['X_angular_velocity_body_covariance'])),
)).flatten(),
),
))
clock_error_pub.publish(Float64(d['X_clock_error']))
tf_pub.publish(tfMessage(
transforms=[
TransformStamped(
header=Header(
stamp=rospy.Time.from_sec(d['timestamp']*1e-9),
frame_id='/enu',
),
child_frame_id=child_frame_id,
transform=Transform(
translation=Point(*d['relative_position_enu']),
rotation=Quaternion(**d['orientation_enu']),
),
),
],
))
count += 1
fr_name = frame_name +str(i)
msg.child_frame_id = fr_name
msg.header.frame_id = fr_name[:-(1 + idepth*bush)]
tfs.append(msg)
#recurse
if depth > 1:
tfs.extend(get_tree(F, depth - 1, step / 2.0,
fr_name, bush, idepth + 1))
return tfs
depth = rospy.get_param('depth', 3)
step = rospy.get_param('step', 0.4)
get_tree.branch_factor = rospy.get_param('branch_factor', 5)
get_tree.twig_height = rospy.get_param('twig_height', 0.1)
bush_topo = rospy.get_param('bush_topoloy', False)
r = rospy.get_param('rate', 10)
rate = rospy.Rate(r)
while not rospy.is_shutdown():
msg = tfMessage()
msg.transforms = get_tree(kdl.Frame(), depth, step, '/fr', bush_topo)
tf_pub.publish(msg)
rate.sleep()
def main():
parser = argparse.ArgumentParser(description="Create the tf messages from "
"the Tango logs.")
parser.add_argument('-d', '--directory',
help="The folder must have images.txt, "
"depth_to_imu_transformation.txt, "
"fisheye_to_imu_transformation.txt, "
"narrow_to_imu_transformation.txt",
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()
folder_name = arguments.directory
standard_file = os.path.join(folder_name, 'images.txt')
alt_file = os.path.join(folder_name, '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(folder_name))
reply = raw_input()
if reply == 'y':
images_file = open(alt_file)
else:
images_file = open(standard_file)
else:
images_file = open(standard_file)
print("Processing data from {}...".format(images_file.name))
#depth to imu
depth_np = np.loadtxt(os.path.join(folder_name,
"depth_to_imu_transformation.txt",
),
delimiter=',').reshape(3, 4)
rotation_matrix = np.vstack((depth_np, [0, 0, 0, 1]))
quaternion = transformations.quaternion_from_matrix(rotation_matrix)
depth_transform_msg = Transform()
depth_transform_msg.translation.x = rotation_matrix[0, 3]
depth_transform_msg.translation.y = rotation_matrix[1, 3]
depth_transform_msg.translation.z = rotation_matrix[2, 3]
depth_transform_msg.rotation.x = quaternion[0]
depth_transform_msg.rotation.y = quaternion[1]
depth_transform_msg.rotation.z = quaternion[2]
depth_transform_msg.rotation.w = quaternion[3]
#fisheye to imu
depth_np = np.loadtxt(os.path.join(folder_name,
"fisheye_to_imu_transformation.txt"),
delimiter=',').reshape(3, 4)
rotation_matrix = np.vstack((depth_np, [0, 0, 0, 1]))
quaternion = transformations.quaternion_from_matrix(rotation_matrix)
fisheye_transform_msg = Transform()
fisheye_transform_msg.translation.x = rotation_matrix[0, 3]
fisheye_transform_msg.translation.y = rotation_matrix[1, 3]
fisheye_transform_msg.translation.z = rotation_matrix[2, 3]
fisheye_transform_msg.rotation.x = quaternion[0]
fisheye_transform_msg.rotation.y = quaternion[1]
fisheye_transform_msg.rotation.z = quaternion[2]
fisheye_transform_msg.rotation.w = quaternion[3]
#narrow to imu
depth_np = np.loadtxt(os.path.join(folder_name,
"narrow_to_imu_transformation.txt"),
delimiter=',').reshape(3, 4)
rotation_matrix = np.vstack((depth_np, [0, 0, 0, 1]))
quaternion = transformations.quaternion_from_matrix(rotation_matrix)
narrow_transform_msg = Transform()
narrow_transform_msg.translation.x = rotation_matrix[0, 3]
narrow_transform_msg.translation.y = rotation_matrix[1, 3]
narrow_transform_msg.translation.z = rotation_matrix[2, 3]
narrow_transform_msg.rotation.x = quaternion[0]
narrow_transform_msg.rotation.y = quaternion[1]
narrow_transform_msg.rotation.z = quaternion[2]
narrow_transform_msg.rotation.w = quaternion[3]
#for each entry in images.txt, add a transformation with the values above
#TODO skip some values? we don't need high frame rate!
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
msg = tfMessage()
#using the android_ts as the timestamp
ts = float(tokens[1])
#depth
tf_stamped = TransformStamped()
tf_stamped.header.frame_id = "/tango/imu"
tf_stamped.header.seq = lineno
tf_stamped.header.stamp = rospy.Time.from_seconds(ts)
tf_stamped.child_frame_id = "/tango/depth"
tf_stamped.transform = depth_transform_msg
msg.transforms.append(tf_stamped)
#fisheye
tf_stamped = TransformStamped()
tf_stamped.header.frame_id = "/tango/imu"
tf_stamped.header.seq = lineno
tf_stamped.header.stamp = rospy.Time.from_seconds(ts)
tf_stamped.child_frame_id = "/tango/fisheye"
tf_stamped.transform = fisheye_transform_msg
msg.transforms.append(tf_stamped)
#narrow
tf_stamped = TransformStamped()
tf_stamped.header.frame_id = "/tango/imu"
tf_stamped.header.seq = lineno
tf_stamped.header.stamp = rospy.Time.from_seconds(ts)
tf_stamped.child_frame_id = "/tango/narrow"
tf_stamped.transform = narrow_transform_msg
msg.transforms.append(tf_stamped)
outputbag.write("tf", msg, rospy.Time.from_seconds(ts))
outputbag.close()
print("Bag creation complete, bagfile: {}".format(outputbag.filename))
q.y = sinR * cosP * cosY + cosR * sinP * sinY
q.z = cosR * sinP * cosY + sinR * cosP * sinY
q.w = cosR * cosP * sinY + sinR * sinP * cosY
return q
class Tracker:
def __init__(self, topic, Msg):
self.sub = rospy.Subscriber(topic, Msg, self.callback)
self.msg = Msg()
def callback(self, msg):
self.msg = msg
t = TransformStamped()
t.header.frame_id = 'base_link'
t.header.seq = 0
t.child_frame_id = 'high_def_frame'
t.transform.translation = xyz(0, 0, 1.25)
t.transform.rotation = rpy(0, 1.3, 0)
rospy.init_node('fake_camera')
pub_tf = rospy.Publisher('/tf_message', tfMessage)
while not rospy.is_shutdown():
t.header.stamp = rospy.get_rostime()
msg = tfMessage([t])
pub_tf.publish(msg)
time.sleep(0.1)
from waymo_viewer.msg import LidarArray
from waymo_viewer.msg import VehicleInfo
from sensor_msgs.msg import Image
from sensor_msgs.msg import PointCloud
from visualization_msgs.msg import MarkerArray
from visualization_msgs.msg import Marker
from geometry_msgs.msg import TransformStamped
from tf.msg import tfMessage
import math
from tf.transformations import quaternion_from_euler
import tf
camera_images = CameraImageArray()
lidars = LidarArray()
br = CvBridge()
transforms = tfMessage()
get_camera_image = False
get_lidars = False
get_vehicle = False
vehicle = (0, 0, 255)
pedestrian = (0, 255, 0)
cyclist = (255, 0, 0)
sign = (0, 255, 255)
#camera_front_labelimage_pub = rospy.Publisher("camera_front_labelimage", Image, queue_size = 100)
#camera_frontleft_labelimage_pub = rospy.Publisher("camera_frontleft_labelimage", Image, queue_size = 100)
#camera_frontright_labelimage_pub = rospy.Publisher("camera_frontright_labelimage", Image, queue_size = 100)
#camera_sideleft_labelimage_pub = rospy.Publisher("camera_sideleft_labelimage", Image, queue_size = 100)
#camera_sideright_labelimage_pub = rospy.Publisher("camera_sideright_labelimage", Image, queue_size = 100)
def convert(inputfile, outputfile):
tfmsg = tfMessage()
seq = 0
csail = False
#open input
try:
inCompleteName = os.path.expanduser(inputfile)
carmenlog = open(inCompleteName, "r")
except (IOError, ValueError):
rospy.logerr("Couldn't open %", inputfile)
exit(-1)
#create output
try:
outCompleteName = os.path.expanduser(outputfile)
bag = rosbag.Bag(outCompleteName, "w")
except (IOError, ValueError):
rospy.logerr("Couldn't open %", outputfile)
exit(-1)
#read all input lines
for line in carmenlog:
words = line.split(" ")
if words[0] == "ODOM":
values = fillUpOdomMessage(tfmsg, words, seq)
tfmsg = values[0]
pose = values[1]
bag.write("/base_odometry/odom", pose, pose.header.stamp)
bag.write("/tf", tfmsg, pose.header.stamp)
seq += 1
if words[0] == "FLASER" and not csail:
laser_msg = fillUpOldLaserMessage(words)
laser_msg.header.seq = laser_msg.header.seq + 1
bag.write("/base_scan", laser_msg, laser_msg.header.stamp)
if words[0] == "ROBOTLASER1":
values = fillUpLaserMessage(tfmsg, words, seq)
laser_msg = values[2]
laser_msg.header.seq = laser_msg.header.seq + 1
pose = values[1]
tfmsg = values[0]
#bag.write("/base_odometry/odom", pose , pose.header.stamp)
#bag.write("/tf", tfmsg , pose.header.stamp)
bag.write("/base_scan", laser_msg, laser_msg.header.stamp)
seq += 1
if words[0] == "FLASER" and csail:
laser_msg = fillUpOldLaserMessage(words)
laser_msg.header.seq = laser_msg.header.seq + 1
bag.write("/base_scan", laser_msg, laser_msg.header.stamp)
values = fillUpOdomFromFlaser(tfmsg, words, seq)
tfmsg = values[0]
pose = values[1]
bag.write("/base_odometry/odom", pose, pose.header.stamp)
bag.write("/tf", tfmsg, pose.header.stamp)
seq += 1
tfmsg = tfMessage()
bag.close()
def _tf_callback(self, msg):
""" called whenever a new tfMessage is received. Creates a list of detected objects
and iterates through these to return their type and location w.r.t. map and odometry frames."""
detected_objects = []
for item in self.item_list:
if self.tf_listener.frameExists(item):
detected_objects.append(item)
rospy.loginfo(detected_objects)
if detected_objects:
for item in detected_objects:
object_h = item # type: str
obj_type = self.get_type(object_h)
t = self.tf_listener.getLatestCommonTime(
"/camera_link", object_h)
#rospy.loginfo("Object detected")
#(pos, rot) = self.tf_listener.lookupTransform("/camera_rgb_optical_frame", , t)
(pos, rot) = self.tf_listener.lookupTransform(
"/camera_link", object_h, t)
# create the object pose
obj_pos = geometry_msgs.msg.PoseStamped()
obj_pos.header.frame_id = "/camera_link"
obj_pos.pose.position.x = pos[0]
obj_pos.pose.position.y = pos[1]
obj_pos.pose.position.z = pos[2]
obj_pos.pose.orientation.x = rot[0]
obj_pos.pose.orientation.y = rot[1]
obj_pos.pose.orientation.z = rot[2]
obj_pos.pose.orientation.w = rot[3]
obj_pose = self.tf_listener.transformPose(
"/base_link", obj_pos)
obj_pose.header.frame_id = obj_type
transform = Transform()
transform.translation = obj_pose.pose.position
transform.rotation = obj_pose.pose.orientation
# Insert new Transform into a TransformStamped object and add to the tf tree
new_tfstamped = TransformStamped()
new_tfstamped.child_frame_id = obj_type
new_tfstamped.header.frame_id = "/base_link"
new_tfstamped.transform = transform
new_tfstamped.header.stamp = t
# add to tf list
self.tf_message = tfMessage(transforms=[new_tfstamped])
self.tf_publisher.publish(self.tf_message)
self.or_pub.publish(obj_pose)
if obj_type in self.in_map_log:
t = self.tf_listener.getLatestCommonTime("/map", object_h)
#(pos, rot) = self.tf_listener.lookupTransform("/camera_rgb_optical_frame", object_h, t)
(pos, rot) = self.tf_listener.lookupTransform(
"/camera_link", object_h, t)
# get the object pose
obj_pos = geometry_msgs.msg.PoseStamped()
obj_pos.header.frame_id = "/camera_link"
obj_pos.pose.position.x = pos[0]
obj_pos.pose.position.y = pos[1]
obj_pos.pose.position.z = pos[2]
obj_pos.pose.orientation.x = rot[0]
obj_pos.pose.orientation.y = rot[1]
obj_pos.pose.orientation.z = rot[2]
obj_pos.pose.orientation.w = rot[3]
self.object_to_map(obj_pos, obj_type, t)
self.in_map_log.remove(obj_type)
def __init__(self, _num, _mapTopic, _algorithmName):
# Initialise fields
self.estimatedpose = PoseWithCovarianceStamped()
self.occupancy_map = OccupancyGrid()
self.particlecloud = PoseArray()
self.tf_message = tfMessage()
self.weights = []
self.maxWeight = 0
self.totalWeight = 0
self.num = _num
self.clusterTask = ClusterTask()
self.floorName = _mapTopic
self.exploded = False
# Initialise objects
self.cloud = UpdateParticleCloud()
self.estimate = EstimatePose()
self.init = InitialiseCloud()
self._weighted_particle_publisher = rospy.Publisher("/weightedParticles", WeightedParticles)
# Parameters
self.ODOM_ROTATION_NOISE = 0 # Odometry model rotation noise
self.ODOM_TRANSLATION_NOISE = 0 # Odometry x axis (forward) noise
self.ODOM_DRIFT_NOISE = 0 # Odometry y axis (side-side) noise
self.NUMBER_PREDICTED_READINGS = 20
# Set 'previous' translation to origin
# All Transforms are given relative to 0,0,0, not in absolute coords.
self.prev_odom_x = 0.0 # Previous odometry translation from origin
self.prev_odom_y = 0.0 # Previous odometry translation from origin
self.prev_odom_heading = 0.0 # Previous heading from odometry data
self.last_odom_pose = None
# Request robot's initial odometry values to be recorded in prev_odom
self.odom_initialised = False
self.sensor_model_initialised = False
# Set default initial pose to initial position and orientation.
self.estimatedpose.pose.pose.position.x = self.INIT_X
self.estimatedpose.pose.pose.position.y = self.INIT_Y
self.estimatedpose.pose.pose.position.z = self.INIT_Z
self.estimatedpose.pose.pose.orientation = rotateQuaternion(Quaternion(w=1.0),self.INIT_HEADING)
# NOTE: Currently not making use of covariance matrix
self.estimatedpose.header.frame_id = "/map"
self.particlecloud.header.frame_id = "/map"
# Sensor model
self.sensor_model = sensor_model.SensorModel()
print(_algorithmName)
# What algorithm do we use?
if _algorithmName == "async":
self.asynchronous = True
elif _algorithmName == "sync":
self.asynchronous = False
else:
print("Invalid argument 3: expected \"sync\" or \"async\"")
sys.exit(1)
# Free Point where Robot can be
self.listFreePoints = []
#!/usr/bin/env python
import roslib
import rospy
from nav_msgs.msg import Odometry
from nav_msgs.msg import OccupancyGrid
from tf.msg import tfMessage
from geometry_msgs.msg import Twist
itf = tfMessage()
igrid = OccupancyGrid()
def callback2(msg):
#print(msg.?)
itf = msg
def callback4(msg):
#print(msg.?)
igrid = msg
def main():
rospy.init_node('NavigationNode', anonymous=True)
pub2 = rospy.Publisher('/simulation_robot/tf', tfMessage, queue_size=10)
rospy.Subscriber('/tf', tfMessage, callback2)
pub4 = rospy.Publisher('/simulation_robot/OccupancyGrid',
OccupancyGrid,
queue_size=10)
x_, y_, z_ = transform.TransformPoint(fData['longitude'][i],
fData['latitude'][i],
fData['height'][i])
geoMsg.transform.translation.x = x_
geoMsg.transform.translation.y = y_
geoMsg.transform.translation.z = z_
roll_imu_ = float(fData['roll'][i]) * math.pi / 180.0
pitch_imu_ = float(fData['pitch'][i]) * math.pi / 180.0
azimuth_imu_ = float(fData['azimuth'][i]) * math.pi / 180.0
quat1 = tf.transformations.quaternion_about_axis(roll_imu_, (1, 0, 0))
quat2 = tf.transformations.quaternion_about_axis(
pitch_imu_, (0, -1, 0))
quat3 = tf.transformations.quaternion_about_axis(
-azimuth_imu_, (0, 0, 1))
tf_quat = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_multiply(quat1, quat2), quat3)
geoMsg.transform.rotation.x = tf_quat[0]
geoMsg.transform.rotation.y = tf_quat[1]
geoMsg.transform.rotation.z = tf_quat[2]
geoMsg.transform.rotation.w = tf_quat[3]
tfMsg = tfMessage()
tfMsg.transforms.append(geoMsg)
bag.write('/tf', tfMsg, t=rosTime)
bag.close()
print('Finished')
def go():
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((host, port))
first = True
buf = ''
count = 0
while True:
d = s.recv(2**12)
if not d:
break
buf += d
lines = buf.split('\n')
buf = lines[-1]
for line in lines[:-1]:
if first:
first = False
continue
if count % decimation == 0:
d = json.loads(line)
ecef_cov = numpy.array(d[
'X_position_relative_position_orientation_ecef_covariance']
)
absodom_pub.publish(
Odometry(
header=Header(
stamp=rospy.Time.from_sec(d['timestamp'] * 1e-9),
frame_id='/ecef',
),
child_frame_id=child_frame_id,
pose=PoseWithCovariance(
pose=Pose(
position=Point(*d['position_ecef']),
orientation=Quaternion(
**d['orientation_ecef']),
),
covariance=numpy.vstack((
numpy.hstack(
(ecef_cov[0:3, 0:3], ecef_cov[0:3, 6:9])),
numpy.hstack(
(ecef_cov[6:9, 0:3], ecef_cov[6:9, 6:9])),
)).flatten(),
),
twist=TwistWithCovariance(
twist=Twist(
linear=Vector3(*d['velocity_body']),
angular=Vector3(*d['angular_velocity_body']),
),
covariance=numpy.vstack((
numpy.hstack((d['X_velocity_body_covariance'],
numpy.zeros((3, 3)))),
numpy.hstack(
(numpy.zeros((3, 3)),
d['X_angular_velocity_body_covariance'])),
)).flatten(),
),
))
odom_pub.publish(
Odometry(
header=Header(
stamp=rospy.Time.from_sec(d['timestamp'] * 1e-9),
frame_id='/enu',
),
child_frame_id=child_frame_id,
pose=PoseWithCovariance(
pose=Pose(
position=Point(*d['relative_position_enu']),
orientation=Quaternion(**d['orientation_enu']),
),
covariance=numpy.array(d[
'X_relative_position_orientation_enu_covariance']
).flatten(),
),
twist=TwistWithCovariance(
twist=Twist(
linear=Vector3(*d['velocity_body']),
angular=Vector3(*d['angular_velocity_body']),
),
covariance=numpy.vstack((
numpy.hstack((d['X_velocity_body_covariance'],
numpy.zeros((3, 3)))),
numpy.hstack(
(numpy.zeros((3, 3)),
d['X_angular_velocity_body_covariance'])),
)).flatten(),
),
))
clock_error_pub.publish(Float64(d['X_clock_error']))
tf_pub.publish(
tfMessage(transforms=[
TransformStamped(
header=Header(
stamp=rospy.Time.from_sec(d['timestamp'] *
1e-9),
frame_id='/enu',
),
child_frame_id=child_frame_id,
transform=Transform(
translation=Point(*d['relative_position_enu']),
rotation=Quaternion(**d['orientation_enu']),
),
),
], ))
count += 1
iterations = 10000
t = tf.Transformer()
def mkm():
m = geometry_msgs.msg.TransformStamped()
m.header.frame_id = "PARENT"
m.child_frame_id = "THISFRAME"
m.transform.translation.y = 5.0
m.transform.rotation = geometry_msgs.msg.Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, 0))
return m
tm = tfMessage([mkm() for i in range(20)])
def deserel_to_string(o):
s = StringIO.StringIO()
o.serialize(s)
return s.getvalue()
mstr = deserel_to_string(tm)
class Timer:
def __init__(self, func):
self.func = func
from tf.msg import tfMessage
import tf
iterations = 10000
t = tf.Transformer()
def mkm():
m = geometry_msgs.msg.TransformStamped()
m.header.frame_id = "PARENT"
m.child_frame_id = "THISFRAME"
m.transform.translation.y = 5.0
m.transform.rotation = geometry_msgs.msg.Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0))
return m
tm = tfMessage([mkm() for i in range(20)])
def deserel_to_string(o):
s = StringIO.StringIO()
o.serialize(s)
return s.getvalue()
mstr = deserel_to_string(tm)
class Timer:
def __init__(self, func):
self.func = func
def mean(self, iterations = 1000000):
started = time.time()
for i in xrange(iterations):
self.func()
