def publish_wheel_parameter_message(self):
		# configure minimum supply voltage
		nmea_sys_par = nmea()
		nmea_sys_par.header.stamp = rospy.Time.now()
		nmea_sys_par.type = 'PFSSP'
		nmea_sys_par.length = 1
		nmea_sys_par.data.append('%d' % (self.min_supply_voltage/self.w_voltage_conv + 0.5)) 
		self.wl_pub.publish (nmea_sys_par)
		self.wr_pub.publish (nmea_sys_par)
		#print self.min_supply_voltage, self.min_supply_voltage/self.w_voltage_conv, nmea_sys_par.data[0]

		# configure PID parameters
		nmea_pid = nmea()
		nmea_pid.header.stamp = rospy.Time.now()
		nmea_pid.type = 'PFSWP'
		nmea_pid.length = 6
		nmea_pid.data.append('1') # enable closed loop PID
		nmea_pid.data.append('%d' % self.pid_update_interval) 
		nmea_pid.data.append('%d' % self.wl_kp) # Kp
		nmea_pid.data.append('%d' % self.wl_ki) # Ki
		nmea_pid.data.append('%d' % self.wl_kd) # Kd
		nmea_pid.data.append('%d' % self.wl_max_integral_output) # Maximum output (integrator anti-windup)
		self.wl_pub.publish (nmea_pid)
		nmea_pid.data = []
		nmea_pid.data.append('1') # enable closed loop PID
		nmea_pid.data.append('%d' % self.pid_update_interval) 
		nmea_pid.data.append('%d' % self.wr_kp) # Kp
		nmea_pid.data.append('%d' % self.wr_ki) # Ki
		nmea_pid.data.append('%d' % self.wr_kd) # Kd
		nmea_pid.data.append('%d' % self.wr_max_integral_output) # Maximum output (integrator anti-windup)
		self.wr_pub.publish (nmea_pid)
    def publish_wheel_parameter_message(self):
        # configure minimum supply voltage
        nmea_sys_par = nmea()
        nmea_sys_par.header.stamp = rospy.Time.now()
        nmea_sys_par.type = 'PFSSP'
        nmea_sys_par.length = 1
        nmea_sys_par.data.append(
            '%d' % (self.min_supply_voltage / self.w_voltage_conv + 0.5))
        self.wl_pub.publish(nmea_sys_par)
        self.wr_pub.publish(nmea_sys_par)
        #print self.min_supply_voltage, self.min_supply_voltage/self.w_voltage_conv, nmea_sys_par.data[0]

        # configure PID parameters
        nmea_pid = nmea()
        nmea_pid.header.stamp = rospy.Time.now()
        nmea_pid.type = 'PFSWP'
        nmea_pid.length = 6
        nmea_pid.data.append('1')  # enable closed loop PID
        nmea_pid.data.append('%d' % self.pid_update_interval)
        nmea_pid.data.append('%d' % self.wl_kp)  # Kp
        nmea_pid.data.append('%d' % self.wl_ki)  # Ki
        nmea_pid.data.append('%d' % self.wl_kd)  # Kd
        nmea_pid.data.append('%d' % self.wl_max_integral_output
                             )  # Maximum output (integrator anti-windup)
        self.wl_pub.publish(nmea_pid)
        nmea_pid.data = []
        nmea_pid.data.append('1')  # enable closed loop PID
        nmea_pid.data.append('%d' % self.pid_update_interval)
        nmea_pid.data.append('%d' % self.wr_kp)  # Kp
        nmea_pid.data.append('%d' % self.wr_ki)  # Ki
        nmea_pid.data.append('%d' % self.wr_kd)  # Kd
        nmea_pid.data.append('%d' % self.wr_max_integral_output
                             )  # Maximum output (integrator anti-windup)
        self.wr_pub.publish(nmea_pid)
