def __init__(self): rospy.loginfo(rospy.get_name() + ": Start") # defines self.update_rate = 50 # set update frequency [Hz] self.pid_update_interval = 20 # [ms] self.w_STATE_ERR_NO_CONFIG = 3 self.w_STATE_ERR = 3 self.count = 0 # locally defined parameters self.deadman_tout_duration = 0.2 # [s] self.cmd_vel_tout_duration = 0.2 # [s] self.w_tout_duration = 0.2 # [s] # variables self.w_voltage_conv = 5.0*(1800.0 + 700.0)/(700.0*1023.0) #(voltage divider 1800/700 ohm) self.supply_voltage_ok = False self.w_current_conv = 5.0/(1023.0*0.075) # according to simple-h controller datasheet self.wl_stat_curr = 0.0 self.wl_stat_volt = 0.0 self.wl_stat_power = 0.0 self.wr_stat_curr = 0.0 self.wr_stat_volt = 0.0 self.wr_stat_power = 0.0 # get parameters self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 1.0) # [m] self.ticks_per_meter_left = rospy.get_param("/ticks_per_meter_left", 1000) self.ticks_per_meter_right = rospy.get_param("/ticks_per_meter_right", 1000) acc_lin_max = rospy.get_param("~max_linear_acceleration", 1.0) # [m/s^2] acc_ang_max = rospy.get_param("~max_angular_acceleration", 0.1) # [rad/s^2] self.acc_lin_max_step = acc_lin_max/(self.update_rate * 1.0) self.acc_ang_max_step = acc_ang_max/(self.update_rate * 1.0) self.wl_kp = rospy.get_param("~wheel_left_kp", 1.0) self.wl_ki = rospy.get_param("~wheel_left_ki", 0.0) self.wl_kd = rospy.get_param("~wheel_left_kd", 0.0) self.wl_max_integral_output = rospy.get_param("~wheel_left_max_integral_output",0.0) self.wr_kp = rospy.get_param("~wheel_right_kp", 1.0) self.wr_ki = rospy.get_param("~wheel_right_ki", 0.0) self.wr_kd = rospy.get_param("~wheel_right_kd", 0.0) self.wr_max_integral_output = rospy.get_param("~wheel_right_max_integral_output", 0.0) self.min_supply_voltage = rospy.get_param("~min_supply_voltage", 12.0) pub_status_rate = rospy.get_param("~publish_wheel_status_rate", 5) if pub_status_rate != 0: self.pub_status_interval = int(self.update_rate/pub_status_rate) else: self.pub_status_interval = 0 pub_fb_rate = rospy.get_param("~publish_wheel_feedback_rate", 0) if pub_fb_rate != 0: self.pub_fb_interval = int(self.update_rate/pub_fb_rate) else: self.pub_fb_interval = 0 show_volt_interval = rospy.get_param("~show_voltage_interval", 60) if show_volt_interval != 0: self.show_volt_interval = int(show_volt_interval) * self.update_rate else: self.show_volt_interval = 0 #rospy.loginfo (rospy.get_name() + ': Differential wheel distance %.2f' % self.w_dist) #rospy.loginfo (rospy.get_name() + ': Ticks per meter left %d' % self.ticks_per_meter_left) #rospy.loginfo (rospy.get_name() + ': Ticks per meter right %d' % self.ticks_per_meter_right) # instantiate differntial kinemaics class self.dk = differential_kinematics(self.w_dist) # get topic names self.deadman_topic = rospy.get_param("~deadman_sub",'/fmCommand/deadman') self.cmd_vel_topic = rospy.get_param("~cmd_vel_sub",'/fmCommand/cmd_vel') self.enc_left_topic = rospy.get_param("~enc_left_pub",'/fmInformation/enc_left') self.enc_right_topic = rospy.get_param("~enc_right_pub",'/fmInformation/enc_right') self.w_fb_left_pub_topic = rospy.get_param("~wheel_feedback_left_pub",'/fmInformation/wheel_feedback_left') self.w_fb_right_pub_topic = rospy.get_param("~wheel_feedback_right_pub",'/fmInformation/wheel_feedback_right') self.w_stat_left_pub_topic = rospy.get_param("~wheel_status_left_pub",'/fmInformation/wheel_status_left') self.w_stat_right_pub_topic = rospy.get_param("~wheel_status_right_pub",'/fmInformation/wheel_status_right') self.wl_sub_topic = rospy.get_param("~wheel_left_sub",'/fmData/wheel_left_nmea_in') self.wl_pub_topic = rospy.get_param("~wheel_left_pub",'/fmSignal/wheel_left_nmea_out') self.wr_sub_topic = rospy.get_param("~wheel_right_sub",'/fmData/wheel_right_nmea_in') self.wr_pub_topic = rospy.get_param("~wheel_right_pub",'/fmSignal/wheel_right_nmea_out') # setup wheel ctrl topic publisher self.wl_pub = rospy.Publisher(self.wl_pub_topic, nmea) self.wr_pub = rospy.Publisher(self.wr_pub_topic, nmea) self.nmea = nmea() self.nmea.data.append('0') self.nmea.valid = True self.vel_lin_desired = 0.0 self.vel_ang_desired = 0.0 self.vel_lin = 0.0 self.vel_ang = 0.0 # setup encoder topic publisher self.wl_fb_state = 0 self.wr_fb_state = 0 self.enc_left = 0 self.enc_right = 0 self.enc_left_pub = rospy.Publisher(self.enc_left_topic, IntStamped) self.enc_right_pub = rospy.Publisher(self.enc_right_topic, IntStamped) self.intstamp = IntStamped() # setup wheel status topic publisher if self.pub_status_interval > 0: self.w_stat_left_pub = rospy.Publisher(self.w_stat_left_pub_topic, PropulsionModuleStatus) self.w_stat_right_pub = rospy.Publisher(self.w_stat_right_pub_topic, PropulsionModuleStatus) self.w_stat = PropulsionModuleStatus() # setup wheel feedback topic publisher if self.pub_fb_interval > 0: self.wl_fb_vel = 0.0 self.wl_fb_vel_set = 0.0 self.wl_fb_thrust = 0.0 self.wr_fb_vel = 0.0 self.wr_fb_vel_set = 0.0 self.wr_fb_thrust = 0.0 self.w_fb_left_pub = rospy.Publisher(self.w_fb_left_pub_topic, PropulsionModuleFeedback) self.w_fb_right_pub = rospy.Publisher(self.w_fb_right_pub_topic, PropulsionModuleFeedback) self.w_fb = PropulsionModuleFeedback() # setup subscription topic callbacks self.deadman_tout = 0 rospy.Subscriber(self.deadman_topic, Bool, self.on_deadman_message) self.cmd_vel_tout = 0 self.cmd_vel_tout_active = True self.wl_tout = 0 self.wl_tout_active = True self.wr_tout = 0 self.wr_tout_active = True rospy.Subscriber(self.cmd_vel_topic, TwistStamped, self.on_cmd_vel_message) rospy.Subscriber(self.wl_sub_topic, nmea, self.on_wheel_left_message) rospy.Subscriber(self.wr_sub_topic, nmea, self.on_wheel_right_message) # call updater function self.r = rospy.Rate(self.update_rate) self.updater()
def __init__(self): rospy.loginfo(rospy.get_name() + ": Start") # defines self.update_rate = 50 # set update frequency [Hz] self.pid_update_interval = 50 # [ms] self.pfbst_update_interval = 20 # [ms] self.STATE_OK = 1 self.STATE_WARN_NMEA_CS = 2 self.STATE_ERR_NO_CONFIG = 3 self.STATE_ERR_WATCHDOG = 4 self.STATE_ERR_LOWBAT = 5 self.STATE_ERR = 3 self.count = 0 self.MOTION_DRIVE = 0 self.MOTION_TURN = 1 self.motion = self.MOTION_DRIVE # locally defined parameters self.deadman_tout_duration = 0.2 # [s] self.cmd_vel_tout_duration = 0.2 # [s] self.frobit_tout_duration = 0.2 # [s] # variables self.frobit_state = 0 self.frobit_state_prev = -1 self.frobit_voltage_conv = 5.0*(1800.0 + 700.0)/(700.0*1023.0) #(voltage divider 1800/700 ohm) self.frobit_voltage_ok = False self.frobit_volt = 0.0 self.send_cfg_tout = 0.0 # get parameters self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 1.0) # [m] self.ticks_per_meter_left = rospy.get_param("/ticks_per_meter_left", 1000) self.ticks_per_meter_right = rospy.get_param("/ticks_per_meter_right", 1000) self.castor_front = rospy.get_param("~castor_front", "false") acc_lin_max = rospy.get_param("~max_linear_acceleration", 1.0) # [m/s^2] acc_ang_max = rospy.get_param("~max_angular_acceleration", 0.1) # [rad/s^2] self.acc_lin_max_step = acc_lin_max/(self.update_rate * 1.0) self.acc_ang_max_step = acc_ang_max/(self.update_rate * 1.0) self.w_drive_ff = rospy.get_param("~wheel_drive_feed_forward", 1.0) self.w_drive_kp = rospy.get_param("~wheel_drive_kp", 1.0) self.w_drive_ki = rospy.get_param("~wheel_drive_ki", 0.0) self.w_drive_kd = rospy.get_param("~wheel_drive_kd", 0.0) self.w_drive_max_int_output = rospy.get_param("~wheel_drive_max_integral_output", 0.0) self.w_turn_ff = rospy.get_param("~wheel_turn_feed_forward", 1.0) self.w_turn_kp = rospy.get_param("~wheel_turn_kp", 1.0) self.w_turn_ki = rospy.get_param("~wheel_turn_ki", 0.0) self.w_turn_kd = rospy.get_param("~wheel_turn_kd", 0.0) self.w_turn_max_int_output = rospy.get_param("~wheel_turn_max_integral_output", 0.0) self.min_supply_voltage = rospy.get_param("~min_supply_voltage", 12.0) pub_status_rate = rospy.get_param("~publish_wheel_status_rate", 5) if pub_status_rate != 0: self.pub_status_interval = int(self.update_rate/pub_status_rate) else: self.pub_status_interval = 0 pub_fb_rate = rospy.get_param("~publish_wheel_feedback_rate", 0) if pub_fb_rate != 0: self.pub_fb_interval = int(self.update_rate/pub_fb_rate) else: self.pub_fb_interval = 0 pub_pid_rate = rospy.get_param("~publish_wheel_pid_rate", 0) if pub_pid_rate != 0: self.pub_pid_interval = int(self.update_rate/pub_pid_rate) else: self.pub_pid_interval = 0 show_volt_interval = rospy.get_param("~show_voltage_interval", 60) if show_volt_interval != 0: self.show_volt_interval = int(show_volt_interval) * self.update_rate else: self.show_volt_interval = 0 # instantiate differntial kinemaics class self.dk = differential_kinematics(self.w_dist) # get topic names self.deadman_topic = rospy.get_param("~deadman_sub",'/fmCommand/deadman') self.cmd_vel_topic = rospy.get_param("~cmd_vel_sub",'/fmCommand/cmd_vel') self.enc_l_topic = rospy.get_param("~enc_left_pub",'/fmInformation/enc_left') self.enc_r_topic = rospy.get_param("~enc_right_pub",'/fmInformation/enc_right') self.w_fb_left_pub_topic = rospy.get_param("~wheel_feedback_left_pub",'/fmInformation/wheel_feedback_left') self.w_fb_right_pub_topic = rospy.get_param("~wheel_feedback_right_pub",'/fmInformation/wheel_feedback_right') self.w_stat_left_pub_topic = rospy.get_param("~wheel_status_left_pub",'/fmInformation/wheel_status_left') self.w_stat_right_pub_topic = rospy.get_param("~wheel_status_right_pub",'/fmInformation/wheel_status_right') self.w_pid_left_pub_topic = rospy.get_param("~wheel_pid_left_pub",'/fmInformation/wheel_pid_left') self.w_pid_right_pub_topic = rospy.get_param("~wheel_pid_right_pub",'/fmInformation/wheel_pid_right') self.frobit_sub_topic = rospy.get_param("~nmea_from_frobit_sub",'/fmSignal/nmea_from_frobit') self.frobit_pub_topic = rospy.get_param("~nmea_to_frobit_pub",'/fmSignal/nmea_to_frobit') # setup frobit NMEA topic publisher self.frobit_pub = rospy.Publisher(self.frobit_pub_topic, nmea) # setup NMEA $PFBCT topic publisher self.nmea_pfbct = nmea() self.nmea_pfbct.type = 'PFBCT' self.nmea_pfbct.length = 2 self.nmea_pfbct.valid = True self.nmea_pfbct.data.append('0') self.nmea_pfbct.data.append('0') self.vel_lin_desired = 0.0 self.vel_ang_desired = 0.0 self.vel_lin = 0.0 self.vel_ang = 0.0 # setup encoder topic publisher self.w_fb_state = 0 self.enc_l = 0 self.enc_r = 0 self.enc_l_buf = [0]*5 self.enc_r_buf = [0]*5 self.enc_l_pub = rospy.Publisher(self.enc_l_topic, IntStamped) self.enc_r_pub = rospy.Publisher(self.enc_r_topic, IntStamped) self.intstamp = IntStamped() # setup wheel status topic publisher if self.pub_status_interval > 0: self.w_stat_left_pub = rospy.Publisher(self.w_stat_left_pub_topic, PropulsionModuleStatus) self.w_stat_right_pub = rospy.Publisher(self.w_stat_right_pub_topic, PropulsionModuleStatus) self.w_stat = PropulsionModuleStatus() # setup wheel feedback topic publisher if self.pub_fb_interval > 0: self.wl_fb_vel = 0.0 self.wl_fb_vel_set = 0.0 self.wl_fb_thrust = 0.0 self.wr_fb_vel = 0.0 self.wr_fb_vel_set = 0.0 self.wr_fb_thrust = 0.0 self.w_fb_left_pub = rospy.Publisher(self.w_fb_left_pub_topic, PropulsionModuleFeedback) self.w_fb_right_pub = rospy.Publisher(self.w_fb_right_pub_topic, PropulsionModuleFeedback) self.w_fb = PropulsionModuleFeedback() self.tick_to_vel_factor = 1000.0/(1.0*self.ticks_per_meter_left*self.pfbst_update_interval) # setup wheel pid topic publisher if self.pub_pid_interval > 0: self.wl_pid_err = 0.0 self.wl_pid_out = 0.0 self.wl_pid_p = 0.0 self.wl_pid_i = 0.0 self.wl_pid_d = 0.0 self.wr_pid_err = 0.0 self.wr_pid_out = 0.0 self.wr_pid_p = 0.0 self.wr_pid_i = 0.0 self.wr_pid_d = 0.0 self.w_pid_left_pub = rospy.Publisher(self.w_pid_left_pub_topic, FloatArrayStamped) self.w_pid_right_pub = rospy.Publisher(self.w_pid_right_pub_topic, FloatArrayStamped) self.w_pid = FloatArrayStamped() self.nmea_pid = nmea() self.nmea_pid.type = 'PFBWP' self.nmea_pid.length = 7 self.nmea_pid.valid = True self.nmea_pid.data.append('1') # enable closed loop PID self.nmea_pid.data.append(str(self.pid_update_interval)) self.nmea_pid.data.append('0') self.nmea_pid.data.append('0') self.nmea_pid.data.append('0') self.nmea_pid.data.append('0') self.nmea_pid.data.append('0') # setup subscription topic callbacks self.deadman_tout = 0 rospy.Subscriber(self.deadman_topic, Bool, self.on_deadman_message) self.cmd_vel_tout = 0 self.cmd_vel_tout_active = True self.frobit_tout = 0 self.frobit_tout_active = True rospy.Subscriber(self.cmd_vel_topic, TwistStamped, self.on_cmd_vel_message) rospy.Subscriber(self.frobit_sub_topic, nmea, self.on_nmea_from_frobit) # call updater function self.r = rospy.Rate(self.update_rate) self.updater()
def __init__(self): rospy.loginfo(rospy.get_name() + ": Start") # defines self.count = 0 # static parameters self.update_rate = 50 # set update frequency [Hz] self.deadman_tout_duration = 0.2 # [s] self.cmd_vel_tout_duration = 0.2 # [s] # get parameters self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2) # [m] self.ticks_per_meter_left = rospy.get_param("/ticks_per_meter_left", 500) self.ticks_per_meter_right = rospy.get_param("/ticks_per_meter_right", 500) acc_lin_max = rospy.get_param("~max_linear_acceleration", 1.0) # [m/s^2] acc_ang_max = rospy.get_param("~max_angular_acceleration", 1.8) # [rad/s^2] self.wheel_speed_variance = 0.001 self.wheel_speed_delay = 0.05 # [s] self.wheel_speed_delay_variance = 0.05 self.wheel_speed_error = 0.03 # [m/s] self.wheel_speed_minimum = 0.02 pub_fb_rate = rospy.get_param("~publish_wheel_feedback_rate", 0) if pub_fb_rate != 0: self.pub_fb_interval = int(self.update_rate / pub_fb_rate) else: self.pub_fb_interval = 0 # get topic names self.topic_deadman = rospy.get_param("~deadman_sub", '/fmCommand/deadman') self.topic_cmd_vel = rospy.get_param("~cmd_vel_sub", '/fmCommand/cmd_vel') self.topic_odom_reset = rospy.get_param("~odom_reset_sub", '/fmInformation/odom_reset') self.topic_pose = rospy.get_param("~pose_pub", '/fmKnowledge/pose') self.topic_w_fb_left_pub = rospy.get_param( "~wheel_feedback_left_pub", '/fmInformation/wheel_feedback_left') self.topic_w_fb_right_pub = rospy.get_param( "~wheel_feedback_right_pub", '/fmInformation/wheel_feedback_right') # initialize internal variables self.update_interval = 1 / (self.update_rate * 1.0) self.pi2 = 2 * pi self.cmd_vel_tout_active = True self.pose = [0.0, 0.0, 0.0] self.deadman_tout = 0.0 self.cmd_vel_msgs = [] self.vel_lin_desired = 0.0 self.vel_ang_desired = 0.0 self.acc_lin_max_step = acc_lin_max * self.update_interval self.acc_ang_max_step = acc_ang_max * self.update_interval self.vel_lin = 0.0 self.vel_ang = 0.0 self.ref_vel_left = 0.0 self.ref_vel_right = 0.0 self.sim_vel_left = 0.0 self.sim_vel_right = 0.0 self.wheel_speed_err_left = 0.0 self.wheel_speed_err_right = 0.0 self.dk = differential_kinematics(self.w_dist) # setup topic publishers self.pose_pub = rospy.Publisher(self.topic_pose, Odometry) self.pose_msg = Odometry() # setup wheel feedback topic publisher if self.pub_fb_interval > 0: self.wl_fb_vel_set = 0.0 self.wr_fb_vel_set = 0.0 self.w_fb_left_pub = rospy.Publisher(self.topic_w_fb_left_pub, PropulsionModuleFeedback) self.w_fb_right_pub = rospy.Publisher(self.topic_w_fb_right_pub, PropulsionModuleFeedback) self.w_fb = PropulsionModuleFeedback() # setup subscription topic callbacks rospy.Subscriber(self.topic_deadman, Bool, self.on_deadman_message) rospy.Subscriber(self.topic_cmd_vel, TwistStamped, self.on_cmd_vel_message) rospy.Subscriber(self.topic_odom_reset, FloatArrayStamped, self.on_odom_reset_message) # call updater function self.r = rospy.Rate(self.update_rate) self.updater()
def __init__(self): rospy.loginfo(rospy.get_name() + ": Start") # defines self.update_rate = 50 # set update frequency [Hz] self.pid_update_interval = 20 # [ms] self.w_STATE_ERR_NO_CONFIG = 3 self.w_STATE_ERR = 3 self.count = 0 # locally defined parameters self.actuation_enable_tout_duration = 0.2 # [s] self.cmd_vel_tout_duration = 0.2 # [s] self.w_tout_duration = 0.2 # [s] # variables self.w_voltage_conv = 5.0 * (1800.0 + 700.0) / ( 700.0 * 1023.0) #(voltage divider 1800/700 ohm) self.supply_voltage_ok = False self.w_current_conv = 5.0 / ( 1023.0 * 0.075) # according to simple-h controller datasheet self.wl_stat_curr = 0.0 self.wl_stat_volt = 0.0 self.wl_stat_power = 0.0 self.wr_stat_curr = 0.0 self.wr_stat_volt = 0.0 self.wr_stat_power = 0.0 # get parameters self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 1.0) # [m] self.ticks_per_meter_left = rospy.get_param("/ticks_per_meter_left", 1000) self.ticks_per_meter_right = rospy.get_param("/ticks_per_meter_right", 1000) acc_lin_max = rospy.get_param("~max_linear_acceleration", 1.0) # [m/s^2] acc_ang_max = rospy.get_param("~max_angular_acceleration", 0.1) # [rad/s^2] self.acc_lin_max_step = acc_lin_max / (self.update_rate * 1.0) self.acc_ang_max_step = acc_ang_max / (self.update_rate * 1.0) self.wl_kp = rospy.get_param("~wheel_left_kp", 1.0) self.wl_ki = rospy.get_param("~wheel_left_ki", 0.0) self.wl_kd = rospy.get_param("~wheel_left_kd", 0.0) self.wl_max_integral_output = rospy.get_param( "~wheel_left_max_integral_output", 0.0) self.wr_kp = rospy.get_param("~wheel_right_kp", 1.0) self.wr_ki = rospy.get_param("~wheel_right_ki", 0.0) self.wr_kd = rospy.get_param("~wheel_right_kd", 0.0) self.wr_max_integral_output = rospy.get_param( "~wheel_right_max_integral_output", 0.0) self.min_supply_voltage = rospy.get_param("~min_supply_voltage", 12.0) pub_status_rate = rospy.get_param("~publish_wheel_status_rate", 5) if pub_status_rate != 0: self.pub_status_interval = int(self.update_rate / pub_status_rate) else: self.pub_status_interval = 0 pub_fb_rate = rospy.get_param("~publish_wheel_feedback_rate", 0) if pub_fb_rate != 0: self.pub_fb_interval = int(self.update_rate / pub_fb_rate) else: self.pub_fb_interval = 0 show_volt_interval = rospy.get_param("~show_voltage_interval", 60) if show_volt_interval != 0: self.show_volt_interval = int( show_volt_interval) * self.update_rate else: self.show_volt_interval = 0 #rospy.loginfo (rospy.get_name() + ': Differential wheel distance %.2f' % self.w_dist) #rospy.loginfo (rospy.get_name() + ': Ticks per meter left %d' % self.ticks_per_meter_left) #rospy.loginfo (rospy.get_name() + ': Ticks per meter right %d' % self.ticks_per_meter_right) # instantiate differntial kinemaics class self.dk = differential_kinematics(self.w_dist) # get topic names self.actuation_enable_topic = rospy.get_param( "~actuation_enable_sub", '/fmCommand/actuation_enable') self.cmd_vel_topic = rospy.get_param("~cmd_vel_sub", '/fmCommand/cmd_vel') self.enc_left_topic = rospy.get_param("~enc_left_pub", '/fmInformation/enc_left') self.enc_right_topic = rospy.get_param("~enc_right_pub", '/fmInformation/enc_right') self.w_fb_left_pub_topic = rospy.get_param( "~wheel_feedback_left_pub", '/fmInformation/wheel_feedback_left') self.w_fb_right_pub_topic = rospy.get_param( "~wheel_feedback_right_pub", '/fmInformation/wheel_feedback_right') self.w_stat_left_pub_topic = rospy.get_param( "~wheel_status_left_pub", '/fmInformation/wheel_status_left') self.w_stat_right_pub_topic = rospy.get_param( "~wheel_status_right_pub", '/fmInformation/wheel_status_right') self.wl_sub_topic = rospy.get_param("~wheel_left_sub", '/fmData/wheel_left_nmea_in') self.wl_pub_topic = rospy.get_param("~wheel_left_pub", '/fmSignal/wheel_left_nmea_out') self.wr_sub_topic = rospy.get_param("~wheel_right_sub", '/fmData/wheel_right_nmea_in') self.wr_pub_topic = rospy.get_param("~wheel_right_pub", '/fmSignal/wheel_right_nmea_out') # setup wheel ctrl topic publisher self.wl_pub = rospy.Publisher(self.wl_pub_topic, nmea, queue_size=0) self.wr_pub = rospy.Publisher(self.wr_pub_topic, nmea, queue_size=0) self.nmea = nmea() self.nmea.data.append('0') self.nmea.valid = True self.vel_lin_desired = 0.0 self.vel_ang_desired = 0.0 self.vel_lin = 0.0 self.vel_ang = 0.0 # setup encoder topic publisher self.wl_fb_state = 0 self.wr_fb_state = 0 self.enc_left = 0 self.enc_right = 0 self.enc_left_pub = rospy.Publisher(self.enc_left_topic, IntStamped, queue_size=10) self.enc_right_pub = rospy.Publisher(self.enc_right_topic, IntStamped, queue_size=10) self.intstamp = IntStamped() # setup wheel status topic publisher if self.pub_status_interval > 0: self.w_stat_left_pub = rospy.Publisher(self.w_stat_left_pub_topic, PropulsionModuleStatus, queue_size=5) self.w_stat_right_pub = rospy.Publisher( self.w_stat_right_pub_topic, PropulsionModuleStatus, queue_size=5) self.w_stat = PropulsionModuleStatus() # setup wheel feedback topic publisher if self.pub_fb_interval > 0: self.wl_fb_vel = 0.0 self.wl_fb_vel_set = 0.0 self.wl_fb_thrust = 0.0 self.wr_fb_vel = 0.0 self.wr_fb_vel_set = 0.0 self.wr_fb_thrust = 0.0 self.w_fb_left_pub = rospy.Publisher(self.w_fb_left_pub_topic, PropulsionModuleFeedback, queue_size=5) self.w_fb_right_pub = rospy.Publisher(self.w_fb_right_pub_topic, PropulsionModuleFeedback, queue_size=5) self.w_fb = PropulsionModuleFeedback() # setup subscription topic callbacks self.actuation_enable_tout = 0 rospy.Subscriber(self.actuation_enable_topic, BoolStamped, self.on_actuation_enable_message) self.cmd_vel_tout = 0 self.cmd_vel_tout_active = True self.wl_tout = 0 self.wl_tout_active = True self.wr_tout = 0 self.wr_tout_active = True rospy.Subscriber(self.cmd_vel_topic, TwistStamped, self.on_cmd_vel_message) rospy.Subscriber(self.wl_sub_topic, nmea, self.on_wheel_left_message) rospy.Subscriber(self.wr_sub_topic, nmea, self.on_wheel_right_message) # call updater function self.r = rospy.Rate(self.update_rate) self.updater()
def __init__(self): rospy.loginfo(rospy.get_name() + ": Start") # defines self.update_rate = 50 # set update frequency [Hz] self.pid_update_interval = 50 # [ms] self.pfbst_update_interval = 20 # [ms] self.STATE_OK = 1 self.STATE_WARN_NMEA_CS = 2 self.STATE_ERR_NO_CONFIG = 3 self.STATE_ERR_WATCHDOG = 4 self.STATE_ERR_LOWBAT = 5 self.STATE_ERR = 3 self.count = 0 self.MOTION_DRIVE = 0 self.MOTION_TURN = 1 self.motion = self.MOTION_DRIVE # locally defined parameters self.actuation_enable_tout_duration = 0.2 # [s] self.cmd_vel_tout_duration = 0.2 # [s] self.frobit_tout_duration = 0.2 # [s] # variables self.frobit_state = 0 self.frobit_state_prev = -1 self.robocard_voltage_divider = 0.03747 # 5.0*(22000 + 3300)/(3300*1023) because of 22000/3300 ohm v-divider self.frobit_voltage_ok = False self.frobit_volt = 0.0 self.send_cfg_tout = 0.0 # get parameters self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 1.0) # [m] self.ticks_per_meter_left = rospy.get_param("/ticks_per_meter_left", 1000) self.ticks_per_meter_right = rospy.get_param("/ticks_per_meter_right", 1000) self.castor_front = rospy.get_param("~castor_front", "false") acc_lin_max = rospy.get_param("~max_linear_acceleration", 1.0) # [m/s^2] acc_ang_max = rospy.get_param("~max_angular_acceleration", 0.1) # [rad/s^2] self.acc_lin_max_step = acc_lin_max / (self.update_rate * 1.0) self.acc_ang_max_step = acc_ang_max / (self.update_rate * 1.0) self.w_drive_ff = rospy.get_param("~wheel_drive_feed_forward", 1.0) self.w_drive_kp = rospy.get_param("~wheel_drive_kp", 1.0) self.w_drive_ki = rospy.get_param("~wheel_drive_ki", 0.0) self.w_drive_kd = rospy.get_param("~wheel_drive_kd", 0.0) self.w_drive_max_int_output = rospy.get_param( "~wheel_drive_max_integral_output", 0.0) self.w_turn_ff = rospy.get_param("~wheel_turn_feed_forward", 1.0) self.w_turn_kp = rospy.get_param("~wheel_turn_kp", 1.0) self.w_turn_ki = rospy.get_param("~wheel_turn_ki", 0.0) self.w_turn_kd = rospy.get_param("~wheel_turn_kd", 0.0) self.w_turn_max_int_output = rospy.get_param( "~wheel_turn_max_integral_output", 0.0) self.supply_voltage_scale_factor = rospy.get_param( "~supply_voltage_scale_factor", self.robocard_voltage_divider) self.min_supply_voltage = rospy.get_param("~min_supply_voltage", 12.0) pub_status_rate = rospy.get_param("~publish_wheel_status_rate", 5) if pub_status_rate != 0: self.pub_status_interval = int(self.update_rate / pub_status_rate) else: self.pub_status_interval = 0 pub_fb_rate = rospy.get_param("~publish_wheel_feedback_rate", 0) if pub_fb_rate != 0: self.pub_fb_interval = int(self.update_rate / pub_fb_rate) else: self.pub_fb_interval = 0 pub_pid_rate = rospy.get_param("~publish_wheel_pid_rate", 0) if pub_pid_rate != 0: self.pub_pid_interval = int(self.update_rate / pub_pid_rate) else: self.pub_pid_interval = 0 show_volt_interval = rospy.get_param("~show_voltage_interval", 60) if show_volt_interval != 0: self.show_volt_interval = int( show_volt_interval) * self.update_rate else: self.show_volt_interval = 0 # instantiate differntial kinemaics class self.dk = differential_kinematics(self.w_dist) # get topic names self.actuation_enable_topic = rospy.get_param( "~actuation_enable_sub", '/fmCommand/actuation_enable') self.cmd_vel_topic = rospy.get_param("~cmd_vel_sub", '/fmCommand/cmd_vel') self.enc_l_topic = rospy.get_param("~enc_left_pub", '/fmInformation/enc_left') self.enc_r_topic = rospy.get_param("~enc_right_pub", '/fmInformation/enc_right') self.w_fb_left_pub_topic = rospy.get_param( "~wheel_feedback_left_pub", '/fmInformation/wheel_feedback_left') self.w_fb_right_pub_topic = rospy.get_param( "~wheel_feedback_right_pub", '/fmInformation/wheel_feedback_right') self.w_stat_left_pub_topic = rospy.get_param( "~wheel_status_left_pub", '/fmInformation/wheel_status_left') self.w_stat_right_pub_topic = rospy.get_param( "~wheel_status_right_pub", '/fmInformation/wheel_status_right') self.w_pid_left_pub_topic = rospy.get_param( "~wheel_pid_left_pub", '/fmInformation/wheel_pid_left') self.w_pid_right_pub_topic = rospy.get_param( "~wheel_pid_right_pub", '/fmInformation/wheel_pid_right') self.frobit_sub_topic = rospy.get_param("~nmea_from_frobit_sub", '/fmSignal/nmea_from_frobit') self.frobit_pub_topic = rospy.get_param("~nmea_to_frobit_pub", '/fmSignal/nmea_to_frobit') # setup frobit NMEA topic publisher self.frobit_pub = rospy.Publisher(self.frobit_pub_topic, nmea, queue_size=50) # setup NMEA $PFBCT topic publisher self.nmea_pfbct = nmea() self.nmea_pfbct.type = 'PFBCT' self.nmea_pfbct.length = 2 self.nmea_pfbct.valid = True self.nmea_pfbct.data.append('0') self.nmea_pfbct.data.append('0') self.vel_lin_desired = 0.0 self.vel_ang_desired = 0.0 self.vel_lin = 0.0 self.vel_ang = 0.0 # setup encoder topic publisher self.w_fb_state = 0 self.enc_l = 0 self.enc_r = 0 self.enc_l_buf = [0] * 5 self.enc_r_buf = [0] * 5 self.enc_l_pub = rospy.Publisher(self.enc_l_topic, IntStamped, queue_size=50) self.enc_r_pub = rospy.Publisher(self.enc_r_topic, IntStamped, queue_size=50) self.intstamp = IntStamped() # setup wheel status topic publisher if self.pub_status_interval > 0: self.w_stat_left_pub = rospy.Publisher(self.w_stat_left_pub_topic, PropulsionModuleStatus, queue_size=10) self.w_stat_right_pub = rospy.Publisher( self.w_stat_right_pub_topic, PropulsionModuleStatus, queue_size=10) self.w_stat = PropulsionModuleStatus() # setup wheel feedback topic publisher if self.pub_fb_interval > 0: self.wl_fb_vel = 0.0 self.wl_fb_vel_set = 0.0 self.wl_fb_thrust = 0.0 self.wr_fb_vel = 0.0 self.wr_fb_vel_set = 0.0 self.wr_fb_thrust = 0.0 self.w_fb_left_pub = rospy.Publisher(self.w_fb_left_pub_topic, PropulsionModuleFeedback, queue_size=10) self.w_fb_right_pub = rospy.Publisher(self.w_fb_right_pub_topic, PropulsionModuleFeedback, queue_size=10) self.w_fb = PropulsionModuleFeedback() self.tick_to_vel_factor = 1000.0 / ( 1.0 * self.ticks_per_meter_left * self.pfbst_update_interval) # setup wheel pid topic publisher if self.pub_pid_interval > 0: self.wl_pid_err = 0.0 self.wl_pid_out = 0.0 self.wl_pid_p = 0.0 self.wl_pid_i = 0.0 self.wl_pid_d = 0.0 self.wr_pid_err = 0.0 self.wr_pid_out = 0.0 self.wr_pid_p = 0.0 self.wr_pid_i = 0.0 self.wr_pid_d = 0.0 self.w_pid_left_pub = rospy.Publisher(self.w_pid_left_pub_topic, FloatArrayStamped, queue_size=10) self.w_pid_right_pub = rospy.Publisher(self.w_pid_right_pub_topic, FloatArrayStamped, queue_size=10) self.w_pid = FloatArrayStamped() self.nmea_pid = nmea() self.nmea_pid.type = 'PFBWP' self.nmea_pid.length = 7 self.nmea_pid.valid = True self.nmea_pid.data.append('1') # enable closed loop PID self.nmea_pid.data.append(str(self.pid_update_interval)) self.nmea_pid.data.append('0') self.nmea_pid.data.append('0') self.nmea_pid.data.append('0') self.nmea_pid.data.append('0') self.nmea_pid.data.append('0') # setup subscription topic callbacks self.actuation_enable_tout = 0 rospy.Subscriber(self.actuation_enable_topic, BoolStamped, self.on_actuation_enable_message) self.cmd_vel_tout = 0 self.cmd_vel_tout_active = True self.frobit_tout = 0 self.frobit_tout_active = True rospy.Subscriber(self.cmd_vel_topic, TwistStamped, self.on_cmd_vel_message) rospy.Subscriber(self.frobit_sub_topic, nmea, self.on_nmea_from_frobit) # call updater function self.r = rospy.Rate(self.update_rate) self.updater()
def __init__(self): rospy.loginfo(rospy.get_name() + ": Start") # defines self.update_rate = 50 # set update frequency [Hz] self.pid_update_interval = 20 # [ms] self.w_STATE_ERR_NO_CONFIG = 3 self.w_STATE_ERR = 3 self.count = 0 # locally defined parameters self.deadman_tout_duration = 0.2 # [s] self.cmd_vel_tout_duration = 0.2 # [s] self.w_tout_duration = 0.2 # [s] # get parameters self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 1.0) # [m] self.ticks_per_meter_left = rospy.get_param("/ticks_per_meter_left", 1000) self.ticks_per_meter_right = rospy.get_param("/ticks_per_meter_right", 1000) acc_lin_max = rospy.get_param("~max_linear_acceleration", 1.0) # [m/s^2] acc_ang_max = rospy.get_param("~max_angular_acceleration", 0.1) # [rad/s^2] self.acc_lin_max_step = acc_lin_max / (self.update_rate * 1.0) self.acc_ang_max_step = acc_ang_max / (self.update_rate * 1.0) self.wl_kp = rospy.get_param("~wheel_left_kp", 1.0) self.wl_ki = rospy.get_param("~wheel_left_ki", 0.0) self.wl_kd = rospy.get_param("~wheel_left_kd", 0.0) self.wl_max_integral_output = rospy.get_param( "~wheel_left_max_integral_output", 0.0) self.wr_kp = rospy.get_param("~wheel_right_kp", 1.0) self.wr_ki = rospy.get_param("~wheel_right_ki", 0.0) self.wr_kd = rospy.get_param("~wheel_right_kd", 0.0) self.wr_max_integral_output = rospy.get_param( "~wheel_right_max_integral_output", 0.0) self.min_supply_voltage = rospy.get_param("~min_supply_voltage", 12.0) pub_status_rate = rospy.get_param("~publish_wheel_status_rate", 5) if pub_status_rate != 0: self.pub_status_interval = int(self.update_rate / pub_status_rate) else: self.pub_status_interval = 0 pub_fb_rate = rospy.get_param("~publish_wheel_feedback_rate", 0) if pub_fb_rate != 0: self.pub_fb_interval = int(self.update_rate / pub_fb_rate) else: self.pub_fb_interval = 0 # get topic names self.deadman_topic = rospy.get_param("~deadman_sub", '/fmCommand/deadman') self.cmd_vel_topic = rospy.get_param("~cmd_vel_sub", '/fmCommand/cmd_vel') self.enc_left_topic = rospy.get_param("~enc_left_pub", '/fmInformation/enc_left') self.enc_right_topic = rospy.get_param("~enc_right_pub", '/fmInformation/enc_right') self.w_fb_left_pub_topic = rospy.get_param( "~wheel_feedback_left_pub", '/fmInformation/wheel_feedback_left') self.w_fb_right_pub_topic = rospy.get_param( "~wheel_feedback_right_pub", '/fmInformation/wheel_feedback_right') self.w_stat_left_pub_topic = rospy.get_param( "~wheel_status_left_pub", '/fmInformation/wheel_status_left') self.w_stat_right_pub_topic = rospy.get_param( "~wheel_status_right_pub", '/fmInformation/wheel_status_right') wl_sub_topic = rospy.get_param("~wheel_left_sub", '/fmData/wheel_left_serial_in') wl_pub_topic = rospy.get_param("~wheel_left_pub", '/fmSignal/wheel_left_serial_out') wr_sub_topic = rospy.get_param("~wheel_right_sub", '/fmData/wheel_right_serial_in') wr_pub_topic = rospy.get_param("~wheel_right_pub", '/fmSignal/wheel_right_serial_out') # instantiate wheel interface classes self.wl = wheel_interface('L: ', wl_pub_topic, wl_sub_topic) self.wr = wheel_interface('R: ', wr_pub_topic, wr_sub_topic) # instantiate differntial kinematics class self.dk = differential_kinematics(self.w_dist) # setup encoder topic publisher self.enc_left_pub = rospy.Publisher(self.enc_left_topic, IntStamped) self.enc_right_pub = rospy.Publisher(self.enc_right_topic, IntStamped) self.intstamp = IntStamped() # setup wheel status topic publisher if self.pub_status_interval > 0: self.w_stat_left_pub = rospy.Publisher(self.w_stat_left_pub_topic, PropulsionModuleStatus) self.w_stat_right_pub = rospy.Publisher( self.w_stat_right_pub_topic, PropulsionModuleStatus) self.w_stat = PropulsionModuleStatus() # setup wheel feedback topic publisher if self.pub_fb_interval > 0: self.wl_fb_vel = 0.0 self.wl_fb_vel_set = 0.0 self.wl_fb_thrust = 0.0 self.wr_fb_vel = 0.0 self.wr_fb_vel_set = 0.0 self.wr_fb_thrust = 0.0 self.w_fb_left_pub = rospy.Publisher(self.w_fb_left_pub_topic, PropulsionModuleFeedback) self.w_fb_right_pub = rospy.Publisher(self.w_fb_right_pub_topic, PropulsionModuleFeedback) self.w_fb = PropulsionModuleFeedback() # setup subscription topic callbacks self.deadman_tout = 0 rospy.Subscriber(self.deadman_topic, Bool, self.on_deadman_message) self.cmd_vel_tout = 0 self.cmd_vel_tout_active = True self.wl_tout = 0 self.wl_tout_active = True self.wr_tout = 0 self.wr_tout_active = True self.vel_lin_desired = 0.0 self.vel_ang_desired = 0.0 self.vel_lin = 0.0 self.vel_ang = 0.0 rospy.Subscriber(self.cmd_vel_topic, TwistStamped, self.on_cmd_vel_message) # call updater function self.r = rospy.Rate(self.update_rate) self.updater()
def __init__(self): rospy.loginfo(rospy.get_name() + ": Start") # defines self.update_rate = 50 # set update frequency [Hz] self.pid_update_interval = 20 # [ms] self.w_STATE_ERR_NO_CONFIG = 3 self.w_STATE_ERR = 3 self.count = 0 # locally defined parameters self.deadman_tout_duration = 0.2 # [s] self.cmd_vel_tout_duration = 0.2 # [s] self.w_tout_duration = 0.2 # [s] # get parameters self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 1.0) # [m] self.ticks_per_meter_left = rospy.get_param("/ticks_per_meter_left", 1000) self.ticks_per_meter_right = rospy.get_param("/ticks_per_meter_right", 1000) acc_lin_max = rospy.get_param("~max_linear_acceleration", 1.0) # [m/s^2] acc_ang_max = rospy.get_param("~max_angular_acceleration", 0.1) # [rad/s^2] self.acc_lin_max_step = acc_lin_max/(self.update_rate * 1.0) self.acc_ang_max_step = acc_ang_max/(self.update_rate * 1.0) self.wl_kp = rospy.get_param("~wheel_left_kp", 1.0) self.wl_ki = rospy.get_param("~wheel_left_ki", 0.0) self.wl_kd = rospy.get_param("~wheel_left_kd", 0.0) self.wl_max_integral_output = rospy.get_param("~wheel_left_max_integral_output",0.0) self.wr_kp = rospy.get_param("~wheel_right_kp", 1.0) self.wr_ki = rospy.get_param("~wheel_right_ki", 0.0) self.wr_kd = rospy.get_param("~wheel_right_kd", 0.0) self.wr_max_integral_output = rospy.get_param("~wheel_right_max_integral_output", 0.0) self.min_supply_voltage = rospy.get_param("~min_supply_voltage", 12.0) pub_status_rate = rospy.get_param("~publish_wheel_status_rate", 5) if pub_status_rate != 0: self.pub_status_interval = int(self.update_rate/pub_status_rate) else: self.pub_status_interval = 0 pub_fb_rate = rospy.get_param("~publish_wheel_feedback_rate", 0) if pub_fb_rate != 0: self.pub_fb_interval = int(self.update_rate/pub_fb_rate) else: self.pub_fb_interval = 0 # get topic names self.deadman_topic = rospy.get_param("~deadman_sub",'/fmCommand/deadman') self.cmd_vel_topic = rospy.get_param("~cmd_vel_sub",'/fmCommand/cmd_vel') self.enc_left_topic = rospy.get_param("~enc_left_pub",'/fmInformation/enc_left') self.enc_right_topic = rospy.get_param("~enc_right_pub",'/fmInformation/enc_right') self.w_fb_left_pub_topic = rospy.get_param("~wheel_feedback_left_pub",'/fmInformation/wheel_feedback_left') self.w_fb_right_pub_topic = rospy.get_param("~wheel_feedback_right_pub",'/fmInformation/wheel_feedback_right') self.w_stat_left_pub_topic = rospy.get_param("~wheel_status_left_pub",'/fmInformation/wheel_status_left') self.w_stat_right_pub_topic = rospy.get_param("~wheel_status_right_pub",'/fmInformation/wheel_status_right') wl_sub_topic = rospy.get_param("~wheel_left_sub",'/fmData/wheel_left_serial_in') wl_pub_topic = rospy.get_param("~wheel_left_pub",'/fmSignal/wheel_left_serial_out') wr_sub_topic = rospy.get_param("~wheel_right_sub",'/fmData/wheel_right_serial_in') wr_pub_topic = rospy.get_param("~wheel_right_pub",'/fmSignal/wheel_right_serial_out') # instantiate wheel interface classes self.wl = wheel_interface('L: ', wl_pub_topic, wl_sub_topic) self.wr = wheel_interface('R: ', wr_pub_topic, wr_sub_topic) # instantiate differntial kinematics class self.dk = differential_kinematics(self.w_dist) # setup encoder topic publisher self.enc_left_pub = rospy.Publisher(self.enc_left_topic, IntStamped) self.enc_right_pub = rospy.Publisher(self.enc_right_topic, IntStamped) self.intstamp = IntStamped() # setup wheel status topic publisher if self.pub_status_interval > 0: self.w_stat_left_pub = rospy.Publisher(self.w_stat_left_pub_topic, PropulsionModuleStatus) self.w_stat_right_pub = rospy.Publisher(self.w_stat_right_pub_topic, PropulsionModuleStatus) self.w_stat = PropulsionModuleStatus() # setup wheel feedback topic publisher if self.pub_fb_interval > 0: self.wl_fb_vel = 0.0 self.wl_fb_vel_set = 0.0 self.wl_fb_thrust = 0.0 self.wr_fb_vel = 0.0 self.wr_fb_vel_set = 0.0 self.wr_fb_thrust = 0.0 self.w_fb_left_pub = rospy.Publisher(self.w_fb_left_pub_topic, PropulsionModuleFeedback) self.w_fb_right_pub = rospy.Publisher(self.w_fb_right_pub_topic, PropulsionModuleFeedback) self.w_fb = PropulsionModuleFeedback() # setup subscription topic callbacks self.deadman_tout = 0 rospy.Subscriber(self.deadman_topic, Bool, self.on_deadman_message) self.cmd_vel_tout = 0 self.cmd_vel_tout_active = True self.wl_tout = 0 self.wl_tout_active = True self.wr_tout = 0 self.wr_tout_active = True self.vel_lin_desired = 0.0 self.vel_ang_desired = 0.0 self.vel_lin = 0.0 self.vel_ang = 0.0 rospy.Subscriber(self.cmd_vel_topic, TwistStamped, self.on_cmd_vel_message) # call updater function self.r = rospy.Rate(self.update_rate) self.updater()
def __init__(self): rospy.loginfo(rospy.get_name() + ": Start") # defines self.count = 0 # static parameters self.update_rate = 50 # set update frequency [Hz] self.act_en_tout_duration = 0.2 # [s] self.cmd_vel_tout_duration = 0.2 # [s] # get parameters self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2) # [m] self.ticks_per_meter_left = rospy.get_param("/ticks_per_meter_left", 500) self.ticks_per_meter_right = rospy.get_param("/ticks_per_meter_right", 500) init_e = rospy.get_param("~init_easting", 0.0) # [m] init_n = rospy.get_param("~init_northing", 0.0) # [m] init_orientation = rospy.get_param("~init_orientation", 0.0) # [m] acc_lin_max = rospy.get_param("~max_linear_acceleration", 1.0) # [m] acc_ang_max = rospy.get_param("~max_angular_acceleration", 1.8) # [rad/s^2] self.wheel_speed_variance = 0.001 self.wheel_speed_delay = 0.05 # [s] self.wheel_speed_delay_variance = 0.05 self.wheel_speed_error = 0.03 # [m/s] self.wheel_speed_minimum = 0.0 pub_fb_rate = rospy.get_param("~publish_wheel_feedback_rate", 0) if pub_fb_rate != 0: self.pub_fb_interval = int(self.update_rate/pub_fb_rate) else: self.pub_fb_interval = 0 # get topic names self.topic_act_en = rospy.get_param("~actuation_enable_sub",'/fmSafe/actuation_enable') self.topic_cmd_vel = rospy.get_param("~cmd_vel_sub",'/fmCommand/cmd_vel') self.topic_odom_reset = rospy.get_param("~odom_reset_sub",'/fmInformation/odom_reset') self.topic_pose = rospy.get_param("~pose_pub",'/fmKnowledge/pose') self.topic_w_fb_left_pub = rospy.get_param("~wheel_feedback_left_pub",'/fmInformation/wheel_feedback_left') self.topic_w_fb_right_pub = rospy.get_param("~wheel_feedback_right_pub",'/fmInformation/wheel_feedback_right') # initialize internal variables self.update_interval = 1/(self.update_rate * 1.0) self.pi2 = 2*pi self.cmd_vel_tout_active = True self.pose = [init_e, init_n, init_orientation] self.act_en_tout = 0.0 self.cmd_vel_msgs = [] self.vel_lin_desired = 0.0 self.vel_ang_desired = 0.0 self.acc_lin_max_step = acc_lin_max*self.update_interval self.acc_ang_max_step = acc_ang_max*self.update_interval self.vel_lin = 0.0 self.vel_ang = 0.0 self.ref_vel_left = 0.0 self.ref_vel_right = 0.0 self.sim_vel_left = 0.0 self.sim_vel_right = 0.0 self.wheel_speed_err_left = 0.0 self.wheel_speed_err_right = 0.0 self.dk = differential_kinematics(self.w_dist) # setup topic publishers self.pose_pub = rospy.Publisher(self.topic_pose, Odometry, queue_size=10) self.pose_msg = Odometry() # setup wheel feedback topic publisher if self.pub_fb_interval > 0: self.wl_fb_vel_set = 0.0 self.wr_fb_vel_set = 0.0 self.w_fb_left_pub = rospy.Publisher(self.topic_w_fb_left_pub, PropulsionModuleFeedback, queue_size=10) self.w_fb_right_pub = rospy.Publisher(self.topic_w_fb_right_pub, PropulsionModuleFeedback, queue_size=10) self.w_fb = PropulsionModuleFeedback() # setup subscription topic callbacks rospy.Subscriber(self.topic_act_en, BoolStamped, self.on_act_en_message) rospy.Subscriber(self.topic_cmd_vel, TwistStamped, self.on_cmd_vel_message) rospy.Subscriber(self.topic_odom_reset, FloatArrayStamped, self.on_odom_reset_message) # call updater function self.r = rospy.Rate(self.update_rate) self.updater()