def __init__(self):
rospy.loginfo(rospy.get_name() + ": Start")
# Variables
self.update_rate = 5
self.first_odom_topic_received = False
self.odometry_x_prev = 0.0
self.odometry_y_prev = 0.0
self.odometry_yaw_prev = 0.0
self.latest_odo_update = 0.0
self.latest_imu_update = 0.0
self.acc_roll = 0.0
self.acc_pitch = 0.0
# Messages
self.pose_msg = Odometry()
self.quaternion = np.empty((4, ), dtype=np.float64)
# Get parameters
self.pose_msg.header.frame_id = rospy.get_param("~frame_id", "base_link")
self.pose_msg.child_frame_id = rospy.get_param("~child_frame_id", "odom")
robot_max_velocity = float(rospy.get_param("~/robot_max_velocity", 1.0)) # Robot maximum velocity [m/s]
self.marker_processing_delay = rospy.get_param("~marker_processing_delay", 0.0)
self.odometry_var_dist = rospy.get_param("~odometry_distance_variance", 0.000001)
self.odometry_var_yaw = rospy.get_param("~odometry_angular_variance", 0.000001)
# Get topic names
self.odom_topic = rospy.get_param("~odom_sub",'/fmKnowledge/odometry')
self.imu_topic = rospy.get_param("~imu_sub",'/fmInformation/imu')
self.line_pose_topic = rospy.get_param("~line_pose_sub",'/fmInformation/line_pose')
self.lrs_pose_topic = rospy.get_param("~lrs_pose_sub",'/fmInformation/lrs_pose')
self.marker_pose_topic = rospy.get_param("~marker_pose_sub",'/fmInformation/lrs_pose')
self.pose_topic = rospy.get_param("~pose_pub",'/fmKnowledge/pose')
# Setup subscription topic callbacks
rospy.Subscriber(self.odom_topic, Odometry, self.on_odom_topic)
rospy.Subscriber(self.imu_topic, Imu, self.on_imu_topic)
rospy.Subscriber(self.line_pose_topic, Odometry, self.on_absolute_pose)
rospy.Subscriber(self.lrs_pose_topic, Odometry, self.on_absolute_pose)
rospy.Subscriber(self.marker_pose_topic, Odometry, self.on_absolute_pose)
# setup publish topics
self.pose_pub = rospy.Publisher(self.pose_topic, Odometry, queue_size=10)
self.br = tf.TransformBroadcaster()
# initialize estimator (preprocessing)
self.pp = odometry_gnss_pose_preprocessor (robot_max_velocity)
# initialize EKF
self.ekf = odometry_pose_ekf()
self.pose = [0.0, 0.0, 0.0]
self.ekf.initial_guess (self.pose, self.odometry_var_dist, self.odometry_var_yaw)
# Call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
rospy.loginfo(rospy.get_name() + ": Start")
# Variables
self.update_rate = 5
self.first_odom_topic_received = False
self.odometry_x_prev = 0.0
self.odometry_y_prev = 0.0
self.odometry_yaw_prev = 0.0
self.latest_odo_update = 0.0
self.latest_imu_update = 0.0
self.acc_roll = 0.0
self.acc_pitch = 0.0
# Messages
self.pose_msg = Odometry()
self.quaternion = np.empty((4, ), dtype=np.float64)
# Get parameters
self.pose_msg.header.frame_id = rospy.get_param("~frame_id", "base_link")
self.pose_msg.child_frame_id = rospy.get_param("~child_frame_id", "odom")
robot_max_velocity = float(rospy.get_param("~/robot_max_velocity", 1.0)) # Robot maximum velocity [m/s]
self.odometry_var_dist = rospy.get_param("~odometry_distance_variance", 0.000001)
self.odometry_var_yaw = rospy.get_param("~odometry_angular_variance", 0.000001)
# Get topic names
self.odom_topic = rospy.get_param("~odom_sub",'/fmKnowledge/odometry')
self.imu_topic = rospy.get_param("~imu_sub",'/fmInformation/imu')
self.line_pose_topic = rospy.get_param("~line_pose_sub",'/fmInformation/line_pose')
self.lrs_pose_topic = rospy.get_param("~lrs_pose_sub",'/fmInformation/lrs_pose')
self.marker_pose_topic = rospy.get_param("~marker_pose_sub",'/fmInformation/lrs_pose')
self.pose_topic = rospy.get_param("~pose_pub",'/fmKnowledge/pose')
# Setup subscription topic callbacks
rospy.Subscriber(self.odom_topic, Odometry, self.on_odom_topic)
rospy.Subscriber(self.imu_topic, Imu, self.on_imu_topic)
rospy.Subscriber(self.line_pose_topic, Odometry, self.on_absolute_pose)
rospy.Subscriber(self.lrs_pose_topic, Odometry, self.on_absolute_pose)
rospy.Subscriber(self.marker_pose_topic, Odometry, self.on_absolute_pose)
# setup publish topics
self.pose_pub = rospy.Publisher(self.pose_topic, Odometry, queue_size=10)
self.br = tf.TransformBroadcaster()
# initialize estimator (preprocessing)
self.pp = odometry_gnss_pose_preprocessor (robot_max_velocity)
# initialize EKF
self.ekf = odometry_pose_ekf()
self.pose = [0.0, 0.0, 0.0]
self.ekf.initial_guess (self.pose, self.odometry_var_dist, self.odometry_var_yaw)
# Call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
# defines
self.pi2 = 2.0*pi
self.SOL_ODO = 0
self.SOL_SPS = 1
self.SOL_DGPS = 2
self.SOL_FLOAT = 3
self.SOL_RTK = 4
self.ERR_NONE = 0
self.ERR_ODO_TOUT = -1
self.ERR_IMU_TOUT = -2
self.ERR_GNSS_TOUT = -3
self.ERR_ORIENTATION_TOUT = -4
# initialization
rospy.loginfo(rospy.get_name() + ": Start")
self.update_rate = 5
self.pose_msg = Odometry()
self.pose_status_msg = IntArrayStamped()
self.pose_status_msg.data.append (self.SOL_ODO)
self.pose_status_msg.data.append (self.ERR_NONE)
self.quaternion = np.empty((4, ), dtype=np.float64)
self.first_odom_topic_received = False
self.odometry_x_prev = 0.0
self.odometry_y_prev = 0.0
self.odometry_yaw_prev = 0.0
self.estimate_orientation_now = False
self.first_absolute_pos_update = False
self.first_absolute_yaw_update = False
self.latest_odo_update = 0.0
self.latest_gnss_update = 0.0
self.latest_imu_update = 0.0
self.latest_orientation_update = 0.0
self.solution = self.SOL_ODO
self.error = self.ERR_NONE
# Get parameters
self.pose_msg.header.frame_id = rospy.get_param("~frame_id", "base_link")
self.pose_msg.child_frame_id = rospy.get_param("~child_frame_id", "odom")
robot_max_velocity = float(rospy.get_param("~/robot_max_velocity", 1.0)) # Robot maximum velocity [m/s]
self.solution_required = rospy.get_param("~solution_required", self.SOL_ODO)
self.odometry_var_dist = rospy.get_param("~odometry_distance_variance", 0.000001)
self.odometry_var_yaw = rospy.get_param("~odometry_angular_variance", 0.000001)
self.gnss_var_yaw = rospy.get_param("~gnss_angular_variance", 0.0001)
self.odo_timeout = rospy.get_param("~odometry_timeout", 0.5)
self.gnss_timeout = rospy.get_param("~gnss_timeout", 2.0)
self.imu_timeout = rospy.get_param("~imu_timeout", 0.5)
self.absolute_orientation_timeout = rospy.get_param("~absolute_orientation_timeout", 30.0)
# Get topic names
self.odom_topic = rospy.get_param("~odom_sub",'/fmKnowledge/odometry')
self.imu_topic = rospy.get_param("~imu_sub",'/fmInformation/imu')
self.gga_topic = rospy.get_param("~gga_sub",'/fmInformation/gpgga_tranmerc')
self.pose_topic = rospy.get_param("~pose_pub",'/fmKnowledge/pose')
self.pose_status_topic = rospy.get_param("~pose_status_pub",'/fmKnowledge/pose_status')
# Setup subscription topic callbacks
rospy.Subscriber(self.odom_topic, Odometry, self.on_odom_topic)
rospy.Subscriber(self.imu_topic, Imu, self.on_imu_topic)
rospy.Subscriber(self.gga_topic, gpgga_tranmerc, self.on_gga_topic)
# setup publish topics
self.pose_pub = rospy.Publisher(self.pose_topic, Odometry, queue_size = 1)
self.br = tf.TransformBroadcaster()
self.pose_status_pub = rospy.Publisher(self.pose_status_topic, IntArrayStamped, queue_size = 0)
# initialize estimator (preprocessing)
self.pp = odometry_gnss_pose_preprocessor (robot_max_velocity)
self.acc_roll = 0.0
self.acc_pitch = 0.0
# initialize EKF
self.ekf = odometry_pose_ekf()
self.pose = [0.0, 0.0, 0.0]
self.ekf.initial_guess (self.pose, self.odometry_var_dist, self.odometry_var_yaw)
# Call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
# defines
self.pi2 = 2.0*pi
self.SOL_ODO = 0
self.SOL_SPS = 1
self.SOL_DGPS = 2
self.SOL_FLOAT = 3
self.SOL_RTK = 4
self.ERR_NONE = 0
self.ERR_ODO_TOUT = -1
self.ERR_IMU_TOUT = -2
self.ERR_GNSS_TOUT = -3
self.ERR_ORIENTATION_TOUT = -4
# initialization
rospy.loginfo(rospy.get_name() + ": Start")
self.update_rate = 5
self.pose_msg = Odometry()
self.pose_status_msg = IntArrayStamped()
self.pose_status_msg.data.append (self.SOL_ODO)
self.pose_status_msg.data.append (self.ERR_NONE)
self.quaternion = np.empty((4, ), dtype=np.float64)
self.first_odom_topic_received = False
self.odometry_x_prev = 0.0
self.odometry_y_prev = 0.0
self.odometry_yaw_prev = 0.0
self.estimate_orientation_now = False
self.first_absolute_pos_update = False
self.first_absolute_yaw_update = False
self.latest_odo_update = 0.0
self.latest_gnss_update = 0.0
self.latest_imu_update = 0.0
self.latest_orientation_update = 0.0
self.solution = self.SOL_ODO
self.error = self.ERR_NONE
# Get parameters
self.pose_msg.header.frame_id = rospy.get_param("~frame_id", "base_link")
self.pose_msg.child_frame_id = rospy.get_param("~child_frame_id", "odom")
robot_max_velocity = float(rospy.get_param("~/robot_max_velocity", 1.0)) # Robot maximum velocity [m/s]
self.solution_required = rospy.get_param("~solution_required", self.SOL_ODO)
self.odometry_var_dist = rospy.get_param("~odometry_distance_variance", 0.000001)
self.odometry_var_yaw = rospy.get_param("~odometry_angular_variance", 0.000001)
self.gnss_var_yaw = rospy.get_param("~gnss_angular_variance", 0.0001)
self.odo_timeout = rospy.get_param("~odometry_timeout", 0.5)
self.gnss_timeout = rospy.get_param("~gnss_timeout", 2.0)
self.imu_timeout = rospy.get_param("~imu_timeout", 0.5)
self.absolute_orientation_timeout = rospy.get_param("~absolute_orientation_timeout", 30.0)
# Get topic names
self.odom_topic = rospy.get_param("~odom_sub",'/fmKnowledge/odometry')
self.imu_topic = rospy.get_param("~imu_sub",'/fmInformation/imu')
self.gga_topic = rospy.get_param("~gga_sub",'/fmInformation/gpgga_tranmerc')
self.pose_topic = rospy.get_param("~pose_pub",'/fmKnowledge/pose')
self.pose_status_topic = rospy.get_param("~pose_status_pub",'/fmKnowledge/pose_status')
# Setup subscription topic callbacks
rospy.Subscriber(self.odom_topic, Odometry, self.on_odom_topic)
rospy.Subscriber(self.imu_topic, Imu, self.on_imu_topic)
rospy.Subscriber(self.gga_topic, gpgga_tranmerc, self.on_gga_topic)
# setup publish topics
self.pose_pub = rospy.Publisher(self.pose_topic, Odometry)
self.br = tf.TransformBroadcaster()
self.pose_status_pub = rospy.Publisher(self.pose_status_topic, IntArrayStamped)
# initialize estimator (preprocessing)
self.pp = odometry_gnss_pose_preprocessor (robot_max_velocity)
self.acc_roll = 0.0
self.acc_pitch = 0.0
# initialize EKF
self.ekf = odometry_pose_ekf()
self.pose = [0.0, 0.0, 0.0]
self.ekf.initial_guess (self.pose, self.odometry_var_dist, self.odometry_var_yaw)
# Call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
# define simulation time based on odometry data
sim_offset = odo_sim.data[0][0]
sim_len = odo_sim.data[-1][0] - sim_offset
sim_steps = sim_len / sim_step_interval
sim_time = 0
print('Simulation')
print(' Step interval: %.2fs' % sim_step_interval)
print(' Total: %.2fs (%.0f steps)' % (sim_len, sim_steps))
# initialize estimator (preprocessing)
robot_max_speed = 3.0 # [m/s]
pp = odometry_gnss_pose_preprocessor(robot_max_speed)
# initialize estimator (EKF)
prev_odometry = [0.0, 0.0, 0.0, 0.0] # [time,X,Y,theta]
ekf = odometry_pose_ekf()
ekf.initial_guess([ekf_easting_init, ekf_northing_init, 0], var_pos_init,
var_yaw_init)
# run simulation
for step in xrange((int(sim_steps) + 1)):
# simulation time housekeeping
log_time = sim_time + sim_offset
sim_time += sim_step_interval
# update odometry position
(odo_updates, odometry) = odo_sim.get_latest(log_time)
if odo_updates > 0:
# odometry data preprocessing
time_recv = odometry[0]
# define simulation time based on odometry data
sim_offset = odo_sim.data[0][0]
sim_len = odo_sim.data[-1][0] - sim_offset
sim_steps = sim_len/sim_step_interval
sim_time = 0
print ('Simulation')
print (' Step interval: %.2fs' % sim_step_interval)
print (' Total: %.2fs (%.0f steps)' % (sim_len, sim_steps))
# initialize estimator (preprocessing)
robot_max_speed = 3.0 # [m/s]
pp = odometry_gnss_pose_preprocessor (robot_max_speed)
# initialize estimator (EKF)
prev_odometry = [0.0, 0.0, 0.0, 0.0] # [time,X,Y,theta]
ekf = odometry_pose_ekf()
ekf.initial_guess ([ekf_easting_init, ekf_northing_init, 0], var_pos_init, var_yaw_init)
# run simulation
for step in xrange ((int(sim_steps)+1)):
# simulation time housekeeping
log_time = sim_time + sim_offset
sim_time += sim_step_interval
# update odometry position
(odo_updates, odometry) = odo_sim.get_latest(log_time)
if odo_updates > 0:
# odometry data preprocessing
time_recv = odometry[0]
delta_dist = sqrt((odometry[1]-prev_odometry[1])**2 + (odometry[2]-prev_odometry[2])**2)