def publish(self):
# Estimator error
est_err_msg = Error()
est_err_msg.header.stamp = rospy.Time.now()
est_err_msg.x = self.est_error_pose[0]
est_err_msg.y = self.est_error_pose[1]
est_err_msg.z = self.est_error_pose[2]
est_err_msg.roll = self.est_error_pose[3]
est_err_msg.pitch = self.est_error_pose[4]
est_err_msg.yaw = self.est_error_pose[5]
est_err_msg.vx = self.est_error_pose[6]
est_err_msg.vy = self.est_error_pose[7]
est_err_msg.vz = self.est_error_pose[8]
est_err_msg.rmse_x = self.est_x_rmse
est_err_msg.rmse_y = self.est_y_rmse
est_err_msg.rmse_z = self.est_z_rmse
est_err_msg.rmse_roll = self.est_roll_rmse
est_err_msg.rmse_pitch = self.est_pitch_rmse
est_err_msg.rmse_yaw = self.est_yaw_rmse
est_err_msg.rmse_vx = self.est_vx_rmse
est_err_msg.rmse_vy = self.est_vy_rmse
est_err_msg.rmse_vz = self.est_vz_rmse
est_err_msg.dist_error = self.est_dist_error
est_err_msg.dist_traveled = self.dist_traveled
self.pub_est_err.publish(est_err_msg)
# DR error
dr_err_msg = Error()
dr_err_msg.header.stamp = rospy.Time.now()
dr_err_msg.x = self.dr_error_pose[0]
dr_err_msg.y = self.dr_error_pose[1]
dr_err_msg.z = self.dr_error_pose[2]
dr_err_msg.roll = self.dr_error_pose[3]
dr_err_msg.pitch = self.dr_error_pose[4]
dr_err_msg.yaw = self.dr_error_pose[5]
dr_err_msg.vx = self.dr_error_pose[6]
dr_err_msg.vy = self.dr_error_pose[7]
dr_err_msg.vz = self.dr_error_pose[8]
dr_err_msg.rmse_x = self.dr_x_rmse
dr_err_msg.rmse_y = self.dr_y_rmse
dr_err_msg.rmse_z = self.dr_z_rmse
dr_err_msg.rmse_roll = self.dr_roll_rmse
dr_err_msg.rmse_pitch = self.dr_pitch_rmse
dr_err_msg.rmse_yaw = self.dr_yaw_rmse
dr_err_msg.rmse_vx = self.dr_vx_rmse
dr_err_msg.rmse_vy = self.dr_vy_rmse
dr_err_msg.rmse_vz = self.dr_vz_rmse
dr_err_msg.dist_error = self.dr_dist_error
dr_err_msg.dist_traveled = self.dist_traveled
self.pub_dr_err.publish(dr_err_msg)
def PublishEst(self):
# Est error
est_err_msg = Error()
est_err_msg.header.stamp = rospy.Time.now()
est_err_msg.x = self.est_error_pose[0]
est_err_msg.y = self.est_error_pose[1]
est_err_msg.z = self.est_error_pose[2]
est_err_msg.roll = self.est_error_pose[3]
est_err_msg.pitch = self.est_error_pose[4]
est_err_msg.yaw = self.est_error_pose[5]
est_err_msg.vx = self.est_error_pose[6]
est_err_msg.vy = self.est_error_pose[7]
est_err_msg.vz = self.est_error_pose[8]
est_err_msg.rmse_x = self.est_rmse[0]
est_err_msg.rmse_y = self.est_rmse[1]
est_err_msg.rmse_z = self.est_rmse[2]
est_err_msg.rmse_roll = self.est_rmse[3]
est_err_msg.rmse_pitch = self.est_rmse[4]
est_err_msg.rmse_yaw = self.est_rmse[5]
est_err_msg.rmse_vx = self.est_rmse[6]
est_err_msg.rmse_vy = self.est_rmse[7]
est_err_msg.rmse_vz = self.est_rmse[8]
est_err_msg.rmse_dist = self.est_dist_error_rmse
est_err_msg.dist_error = self.est_dist_error
# est_err_msg.dist_traveled = self.dist_traveled
self.pub_est_err.publish(est_err_msg)
def PublishDr(self):
# DR error
dr_err_msg = Error()
dr_err_msg.header.stamp = rospy.Time.now()
dr_err_msg.x = self.dr_error_pose[0]
dr_err_msg.y = self.dr_error_pose[1]
dr_err_msg.z = self.dr_error_pose[2]
dr_err_msg.roll = self.dr_error_pose[3]
dr_err_msg.pitch = self.dr_error_pose[4]
dr_err_msg.yaw = self.dr_error_pose[5]
dr_err_msg.vx = self.dr_error_pose[6]
dr_err_msg.vy = self.dr_error_pose[7]
dr_err_msg.vz = self.dr_error_pose[8]
dr_err_msg.rmse_x = self.dr_rmse[0]
dr_err_msg.rmse_y = self.dr_rmse[1]
dr_err_msg.rmse_z = self.dr_rmse[2]
dr_err_msg.rmse_roll = self.dr_rmse[3]
dr_err_msg.rmse_pitch = self.dr_rmse[4]
dr_err_msg.rmse_yaw = self.dr_rmse[5]
dr_err_msg.rmse_vx = self.dr_rmse[6]
dr_err_msg.rmse_vy = self.dr_rmse[7]
dr_err_msg.rmse_vz = self.dr_rmse[8]
dr_err_msg.rmse_dist = self.dr_dist_error_rmse
dr_err_msg.dist_error = self.dr_dist_error
# dr_err_msg.dist_traveled = self.dist_traveled
self.pub_dr_err.publish(dr_err_msg)