def __init__(self):
self.pose_msg = Odometry()
self.quaternion = np.empty((4, ), dtype=np.float64)
self.odom_topic_received = False
self.odometry_x_prev = 0.0
self.odometry_y_prev = 0.0
self.odometry_yaw_prev = 0.0
# 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 = rospy.get_param(
"~/robot_max_velocity", "1.0") # Robot maximum velocity [m/s]
ekf_init_guess_easting = rospy.get_param("~ekf_initial_guess_easting",
"0.0")
ekf_init_guess_northing = rospy.get_param(
"~ekf_initial_guess_northing", "0.0")
ekf_init_guess_yaw = rospy.get_param("~ekf_initial_guess_yaw", "0.0")
self.odometry_var_dist = rospy.get_param("~odometry_distance_variance",
"0.0")
self.odometry_var_angle = rospy.get_param("~odometry_angular_variance",
"0.0")
# Get topic names
self.odom_topic = rospy.get_param("~odom_sub",
'/fmKnowledge/encoder_odom')
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')
# 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()
# initialize estimator (preprocessing)
self.pp = pose_2d_preprocessor(robot_max_velocity)
# initialize estimator (EKF)
self.ekf = pose_2d_ekf()
self.pose = [
ekf_init_guess_easting, ekf_init_guess_northing, ekf_init_guess_yaw
]
self.ekf.set_initial_guess(self.pose)
self.yawekf = yaw_ekf() # !!! TEMPORARY HACK
# Call updater function
rospy.loginfo(rospy.get_name() + ": Start")
self.r = rospy.Rate(10) # set updater frequency [Hz]
self.updater()
def __init__(self):
# initialization
rospy.loginfo(rospy.get_name() + ": Start")
self.count = 0
self.pi2 = 2.0*pi
self.update_rate = 5
self.pose_msg = Odometry()
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.time = 0.0
# 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.publish_rel_pose = rospy.get_param("~publish_relative_pose", True)
ekf_init_guess_easting = rospy.get_param("~ekf_initial_guess_easting", 0.0)
ekf_init_guess_northing = rospy.get_param("~ekf_initial_guess_northing", 0.0)
ekf_init_guess_yaw = rospy.get_param("~ekf_initial_guess_yaw", 0.0)
self.odometry_var_dist = rospy.get_param("~odometry_distance_variance", 0.0)
self.odometry_var_angle = rospy.get_param("~odometry_angular_variance", 0.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')
# 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()
# initialize estimator (preprocessing)
self.pp = pose_2d_preprocessor (robot_max_velocity)
# initialize estimator (EKF)
self.pose = [ekf_init_guess_easting, ekf_init_guess_northing, ekf_init_guess_yaw]
self.ekf = pose_2d_ekf(self.pose)
self.yawekf = yaw_ekf() # !!! TEMPORARY HACK
# Call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
self.pose_msg = Odometry()
self.quaternion = np.empty((4, ), dtype=np.float64)
self.odom_topic_received = False
self.odometry_x_prev = 0.0
self.odometry_y_prev = 0.0
self.odometry_yaw_prev = 0.0
self.imu_yaw_prev = 0.0
self.imu_topic_received = False
# Get parametersgpgga_tranmerc.h
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 = rospy.get_param("~/robot_max_velocity", "1.0") # Robot maximum velocity [m/s]
ekf_init_guess_easting = rospy.get_param("~ekf_initial_guess_easting", "0.0")
ekf_init_guess_northing = rospy.get_param("~ekf_initial_guess_northing", "0.0")
ekf_init_guess_yaw = rospy.get_param("~ekf_initial_guess_yaw", "0.0")
self.odometry_var_dist = rospy.get_param("~odometry_distance_variance", "0.0")
self.odometry_var_angle = rospy.get_param("~odometry_angular_variance", "0.0")
self.imu_var_angle = rospy.get_param("~imu_variance","0.0")
# Get topic names
print"imu_var %.3f, odom_var_ang %3f" %(self.imu_var_angle,self.odometry_var_angle)
self.odom_topic = rospy.get_param("~odom_sub",'/fmKnowledge/encoder_odom')
self.imu_topic = rospy.get_param("~imu_sub",'/imu/data')
self.gga_topic = rospy.get_param("~gga_sub",'/fmInformation/gpgga_tranmerc')
self.pose_topic = rospy.get_param("pose_pub",'/fmKnowledge/pose')
self.use_gps_topic=rospy.get_param("~use_gps_topic",'/fmController/use_gps')
print "IMU_SUB=%s" %(self.imu_topic)
print "POSE_SUB=%s" %(self.pose_topic)
# 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)
rospy.Subscriber(self.use_gps_topic, Int16, self.on_use_gps_topic)
# setup publish topics
self.pose_pub = rospy.Publisher(self.pose_topic, Odometry)
self.br = tf.TransformBroadcaster()
# initialize estimator (preprocessing)
self.pp = pose_2d_preprocessor (robot_max_velocity)
# initialize estimator (EKF)
self.use_gps = True;
self.ekf = pose_2d_ekf()
self.pose = [ekf_init_guess_easting, ekf_init_guess_northing, ekf_init_guess_yaw]
self.ekf.set_initial_guess(self.pose)
self.yawekf = yaw_ekf() # !!! TEMPORARY HACK
#self.gnss_initialized = False
# Call updater function
rospy.loginfo(rospy.get_name() + ": Start")
self.r = rospy.Rate(10) # set updater frequency [Hz]
self.updater()
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 = pose_2d_preprocessor (robot_max_speed)
# initialize estimator (EKF)
prev_odometry = [0.0, 0.0, 0.0, 0.0] # [time,X,Y,theta]
ekf = pose_2d_ekf()
ekf.set_initial_guess([ekf_easting_init, ekf_northing_init, 0])
yawekf = yaw_ekf() # !!! TEMPORARY HACK
# 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)
delta_angle = angle_diff (odometry[3], prev_odometry[3])
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 = pose_2d_preprocessor(robot_max_speed)
# initialize estimator (EKF)
prev_odometry = [0.0, 0.0, 0.0, 0.0] # [time,X,Y,theta]
ekf = pose_2d_ekf()
ekf.set_initial_guess([ekf_easting_init, ekf_northing_init, 0])
yawekf = yaw_ekf() # !!! TEMPORARY HACK
# 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)