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