def gnss_ins_sim_recorder():
"""
Record simulated GNSS/IMU data as ROS bag
"""
# ensure gnss_ins_sim_node is unique:
rospy.init_node('gnss_ins_sim_recorder_node')
# parse params:
motion_def_name = rospy.get_param('/gnss_ins_sim_recorder_node/motion_file')
sample_freq_imu = rospy.get_param('/gnss_ins_sim_recorder_node/sample_frequency/imu')
sample_freq_gps = rospy.get_param('/gnss_ins_sim_recorder_node/sample_frequency/gps')
topic_name_imu = rospy.get_param('/gnss_ins_sim_recorder_node/topic_name')
rosbag_output_path = rospy.get_param('/gnss_ins_sim_recorder_node/output_path')
rosbag_output_name = rospy.get_param('/gnss_ins_sim_recorder_node/output_name')
# generate simulated data:
motion_def_path = os.path.join(
rospkg.RosPack().get_path('gnss_ins_sim'), 'config', 'motion_def', motion_def_name
)
imu_simulator = get_gnss_ins_sim(
# motion def file:
motion_def_path,
# gyro-accel/gyro-accel-mag sample rate:
sample_freq_imu,
# GPS sample rate:
sample_freq_gps
)
with rosbag.Bag(
os.path.join(rosbag_output_path, rosbag_output_name), 'w'
) as bag:
# get timestamp base:
timestamp_start = rospy.Time.now()
is_first = False;
for measurement in imu_simulator:
#this style learn from generator to genarate data
# init:
msg = Imu()
# a. set header:
msg.header.frame_id = 'ENU'
msg.header.stamp = timestamp_start + rospy.Duration.from_sec(measurement['stamp'])
# b. set orientation estimation:
msg.orientation.x = 0.0
msg.orientation.y = 0.0
msg.orientation.z = 0.0
msg.orientation.w = 1.0
# c. gyro:
msg.angular_velocity.x = measurement['data']['gyro_x']
msg.angular_velocity.y = measurement['data']['gyro_y']
msg.angular_velocity.z = measurement['data']['gyro_z']
msg.linear_acceleration.x = measurement['data']['accel_x']
msg.linear_acceleration.y = measurement['data']['accel_y']
msg.linear_acceleration.z = measurement['data']['accel_z']
# write:
bag.write(topic_name_imu, msg, msg.header.stamp)
# init:
msg = Odometry()
# a. set header:
msg.header.frame_id = 'inertial'
msg.child_frame_id = 'inertial'
msg.header.stamp = timestamp_start + rospy.Duration.from_sec(measurement['stamp'])
## b. set orientation:
msg.pose.pose.orientation.x = measurement['data']['ref_att_quat_x']
msg.pose.pose.orientation.y = measurement['data']['ref_att_quat_y']
msg.pose.pose.orientation.z = measurement['data']['ref_att_quat_z']
msg.pose.pose.orientation.w = measurement['data']['ref_att_quat_w']
# c. set position:
msg.pose.pose.position.x = measurement['data']['ref_pos_x']
msg.pose.pose.position.y = measurement['data']['ref_pos_y']
msg.pose.pose.position.z = measurement['data']['ref_pos_z']
# c. set velocity:
msg.twist.twist.linear.x = measurement['data']['ref_vel_x']
msg.twist.twist.linear.y = measurement['data']['ref_vel_y']
msg.twist.twist.linear.z = measurement['data']['ref_vel_z']
# write:
bag.write('/pose/ground_truth', msg, msg.header.stamp)
msg.angular_velocity.z = mat['in_data']['IMU'][0][0]['gyro'][0][0][2][k]
msg.angular_velocity_covariance[0] = cov_gyro
msg.angular_velocity_covariance[4] = cov_gyro
msg.angular_velocity_covariance[8] = cov_gyro
imu_data.append(msg)
# Load GNSS data
gnss_data = []
for k in range(len(mat['in_data']['GNSS'][0][0]['t'][0][0])):
msg = Odometry()
t = mat['in_data']['GNSS'][0][0]['t'][0][0][k][0]
msg.header.stamp = base_time + rospy.Duration.from_sec(t)
msg.header.frame_id = 'map'
msg.child_frame_id = 'base_link'
msg.pose.pose.position.x = mat['in_data']['GNSS'][0][0]['pos_ned'][0][0][0][k] # north
msg.pose.pose.position.y = mat['in_data']['GNSS'][0][0]['pos_ned'][0][0][1][k] # east
msg.pose.pose.position.z = -mat['in_data']['GNSS'][0][0]['pos_ned'][0][0][2][k] # down
msg.pose.covariance[0] = cov_gnss
msg.pose.covariance[7] = cov_gnss
msg.pose.covariance[14] = cov_gnss
# Heading
if k > 0:
dx = msg.pose.pose.position.x - gnss_data[k-1].pose.pose.position.x
dy = msg.pose.pose.position.y - gnss_data[k-1].pose.pose.position.y
heading = atan2(dy, dx)
quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, heading)
msg.pose.pose.orientation.x = quaternion[0]