def talker():
    global first_time
    global nmea_pfbst_interval
    global nmea_pfbct_watchdog_timeout
    pub = rospy.Publisher('/fmLib/nmea_to_frobit', nmea)
    nm = nmea()
    rospy.init_node('talker')
    r = rospy.Rate(node_upd_freq)  # hz

    while not rospy.is_shutdown():
        if first_time == 1:
            #			nm.type = 'PFBCO'
            #			nm.data = ['%d' %(nmea_pfbst_interval), '%d' % (nmea_pfbct_watchdog_timeout)]
            #			nm.header.stamp = rospy.Time.now()
            #			pub.publish(nm)
            first_time = 0


#		nm.type = 'PFBCO'
#		nm.data = ['%d' %(nmea_pfbst_interval), '%d' % (nmea_pfbct_watchdog_timeout)]
        nm.type = 'PFBCT'
        nm.data = ['%d' % (spd_left), '%d' % (spd_right)]
        nm.header.stamp = rospy.Time.now()
        pub.publish(nm)
        r.sleep()
	def publish_wheel_parameter_open_loop_message(self):
		nmea_pid = nmea()
		nmea_pid.header.stamp = rospy.Time.now()
		nmea_pid.type = 'PFBWP'
		nmea_pid.length = 1
		nmea_pid.data.append('0') # enable open loop
		self.frobit_pub.publish (nmea_pid)
Example #5
0
 def publish_wheel_parameter_open_loop_message(self):
     nmea_pid = nmea()
     nmea_pid.header.stamp = rospy.Time.now()
     nmea_pid.type = 'PFBWP'
     nmea_pid.length = 1
     nmea_pid.data.append('0')  # enable open loop
     self.frobit_pub.publish(nmea_pid)
 def publish_wheel_parameter_open_loop_message(self):
     nmea_pid = nmea()
     nmea_pid.header.stamp = rospy.Time.now()
     nmea_pid.type = "PFSWP"
     nmea_pid.length = 1
     nmea_pid.data.append("0")  # enable open loop
     self.wl_pub.publish(nmea_pid)
     self.wr_pub.publish(nmea_pid)
    def __init__(self):
        # constants

        # parameters
        update_frequency = 5

        # state variable initialization
        self.automode = False
        self.deadman = False
        self.pose = False
        self.pose_timeout = 1.0
        self.latest_pose = 0.0
        self.gps_timeout = 1.0
        self.latest_gps = 0.0
        self.gps_fix = 0

        # Get parameters
        topic_gga = rospy.get_param("~gga_sub",
                                    '/fmInformation/gpgga_tranmerc')
        topic_pose = rospy.get_param("~pose_sub", '/fmKnowledge/pose')
        topic_deadman = rospy.get_param("~deadman_sub", '/fmCommand/deadman')
        topic_automode = rospy.get_param("~automode_sub",
                                         '/fmDecision/automode')
        topic_wptnav_status = rospy.get_param("~wptnav_status_sub",
                                              '/fmInformation/wptnav_status')
        topic_lighthouse_rx = rospy.get_param("~lighthouse_sub",
                                              '/fmData/nmea_from_lighthouse')
        topic_lighthouse_tx = rospy.get_param("~lighthouse_pub",
                                              '/fmData/nmea_to_lighthouse')

        # define NMEA $PFLSL topic
        self.nmea_pflsl = nmea()
        self.nmea_pflsl.type = 'PFLSL'
        self.nmea_pflsl.length = 3
        self.nmea_pflsl.valid = True
        self.light_red = 0
        self.light_yellow = 0
        self.light_green = 0
        self.nmea_pflsl.data.append(str(self.light_red))
        self.nmea_pflsl.data.append(str(self.light_yellow))
        self.nmea_pflsl.data.append(str(self.light_green))

        # Setup subscription topic callbacks
        rospy.Subscriber(topic_gga, gpgga_tranmerc, self.on_gga_msg)
        rospy.Subscriber(topic_pose, Odometry, self.on_pose_msg)
        rospy.Subscriber(topic_deadman, Bool, self.on_deadman_msg)
        rospy.Subscriber(topic_automode, Bool, self.on_automode_msg)
        rospy.Subscriber(topic_wptnav_status, waypoint_navigation_status,
                         self.on_wptnav_status_msg)

        # setup topic publishers
        self.nmea_pub = rospy.Publisher(topic_lighthouse_tx, nmea)

        # Call updater function
        self.r = rospy.Rate(update_frequency)  # set updater frequency
        self.updater()
	def publish_configuration_messages(self):
		if rospy.get_time() > self.send_cfg_tout:
			self.send_cfg_tout = rospy.get_time() + 1.0 # 1 second timeout

			# configure minimum supply voltage
			nmea_sys_par = nmea()
			nmea_sys_par.header.stamp = rospy.Time.now()
			nmea_sys_par.type = 'PFBSP'
			nmea_sys_par.length = 1
			nmea_sys_par.data.append('%d' % (self.min_supply_voltage/self.frobit_voltage_conv + 0.5)) 
			self.frobit_pub.publish (nmea_sys_par)
			self.publish_pid_drive_param_message()
			rospy.loginfo (rospy.get_name() + ': Sending configuration to Frobit')
