def odom_callback(self):
odom = gps_pb2.Gps()
now = rospy.get_time()
odom.header.timestamp_sec = now
odom.localization.position.x = self.position_x
odom.localization.position.y = self.position_y
odom.localization.position.z = self.position_z
odom.localization.orientation.qx = self.orientation_x
odom.localization.orientation.qy = self.orientation_y
odom.localization.orientation.qz = self.orientation_z
odom.localization.orientation.qw = self.orientation_w
odom.localization.linear_velocity.x = self.linear_velocity_x
odom.localization.linear_velocity.y = self.linear_velocity_y
odom.localization.linear_velocity.z = self.linear_velocity_z
rospy.loginfo("%s" % odom)
self.gps_odom_pub.publish(odom)
def publish_odom(self):
odom = gps_pb2.Gps()
now = cyber_time.Time.now().to_sec()
odom.header.timestamp_sec = now
odom.header.module_name = "odometry"
odom.header.sequence_num = self.sequence_num
self.sequence_num = self.sequence_num + 1
odom.localization.position.x = self.position_x
odom.localization.position.y = self.position_y
odom.localization.position.z = self.position_z
odom.localization.orientation.qx = self.orientation_x
odom.localization.orientation.qy = self.orientation_y
odom.localization.orientation.qz = self.orientation_z
odom.localization.orientation.qw = self.orientation_w
odom.localization.linear_velocity.x = self.linear_velocity_x
odom.localization.linear_velocity.y = self.linear_velocity_y
odom.localization.linear_velocity.z = self.linear_velocity_z
#self.logger.info("%s"%odom)
self.gps_odom_pub.write(odom)
def _init_parser(self):
self._msg_parser = gps_pb2.Gps()
'/apollo/sensor/gnss/odometry',
'/apollo/sensor/gnss/raw_data',
'/apollo/sensor/gnss/rtk_eph',
'/apollo/sensor/gnss/rtk_obs',
'/apollo/sensor/gnss/heading',
'/apollo/sensor/mobileye',
'/tf',
'/tf_static',
]
CYBER_PATH = os.environ['CYBER_PATH']
CYBER_RECORD_HEADER_LENGTH = 2048
IMAGE_OBJ = sensor_image_pb2.Image()
POINTCLOUD_OBJ = pointcloud_pb2.PointCloud()
GPS_OBJ = gps_pb2.Gps()
def process_dir(path, operation):
"""Create or remove directory."""
try:
if operation == 'create':
print("create folder %s" % path)
os.makedirs(path)
elif operation == 'remove':
os.remove(path)
else:
print('Error! Unsupported operation %s for directory.' % operation)
return False
except OSError as e:
print('Failed to %s directory: %s. Error: %s' %
(operation, path, six.text_type(e)))