def save_dynamic_tf(bag, kitti_type, kitti, initial_time):
print("Exporting time dependent transformations")
if kitti_type.find("raw") != -1:
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
tf_oxts_msg = TFMessage()
tf_oxts_transform = TransformStamped()
tf_oxts_transform.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
tf_oxts_transform.header.frame_id = 'world'
tf_oxts_transform.child_frame_id = 'base_link'
transform = (oxts.T_w_imu)
T_imu_to_base_link = np.eye(4, 4)
T_imu_to_base_link[0:3, 3] = [2.71/2.0+0.05, -0.32, -0.93]
transform = np.dot(transform,T_imu_to_base_link)
t = transform[0:3, 3]
q = tf.transformations.quaternion_from_matrix(transform)
oxts_tf = Transform()
oxts_tf.translation.x = t[0]
oxts_tf.translation.y = t[1]
oxts_tf.translation.z = t[2]
oxts_tf.rotation.x = q[0]
oxts_tf.rotation.y = q[1]
oxts_tf.rotation.z = q[2]
oxts_tf.rotation.w = q[3]
tf_oxts_transform.transform = oxts_tf
tf_oxts_msg.transforms.append(tf_oxts_transform)
bag.write('/tf', tf_oxts_msg, tf_oxts_msg.transforms[0].header.stamp)
elif kitti_type.find("odom") != -1:
timestamps = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
for timestamp, tf_matrix in zip(timestamps, kitti.poses):
tf_msg = TFMessage()
tf_stamped = TransformStamped()
tf_stamped.header.stamp = rospy.Time.from_sec(timestamp)
tf_stamped.header.frame_id = 'camera_init'
tf_stamped.child_frame_id = 'camera_gray_left'
t = tf_matrix[0:3, 3]
q = tf.transformations.quaternion_from_matrix(tf_matrix)
transform = Transform()
transform.translation.x = t[0]
transform.translation.y = t[1]
transform.translation.z = t[2]
transform.rotation.x = q[0]
transform.rotation.y = q[1]
transform.rotation.z = q[2]
transform.rotation.w = q[3]
tf_stamped.transform = transform
tf_msg.transforms.append(tf_stamped)
bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp)
def _receive_message(self,message):
global my
rospy.loginfo(rospy.get_caller_id() + " Message type %s ",self._message_type)
rospy.loginfo(rospy.get_caller_id() + " Time from previous message %s ",(rospy.get_time()-my))
my=rospy.get_time()
try:
msg=TFMessage()
msg1=TransformStamped()
list_msg=[]
for i in range(len(message['transforms'])) :
msg1.header.seq=message['transforms'][i]['header']['seq']
msg1.header.stamp.secs=message['transforms'][i]['header']['stamp']['secs']
msg1.header.stamp.nsecs=message['transforms'][i]['header']['stamp']['nsecs']
msg1.header.frame_id=message['transforms'][i]['header']['frame_id']
msg1.child_frame_id=message['transforms'][i]['child_frame_id']
msg1.transform.translation.x=message['transforms'][i]['transform']['translation']['x']
msg1.transform.translation.y=message['transforms'][i]['transform']['translation']['y']
msg1.transform.translation.z=message['transforms'][i]['transform']['translation']['z']
msg1.transform.rotation.x=message['transforms'][i]['transform']['rotation']['x']
msg1.transform.rotation.y=message['transforms'][i]['transform']['rotation']['y']
msg1.transform.rotation.z=message['transforms'][i]['transform']['rotation']['z']
msg1.transform.rotation.w=message['transforms'][i]['transform']['rotation']['w']
list_msg.append(msg1)
msg=TFMessage(list_msg)
self._rosPub=rospy.Publisher(self._local_topic_name, TFMessage, queue_size=10)
self._rosPub.publish(msg)
except:
print('Error')
def wjx_save_dynamic_tf(bag, kitti, initial_time):
timestamps = map(lambda x: initial_time + x.total_seconds(),
kitti.timestamps)
for timestamp, tf_matrix in zip(timestamps, kitti.poses):
print len(kitti.poses)
tf_msg = TFMessage()
tf_stamped = TransformStamped()
tf_stamped.header.stamp = rospy.Time.from_sec(timestamp)
tf_stamped.header.frame_id = 'world'
tf_stamped.child_frame_id = 'camera_left'
t = tf_matrix[0:3, 3]
q = tf.transformations.quaternion_from_matrix(tf_matrix)
transform = Transform()
transform.translation.x = t[0]
transform.translation.y = t[1]
transform.translation.z = t[2]
transform.rotation.x = q[0]
transform.rotation.y = q[1]
transform.rotation.z = q[2]
transform.rotation.w = q[3]
tf_stamped.transform = transform
tf_msg.transforms.append(tf_stamped)
bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp)
def save_static_transforms(bag, kitti, imu_frame_id, velo_frame_id):
print("Exporting static transformations")
T_base_link_to_imu = np.eye(4, 4)
T_base_link_to_imu[0:3, 3] = [-2.71 / 2.0 - 0.05, 0.32, 0.93]
# tf_static
transforms = [
(imu_frame_id, 'base_link', inv(T_base_link_to_imu)),
(imu_frame_id, velo_frame_id, inv(kitti.calib.T_velo_imu)),
(imu_frame_id, cameras[0].frame_id, inv(kitti.calib.T_cam0_imu)),
(imu_frame_id, cameras[1].frame_id, inv(kitti.calib.T_cam1_imu)),
(imu_frame_id, cameras[2].frame_id, inv(kitti.calib.T_cam2_imu)),
(imu_frame_id, cameras[3].frame_id, inv(kitti.calib.T_cam3_imu))
]
tfm = TFMessage()
for transform in transforms:
t = get_static_transform(from_frame_id=transform[0],
to_frame_id=transform[1],
transform=transform[2])
tfm.transforms.append(t)
for timestamp in kitti.timestamps:
time = to_rostime(timestamp)
for transform in tfm.transforms:
transform.header.stamp = time
bag.write('/tf_static', tfm, t=time)
def calculate_joints(self, position):
self.joint_angle_matricies = []
for angle in position:
self.joint_angle_matricies.append(transformations.euler_matrix(0, 0, angle))
T_c = [np.identity(4)]
tf_msg = TFMessage()
for index in xrange(len(self.urdf_transform_matricies)):
urdf_transform_matrix = self.urdf_transform_matricies[index]
joint_angle_matrix = self.joint_angle_matricies[index]
transform_matrix = np.dot(urdf_transform_matrix, joint_angle_matrix)
tf_stamp = TransformStamped()
tf_stamp.child_frame_id = self.urdf_transforms[index].child_frame_id
tf_stamp.header.frame_id = self.urdf_transforms[index].header.frame_id
if index in (8, 10): #sets parent of fingers to link6
index = 6
T_c.append(np.dot(T_c[index], transform_matrix))
tf_stamp.transform = msgify(Transform, T_c[-1])
#tf_stamp.transform = msgify(Transform, transform_matrix)
tf_msg.transforms.append(tf_stamp)
#print transforms.header
#print link_states.name[-1]
#print link_states.pose[-1]
#print '-----------------------------------------------'
return tf_msg
def tf_listener():
global all_static_transforms
rospy.init_node('static_tf2_listener', anonymous=True)
sub = rospy.Subscriber("tf_static", TFMessage, tf_msg_callback)
pub = rospy.Publisher('tf_static', TFMessage, latch=True, queue_size=1)
# check what transforms we've heart
while not rospy.is_shutdown():
print("Num. of TFs received so far: " +
str(len(all_static_transforms)))
control_char = raw_input(
"c: clear, p: publish latched (and un-suscribe), q: quit: ")
if control_char == "c":
all_static_transforms = []
elif control_char == "p":
sub.unregister()
time.sleep(2)
newTFMessage = TFMessage()
newTFMessage.transforms = all_static_transforms
pub.publish(newTFMessage)
print(
"Publishing the latched tf_static thing and waiting for the final CTRL+C..."
)
break
elif control_char == "q":
return
else:
print("Input c, p, or q. Not " + control_char + ". Facepalm.")
rospy.spin()
def send_msgs(self):
"""
Function to actually send the collected ROS messages out via the publisher
:return:
"""
# prepare tf message
tf_msg = TFMessage(self.tf_to_publish)
try:
self.publishers['tf'].publish(tf_msg)
except rospy.ROSSerializationException as error:
rospy.logwarn(
"Failed to serialize message on publishing: {}".format(error))
except Exception as error: # pylint: disable=broad-except
rospy.logwarn("Failed to publish message: {}".format(error))
for publisher, msg in self.msgs_to_publish:
try:
publisher.publish(msg)
except rospy.ROSSerializationException as error: # pylint: disable=broad-except
rospy.logwarn(
"Failed to serialize message on publishing: {}".format(
error))
except Exception as error: # pylint: disable=broad-except
rospy.logwarn("Failed to publish message: {}".format(error))
self.msgs_to_publish = []
self.tf_to_publish = []
def send_msgs(self):
for publisher, msg in self.msgs_to_publish:
self.bag.write(publisher.name, msg, self.cur_time)
tf_msg = TFMessage(self.tf_to_publish)
self.bag.write('tf', tf_msg, self.cur_time)
super(CarlaRosBridgeWithBagExperiment, self).send_msgs()
def gen_transform(nanostamp):
import tf
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import Transform
from geometry_msgs.msg import TransformStamped
tf_msg = TFMessage()
tf_transform = TransformStamped()
tf_transform.header.stamp = nanostamp_to_rostime(nanostamp)
tf_transform.header.frame_id = 'src_frame'
tf_transform.child_frame_id = 'child_frame'
transform = Transform()
r_4x4 = np.ones((4, 4))
q = tf.transformations.quaternion_from_matrix(r_4x4)
transform.rotation.x = q[0]
transform.rotation.y = q[1]
transform.rotation.z = q[2]
transform.rotation.w = q[3]
transform.translation.x = 1
transform.translation.y = 2
transform.translation.z = 3
tf_transform.transform = transform
tf_msg.transforms.append(tf_transform)
return tf_msg
def convert(self):
# Keep first timestamp for publishing static TF message
static_timestamp = self.lidar_timestamps[0]
# Publish LiDAR PointCloud messages
for timestamp in tqdm(self.lidar_timestamps, desc='LiDAR, Pose'):
lidar_msg = self.get_lidar_message(timestamp)
self.bag.write(self.lidar_topic, lidar_msg,
utils.ros_time_from_nsecs(timestamp))
# Publish ego vehicle pose
tf_msg = utils.argoverse_pose_to_transform_message(self.dataset_dir,
self.log_id, self.map_frame, self.vehicle_frame, timestamp, read_json_file)
self.bag.write('/tf', tf_msg, utils.ros_time_from_nsecs(timestamp))
# Publish camera messages
for camera in tqdm(self.cameras_list, desc='CameraInfo'):
static_timestamp = min(static_timestamp, self.image_timestamps[camera][0])
for timestamp in tqdm(self.image_timestamps[camera], desc=camera, leave=False):
camera_info_msg = self.get_camera_info_message(camera, timestamp)
self.bag.write(self.camera_info_topic_template % camera, camera_info_msg,
utils.ros_time_from_nsecs(timestamp))
# Publish static TF messages
tf_msg = TFMessage()
for camera in tqdm(self.camera_static_tf_dict, desc='Static TF'):
msg = self.camera_static_tf_dict[camera]
msg.header.stamp = utils.ros_time_from_nsecs(static_timestamp)
tf_msg.transforms.append(msg)
self.bag.write('/tf_static', tf_msg, utils.ros_time_from_nsecs(static_timestamp))
# Close rosbag file
self.bag.close()
def tf_publisher_callback(self):
# Publish TF for the next step
# we use one step in advance to make sure no sensor data are published before
tFMessage = TFMessage()
nextTime = self.robot.getTime() + 0.001 * self.timestep
nextSec = int(nextTime)
# rounding prevents precision issues that can cause problems with ROS timers
nextNanosec = int(round(1000 * (nextTime - nextSec)) * 1.0e+6)
for name in self.nodes:
position = self.nodes[name].getPosition()
orientation = self.nodes[name].getOrientation()
transformStamped = TransformStamped()
transformStamped.header.stamp = Time(sec=nextSec,
nanosec=nextNanosec)
transformStamped.header.frame_id = 'map'
transformStamped.child_frame_id = name
transformStamped.transform.translation.x = position[0]
transformStamped.transform.translation.y = position[1]
transformStamped.transform.translation.z = position[2]
qw = math.sqrt(1.0 + orientation[0] + orientation[4] +
orientation[8]) / 2.0
qx = (orientation[7] - orientation[5]) / (4.0 * qw)
qy = (orientation[2] - orientation[6]) / (4.0 * qw)
qz = (orientation[3] - orientation[1]) / (4.0 * qw)
transformStamped.transform.rotation.x = qx
transformStamped.transform.rotation.y = qy
transformStamped.transform.rotation.z = qz
transformStamped.transform.rotation.w = qw
tFMessage.transforms.append(transformStamped)
self.tfPublisher.publish(tFMessage)
def save_static_transforms(bag,
kitti_type,
transforms,
timestamps,
onlygray=False):
print("Exporting static transformations")
tfm = TFMessage()
for transform in transforms:
t = get_static_transform(from_frame_id=transform[0],
to_frame_id=transform[1],
transform=transform[2])
tfm.transforms.append(t)
if kitti_type.find("raw") != -1:
for t in timestamps:
time = rospy.Time.from_sec(float(t.strftime("%s.%f")))
for tf in tfm.transforms:
tf.header.stamp = time
#tf.header.seq += 1
bag.write('/tf_static', tfm, t=time)
elif kitti_type.find("odom") != -1:
for t in timestamps:
time = rospy.Time.from_sec(t)
for tf in tfm.transforms:
tf.header.stamp = time
#tf.header.seq += 1
bag.write('/tf_static', tfm, t=time)
def publish_items(self, items):
if not items:
return
if not self.publish_tf:
return
transforms = [item_to_tf(item, i) for i, item in enumerate(items)]
self.pub_tf.publish(TFMessage(transforms=transforms))
def sendTransform(self, transform: Union[TransformStamped, List[TransformStamped]]) -> None:
if not isinstance(transform, list):
if hasattr(transform, '__iter__'):
transform = list(transform)
else:
transform = [transform]
self.pub_tf.publish(TFMessage(transforms=transform))
def publish_perception_frame(self, person, header):
offset = euler_matrix(0, math.radians(90), math.radians(90), "rxyz")
trans = [
person.head_pose.position.x, person.head_pose.position.y,
person.head_pose.position.z
]
rot = [
person.head_pose.orientation.x, person.head_pose.orientation.y,
person.head_pose.orientation.z, person.head_pose.orientation.w
]
transform = numpy.dot(transformation_matrix(trans, rot), offset)
t = TransformStamped()
t.header.frame_id = header.frame_id
t.header.stamp = header.stamp
t.child_frame_id = "human_head_gaze-" + str(person.person_id)
position = translation_from_matrix(transform)
t.transform.translation.x = position[0]
t.transform.translation.y = position[1]
t.transform.translation.z = position[2]
orientation = quaternion_from_matrix(transform)
t.transform.rotation.x = orientation[0]
t.transform.rotation.y = orientation[1]
t.transform.rotation.z = orientation[2]
t.transform.rotation.w = orientation[3]
tfm = TFMessage([t])
self.ros_pub["tf"].publish(tfm)
def _publish_objects(self):
def convert_pose_to_tf_msg(pose):
transform_msg = Transform()
transform_msg.translation.x = pose.position.x * 0.001
transform_msg.translation.y = pose.position.y * 0.001
transform_msg.translation.z = pose.position.z * 0.001
transform_msg.rotation.w = pose.rotation.q0
transform_msg.rotation.x = pose.rotation.q1
transform_msg.rotation.y = pose.rotation.q2
transform_msg.rotation.z = pose.rotation.q3
return transform_msg
def convert_pose_to_ros_msg(pose, name):
transform_msg = TransformStamped()
transform_msg.header.stamp = rospy.Time.now()
transform_msg.header.frame_id = 'map'
transform_msg.child_frame_id = name
transform_msg.transform = convert_pose_to_tf_msg(pose)
return transform_msg
tf_messages = [
convert_pose_to_ros_msg(obj.pose, 'cube_' + str(obj.object_id))
for obj in self._cozmo.world.visible_objects
]
tf_messages.append(convert_pose_to_ros_msg(self._cozmo.pose, 'cozmo'))
self._tf_br.publish(TFMessage(tf_messages))
def __init__(self):
self.seq = -1
self.initseq = -1
self.goalseq = -1
qframe = tf_conversions.transformations.quaternion_from_euler(0, 0, 0)
self.simpletf = TFMessage()
self.simpletf.transforms.append(TransformStamped())
self.simpletf.transforms[0].header.frame_id = "global_ref"
self.simpletf.transforms[0].child_frame_id = "car_frame"
self.simpletf.transforms[0].transform.rotation.x = qframe[0]
self.simpletf.transforms[0].transform.rotation.y = qframe[1]
self.simpletf.transforms[0].transform.rotation.z = qframe[2]
self.simpletf.transforms[0].transform.rotation.w = qframe[3]
self.simpletf.transforms[0].transform.translation.x = 0
self.simpletf.transforms[0].transform.translation.y = 0
qmap = tf_conversions.transformations.quaternion_from_euler(0, 0, 0)
self.simplemap = OccupancyGrid()
self.simplemap.header.frame_id = "car_frame"
self.simplemap.info.resolution = resolution #0.5
self.simplemap.info.width = 200 #100
self.simplemap.info.height = 200 #100
self.simplemap.info.origin.position.x = -3 #-25
self.simplemap.info.origin.position.y = -3 #-25
self.simplemap.info.origin.orientation.x = qmap[0]
self.simplemap.info.origin.orientation.y = qmap[1]
self.simplemap.info.origin.orientation.z = qmap[2]
self.simplemap.info.origin.orientation.w = qmap[3]
self.simplemap.data = self.makemap(self.simplemap.info.height,
self.simplemap.info.width)
def GetTFFromState(state, spot_wrapper):
"""Maps robot link state data from robot state proto to ROS TFMessage message
Args:
data: Robot State proto
spot_wrapper: A SpotWrapper object
Returns:
TFMessage message
"""
tf_msg = TFMessage()
for frame_name in state.kinematic_state.transforms_snapshot.child_to_parent_edge_map:
if state.kinematic_state.transforms_snapshot.child_to_parent_edge_map.get(frame_name).parent_frame_name:
transform = state.kinematic_state.transforms_snapshot.child_to_parent_edge_map.get(frame_name)
new_tf = TransformStamped()
local_time = spot_wrapper.robotToLocalTime(state.kinematic_state.acquisition_timestamp)
new_tf.header.stamp = rospy.Time(local_time.seconds, local_time.nanos)
new_tf.header.frame_id = transform.parent_frame_name
new_tf.child_frame_id = frame_name
new_tf.transform.translation.x = transform.parent_tform_child.position.x
new_tf.transform.translation.y = transform.parent_tform_child.position.y
new_tf.transform.translation.z = transform.parent_tform_child.position.z
new_tf.transform.rotation.x = transform.parent_tform_child.rotation.x
new_tf.transform.rotation.y = transform.parent_tform_child.rotation.y
new_tf.transform.rotation.z = transform.parent_tform_child.rotation.z
new_tf.transform.rotation.w = transform.parent_tform_child.rotation.w
tf_msg.transforms.append(new_tf)
return tf_msg
def update(self):
try:
with self.get_god_map() as god_map:
tf_msg = TFMessage()
if god_map.unsafe_get_data(
identifier.publish_attached_objects):
robot_links = set(self.unsafe_get_robot().get_link_names())
attached_links = robot_links - self.original_links
if attached_links:
get_fk = self.unsafe_get_robot().get_fk_pose
for link_name in attached_links:
parent_link_name = self.unsafe_get_robot(
).get_parent_link_of_link(link_name)
fk = get_fk(parent_link_name, link_name)
tf = self.make_transform(fk.header.frame_id,
link_name, fk.pose)
tf_msg.transforms.append(tf)
if god_map.unsafe_get_data(identifier.publish_world_objects):
world_objects = self.unsafe_get_world().get_objects()
map_frame = god_map.unsafe_get_data(identifier.map_frame)
for object in world_objects.values():
tf = self.make_transform(map_frame, object.get_name(),
object.base_pose)
tf_msg.transforms.append(tf)
self.tf_pub.publish(tf_msg)
except KeyError as e:
pass
except UnboundLocalError as e:
pass
except ValueError as e:
pass
return Status.SUCCESS
def callBack(linkStates):
global delta, first
found = False
for i in range(len(linkStates.name)):
if linkStates.name[i] == gazebo_frame_id:
p = linkStates.pose[i]
found = True
break
if not found:
roslog.warn("Gazebo link state \"" + gazebo_frame_id +
"\" not found, cannot generate ground truth.")
return
t = TransformStamped()
t.header.frame_id = frame_id
t.header.stamp = rospy.Time.now()
t.child_frame_id = child_frame_id
t.transform.translation.x = p.position.x
t.transform.translation.y = p.position.y
t.transform.translation.z = p.position.z
t.transform.rotation.x = p.orientation.x
t.transform.rotation.y = p.orientation.y
t.transform.rotation.z = p.orientation.z
t.transform.rotation.w = p.orientation.w
tf_pub.publish(TFMessage([t]))
def drone_att_callback(self, rcv):
print("POSE: " + str(rcv))
self.lastPose = rcv
rcv.x *= math.pi / 180.0
rcv.y *= math.pi / 180.0
rcv.z *= math.pi / 180.0
# Publish pose for rviz
msg = TransformStamped()
msg.header.frame_id = "/map"
msg.header.stamp = Clock().now().to_msg()
msg.child_frame_id = "/base_footprint_drone_attitude"
quat = self.euler_to_quaternion(rcv.y, rcv.x, rcv.z)
orientation = Quaternion()
orientation.x = quat[0]
orientation.y = quat[1]
orientation.z = quat[2]
orientation.w = quat[3]
msg.transform.rotation = orientation
tfmsg = TFMessage()
tfmsg.transforms = [msg]
self.pub_tf.publish(tfmsg)
# Publish velocity
msg = Twist()
msg.linear.x = rcv.x
msg.angular.z = rcv.y
self.pub_vel.publish(msg)
def sendTransform(self, transform):
if not isinstance(transform, list):
if hasattr(transform, '__iter__'):
transform = list(transform)
else:
transform = [transform]
self.pub_tf.publish(TFMessage(transforms=transform))
def tf_publisher_callback(self):
"""Publish the current transforms."""
# Publish TF for the next step
# we use one step in advance to make sure no sensor data are published before
stamp = Time(seconds=self.robot.getTime() +
0.001 * self.timestep).to_msg()
tFMessage = TFMessage()
for name in self.nodes:
position = self.nodes[name].getPosition()
orientation = self.nodes[name].getOrientation()
transformStamped = TransformStamped()
transformStamped.header.stamp = stamp
transformStamped.header.frame_id = 'map'
transformStamped.child_frame_id = self.prefix + name
transformStamped.transform.translation.x = position[0]
transformStamped.transform.translation.y = position[1]
transformStamped.transform.translation.z = position[2]
qw = math.sqrt(1.0 + orientation[0] + orientation[4] +
orientation[8]) / 2.0
qx = (orientation[7] - orientation[5]) / (4.0 * qw)
qy = (orientation[2] - orientation[6]) / (4.0 * qw)
qz = (orientation[3] - orientation[1]) / (4.0 * qw)
transformStamped.transform.rotation.x = qx
transformStamped.transform.rotation.y = qy
transformStamped.transform.rotation.z = qz
transformStamped.transform.rotation.w = qw
tFMessage.transforms.append(transformStamped)
self.tfPublisher.publish(tFMessage)
def publish_human_tf_frames(self):
for node in self.target.scene.nodes:
if re.match("human-", node.name) and node.type == CAMERA:
t = TransformStamped()
t.header.frame_id = "map"
t.header.stamp = rospy.Time.now()
t.child_frame_id = node.name
position = translation_from_matrix(get_world_transform(self.target.scene, node))
t.transform.translation.x = position[0]
t.transform.translation.y = position[1]
t.transform.translation.z = position[2]
orientation = quaternion_from_matrix(get_world_transform(self.target.scene, node))
t.transform.rotation.x = orientation[0]
t.transform.rotation.y = orientation[1]
t.transform.rotation.z = orientation[2]
t.transform.rotation.w = orientation[3]
# Publish a human footprint
t_footprint = TransformStamped()
t_footprint.header.frame_id = "map"
t_footprint.header.stamp = rospy.Time.now()
t_footprint.child_frame_id = node.name + "_footprint"
t_footprint.transform.translation.x = t.transform.translation.x
t_footprint.transform.translation.y = t.transform.translation.y
t_footprint.transform.translation.z = 0
roll, pitch, yaw = euler_from_quaternion(orientation)
rx, ry, rz, rw = quaternion_from_euler(0, 0, yaw + math.pi / 2)
t_footprint.transform.rotation.x = rx
t_footprint.transform.rotation.y = ry
t_footprint.transform.rotation.z = rz
t_footprint.transform.rotation.w = rw
tfm = TFMessage([t, t_footprint])
self.ros_pub["tf"].publish(tfm)
def send_transform_message(self, transform):
"""
:param transform: geometry_msgs.msg.TransformStamped
Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``.
"""
tfm = TFMessage([transform])
self.pub_tf.publish(tfm)
def __init__(self):
self.bridge_rgb = CvBridge()
self.msg_rgb = Image()
self.bridge_d = CvBridge()
self.msg_d = Image()
self.msg_info = CameraInfo()
self.msg_tf = TFMessage()
def publish_tf(self):
# modded from https://github.com/cyberbotics/webots_ros2/blob/master/webots_ros2_core/webots_ros2_core/tf_publisher.py
# Publish TF for the next step
# we use one step in advance to make sure no sensor data are published before
nextTime = self.robot.getTime() + 0.001 * self.timestep
nextSec = int(nextTime)
# rounding prevents precision issues that can cause problems with ROS timers
nextNanosec = int(round(1000 * (nextTime - nextSec)) * 1.0e+6)
node = self.robot.getSelf()
if node is None:
print("No self node")
return
position = node.getPosition()
orientation = node.getOrientation()
transformStamped = TransformStamped()
transformStamped.header.stamp = Time(sec=nextSec, nanosec=nextNanosec)
transformStamped.header.frame_id = 'map'
transformStamped.child_frame_id = self.robot.getName()
transformStamped.transform.translation.x = position[0]
transformStamped.transform.translation.y = position[2] # ROS is Z-up
transformStamped.transform.translation.z = position[1]
qw = math.sqrt(1.0 + orientation[0] + orientation[4] +
orientation[8]) / 2.0
qx = (orientation[7] - orientation[5]) / (4.0 * qw)
qy = (orientation[2] - orientation[6]) / (4.0 * qw)
qz = (orientation[3] - orientation[1]) / (4.0 * qw)
transformStamped.transform.rotation.x = qx
transformStamped.transform.rotation.y = qy
transformStamped.transform.rotation.z = qz
transformStamped.transform.rotation.w = qw
self.pub.publish(TFMessage(transforms=[transformStamped]))
def callback(msg):
print "got imu"
# Offset to put the cloud in a different place than the map:
x_offset = 120
y_offset = 0
p = PoseWithCovarianceStamped()
p.header = msg.header
p.header.frame_id = "map"
p.pose.pose.orientation = msg.orientation
p.pose.pose.position.x = x_offset
p.pose.pose.position.y = y_offset
pub.publish(p)
tfmsg = TFMessage()
transformS = TransformStamped()
transformS.header = msg.header
transform = Transform()
transform.rotation = msg.orientation
transform.translation.x = x_offset
transform.translation.y = y_offset
transformS.transform = transform
transformS.child_frame_id = "base"
tfmsg.transforms.append(transformS)
pub_tf.publish(tfmsg)
def save_dynamic_tf(bag, timestamps, tf_matrices, child_frame_id):
print("Exporting time dependent transformations")
for timestamp, tf_matrix in zip(timestamps, tf_matrices):
tf_msg = TFMessage()
tf_stamped = TransformStamped()
tf_stamped.header.stamp = to_rostime(timestamp)
tf_stamped.header.frame_id = 'world'
tf_stamped.child_frame_id = child_frame_id
t = tf_matrix[0:3, 3]
q = quaternion_from_matrix(tf_matrix)
transform = Transform()
transform.translation.x = t[0]
transform.translation.y = t[1]
transform.translation.z = t[2]
transform.rotation.x = q[0]
transform.rotation.y = q[1]
transform.rotation.z = q[2]
transform.rotation.w = q[3]
tf_stamped.transform = transform
tf_msg.transforms.append(tf_stamped)
bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp)
def save_static_transforms(bag, transforms, timestamps):
print("Exporting static transformations")
tfm = TFMessage()
for transform in transforms:
t = get_static_transform(from_frame_id=transform[0],
to_frame_id=transform[1],
transform=transform[2])
tfm.transforms.append(t)
for timestamp in timestamps:
time = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
for i in range(len(tfm.transforms)):
tfm.transforms[i].header.stamp = time
bag.write('/tf_static', tfm, t=time)
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
twist_msg = TwistStamped()
twist_msg.header.frame_id = gps_frame_id
twist_msg.header.stamp = rospy.Time.from_sec(
float(timestamp.strftime("%s.%f")))
twist_msg.twist.linear.x = oxts.packet.vf
twist_msg.twist.linear.y = oxts.packet.vl
twist_msg.twist.linear.z = oxts.packet.vu
twist_msg.twist.angular.x = oxts.packet.wf
twist_msg.twist.angular.y = oxts.packet.wl
twist_msg.twist.angular.z = oxts.packet.wu
bag.write(topic, twist_msg, t=twist_msg.header.stamp)
