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)
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')
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')
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 __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)
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()
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()