Example #9
0
	def __init__(self):
		# constants

		# parameters
		update_frequency = 5

		# state variable initialization
		self.automode = False
		self.deadman = False
		self.pose = False
		self.pose_timeout = 1.0
		self.latest_pose = 0.0
		self.gps_timeout = 1.0
		self.latest_gps = 0.0
		self.gps_fix = 0

		# Get parameters
		topic_gga = rospy.get_param("~gga_sub",'/fmInformation/gpgga_tranmerc')
		topic_pose = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
		topic_deadman = rospy.get_param("~deadman_sub",'/fmCommand/deadman')
		topic_automode = rospy.get_param("~automode_sub",'/fmDecision/automode')
		topic_wptnav_status = rospy.get_param("~wptnav_status_sub",'/fmInformation/wptnav_status')
		topic_lighthouse_rx = rospy.get_param("~lighthouse_sub",'/fmData/nmea_from_lighthouse')
		topic_lighthouse_tx = rospy.get_param("~lighthouse_pub",'/fmData/nmea_to_lighthouse')

		# define NMEA $PFLSL topic
		self.nmea_pflsl = nmea()
		self.nmea_pflsl.type = 'PFLSL'
		self.nmea_pflsl.length = 3
		self.nmea_pflsl.valid = True
		self.light_red = 0
		self.light_yellow = 0
		self.light_green = 0
		self.nmea_pflsl.data.append(str(self.light_red))
		self.nmea_pflsl.data.append(str(self.light_yellow))
		self.nmea_pflsl.data.append(str(self.light_green))

		# Setup subscription topic callbacks
		rospy.Subscriber(topic_gga, gpgga_tranmerc, self.on_gga_msg)
		rospy.Subscriber(topic_pose, Odometry, self.on_pose_msg)
		rospy.Subscriber(topic_deadman, Bool, self.on_deadman_msg)
		rospy.Subscriber(topic_automode, Bool, self.on_automode_msg)
		rospy.Subscriber(topic_wptnav_status, waypoint_navigation_status, self.on_wptnav_status_msg)

		# setup topic publishers
		self.nmea_pub = rospy.Publisher(topic_lighthouse_tx, nmea)

		# Call updater function
		self.r = rospy.Rate(update_frequency) # set updater frequency
		self.updater()
    def publish_configuration_messages(self):
        if rospy.get_time() > self.send_cfg_tout:
            self.send_cfg_tout = rospy.get_time() + 1.0  # 1 second timeout

            # configure minimum supply voltage
            nmea_sys_par = nmea()
            nmea_sys_par.header.stamp = rospy.Time.now()
            nmea_sys_par.type = 'PFBSP'
            nmea_sys_par.length = 1
            nmea_sys_par.data.append(
                '%d' %
                (self.min_supply_voltage / self.frobit_voltage_conv + 0.5))
            self.frobit_pub.publish(nmea_sys_par)
            self.publish_pid_drive_param_message()
            rospy.loginfo(rospy.get_name() +
                          ': Sending configuration to Frobit')
