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()
# 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 = 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]
# 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 = 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]