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]