Example #11
0
    def __init__(self):
        rospy.init_node('topcon_initializer')

        self.rate = rospy.Rate(1)

        self.serial_pub = rospy.Publisher("/fmData/rx", serial)
        self.serial_sub = rospy.Subscriber("/fmData/tx", serial, self.onSerial)
        self.nmea_pub = rospy.Publisher("/fmData/nmea_out", nmea)
        self.nmea_sub = rospy.Subscriber("/fmData/nmea_in", nmea, self.onNMEA)

        self.serial_msg = serial()

        self.nmea_msg = nmea()
        self.nmea_msg.type = "GPGGA"
        self.nmea_msg.length = 5
        self.nmea_msg.data = ["100", "12.15", "", "0", "01.10"]
Example #12
0
 def __init__(self):
     rospy.init_node('topcon_initializer')
     
     self.rate = rospy.Rate(1)
     
     self.serial_pub = rospy.Publisher("/fmData/rx", serial)
     self.serial_sub = rospy.Subscriber("/fmData/tx", serial, self.onSerial )
     self.nmea_pub = rospy.Publisher("/fmData/nmea_out", nmea)
     self.nmea_sub = rospy.Subscriber("/fmData/nmea_in", nmea, self.onNMEA )
     
     self.serial_msg = serial()
     
     self.nmea_msg = nmea()
     self.nmea_msg.type = "GPGGA"
     self.nmea_msg.length = 5
     self.nmea_msg.data = ["100","12.15","","0","01.10"]
	def publish_wheel_parameter_message(self):
		nmea_pid = nmea()
		nmea_pid.header.stamp = rospy.Time.now()
		nmea_pid.type = 'PFSWP'
		nmea_pid.length = 5
		nmea_pid.data.append('1') # enable closed loop PID
		nmea_pid.data.append('%d' % self.pid_update_interval) 
		nmea_pid.data.append('%d' % self.wheel_left_kp) # Kp
		nmea_pid.data.append('%d' % self.wheel_left_ki) # Ki
		nmea_pid.data.append('%d' % self.wheel_left_kd) # Kd
		self.wheel_left_pub.publish (nmea_pid)
		nmea_pid.length = 5
		nmea_pid.data.append('1') # enable closed loop PID
		nmea_pid.data.append('%d' % self.wheel_right_kp) # Kp
		nmea_pid.data.append('%d' % self.wheel_right_ki) # Ki
		nmea_pid.data.append('%d' % self.wheel_right_kd) # Kd
		self.wheel_right_pub.publish (nmea_pid)
Example #14
0
	def __init__(self):
		# constants
		
		# parameters
		update_frequency = 5

		# state variable initialization
		self.painter_state = ST_PAINT_IDLE
		self.last_painter_state = ST_PAINT_IDLE
		self.last_state = False

		# Get parameters
		topic_implement_painter = rospy.get_param("~painter_cmd_sub",'/fmCommand/implement')
		topic_painter_rx = rospy.get_param("~nmea_from_painter_sub",'/fmData/nmea_from_painter')
		topic_painter_tx = rospy.get_param("~nmea_to_painter_pub",'/fmData/nmea_to_painter')
		self.paintertime = rospy.get_param("~paint_time",'0.5') # [s]
		if self.paintertime < 0.01:
			self.paint_timeout = False
		else:
			self.paint_timeout = True

		# define NMEA $pfrrs topic
		self.nmea_pfrrs = nmea()
		self.nmea_pfrrs.type = 'PFRRS'
		self.nmea_pfrrs.length = 1
		self.nmea_pfrrs.valid = True
		self.nmea_pfrrs.data.append(str(self.painter_state))

		# Setup subscription topic callbacks
		rospy.Subscriber(topic_implement_painter, BoolStamped, self.on_painter_msg)

		# setup topic publishers
		self.nmea_pub = rospy.Publisher(topic_painter_tx, nmea, queue_size=1)

		# Call updater function
		self.r = rospy.Rate(update_frequency) # set updater frequency
		self.updater()
def talker():
	global first_time
	global nmea_pfbst_interval
	global nmea_pfbct_watchdog_timeout
	pub = rospy.Publisher('/fmLib/nmea_to_frobit', nmea)
	nm = nmea()
	rospy.init_node('talker')
	r = rospy.Rate(node_upd_freq) # hz

	while not rospy.is_shutdown():
		if first_time == 1:
#			nm.type = 'PFBCO'
#			nm.data = ['%d' %(nmea_pfbst_interval), '%d' % (nmea_pfbct_watchdog_timeout)]
#			nm.header.stamp = rospy.Time.now()
#			pub.publish(nm)
			first_time = 0

#		nm.type = 'PFBCO'
#		nm.data = ['%d' %(nmea_pfbst_interval), '%d' % (nmea_pfbct_watchdog_timeout)]
		nm.type = 'PFBCT'
		nm.data = ['%d' %(spd_left), '%d' % (spd_right)]
		nm.header.stamp = rospy.Time.now()
		pub.publish(nm)
		r.sleep()
	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.update_rate = 20 # set update frequency [Hz]
		self.pid_update_interval = 20 # [ms]

		# locally defined parameters
		self.deadman_tout_duration = 0.2 # [s]
		self.cmd_vel_tout_duration = 0.2 # [s]

		# get parameters
		self.wheel_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.wheel_left_kp = rospy.get_param("~wheel_left_kp",'1.0')
		self.wheel_left_ki = rospy.get_param("~wheel_left_ki",'0.0')
		self.wheel_left_kd = rospy.get_param("~wheel_left_kd",'0.0')
		self.wheel_right_kp = rospy.get_param("~wheel_right_kp",'1.0')
		self.wheel_right_ki = rospy.get_param("~wheel_right_ki",'0.0')
		self.wheel_right_kd = rospy.get_param("~wheel_right_kd",'0.0')
		rospy.loginfo (rospy.get_name() + ': Differential wheel distance %.2f' % self.wheel_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.wheel_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.wheel_left_sub_topic = rospy.get_param("~wheel_left_sub",'/fmData/wheel_left_nmea_in')
		self.wheel_left_pub_topic = rospy.get_param("~wheel_left_pub",'/fmSignal/wheel_left_nmea_out')
		self.wheel_right_sub_topic = rospy.get_param("~wheel_right_sub",'/fmData/wheel_right_nmea_in')
		self.wheel_right_pub_topic = rospy.get_param("~wheel_right_pub",'/fmSignal/wheel_right_nmea_out')

		# setup wheel ctrl topic publisher
		self.wheel_left_pub = rospy.Publisher(self.wheel_left_pub_topic, nmea)
		self.wheel_right_pub = rospy.Publisher(self.wheel_right_pub_topic, nmea)
		self.nmea = nmea()
		self.nmea.data.append('0')
		self.nmea.valid = True

		# setup encoder topic publisher
		self.state_left = 0
		self.enc_left = 0
		self.volt_left = 0
		self.state_right = 0
		self.enc_right = 0
		self.volt_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 subscription topic callbacks
		self.deadman_tout = 0
		rospy.Subscriber(self.deadman_topic, Bool, self.on_deadman_message)
		self.reset_vel() # set velocity to zero
		self.cmd_vel_tout = 0
		self.cmd_vel_tout_active = True
		rospy.Subscriber(self.cmd_vel_topic, TwistStamped, self.on_cmd_vel_message)
		rospy.Subscriber(self.wheel_left_sub_topic, nmea, self.on_wheel_left_message)
		rospy.Subscriber(self.wheel_right_sub_topic, nmea, self.on_wheel_right_message)

		# send parameters to wheel modules		
		self.publish_wheel_parameter_message()
		self.publish_wheel_parameter_message()
		self.publish_wheel_parameter_message() # necessary due to error in serial driver?

		# 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 = 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()
Example #20
0
    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()