def __init__(self):
rospy.loginfo(rospy.get_name() + ": Start")
# defines
self.c = '$'
# static parameters
self.update_rate = 100 # set update frequency [Hz]
# get parameters
self.device = rospy.get_param("~device", "/dev/ttyUSB0") #
# get topic names
self.topic_timing = rospy.get_param("~timing_pub",
'/fmInformation/rt_timing')
# setup topic publishers
self.timing_pub = rospy.Publisher(self.topic_timing, IntStamped)
self.timing_msg = IntStamped()
# configure and open serial device
ser_error = False
try:
self.ser = serial.Serial(self.device, 57600, timeout=0)
except Exception as e:
rospy.logerr(rospy.get_name() +
": Unable to open serial device: %s" % self.device)
ser_error = True
if ser_error == False:
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
rospy.loginfo(rospy.get_name() + ": Start")
# defines
# static parameters
self.update_rate = 20 # set update frequency [Hz]
# get topic names
self.topic_obstacle = rospy.get_param("~obstacle_sub",
'/fmKnowledge/obstacle')
self.topic_obstacle_safe = rospy.get_param("~obstacle_safe_pub",
'/fmSafe/obstacle')
# setup topic publishers
self.obstacle_safe_pub = rospy.Publisher(self.topic_obstacle_safe,
IntStamped,
queue_size=0)
self.obstacle_safe_msg = IntStamped()
# setup topic subscribers
rospy.Subscriber(self.topic_obstacle, IntArrayStamped,
self.on_obstacle_msg)
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
rospy.loginfo(rospy.get_name() + ": Start")
# get parameters
self.update_rate = rospy.get_param("~update_rate",
"10") # update frequency [Hz]
self.polygons_per_update = rospy.get_param("~polygons_per_update",
"100")
self.nearby_threshold = rospy.get_param("~nearby_threshold",
"5.0") # [m]
rospy.loginfo(rospy.get_name() + ": Update rate: %ld Hz",
self.update_rate)
rospy.loginfo(rospy.get_name() + ": Polygons per update: %ld",
self.polygons_per_update)
rospy.loginfo(rospy.get_name() + ": Nearby threshold: %.3f m",
self.nearby_threshold)
# get topic names
self.pose_topic = rospy.get_param("~pose_sub", '/fmKnowledge/pose')
self.polygon_map_topic = rospy.get_param("~polygon_map_pub",
'/fmKnowledge/polygon_map')
# setup subscription topic callbacks
rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
# setup publish topics
self.polygon_change_pub = rospy.Publisher(self.polygon_map_topic,
IntStamped)
self.polygon_change = IntStamped()
# initialize the polygon map
self.polymap = polygon_map()
self.polymap.set_nearby_threshold(self.nearby_threshold)
self.polymap.set_polygons_per_update(self.polygons_per_update)
# import polygons
file = open('polygon_map.txt', 'r')
file_content = csv.reader(file, delimiter='\t')
for name, e1, n1, e2, n2, e3, n3, e4, n4 in file_content:
polygon = [[float(e1), float(n1)], [float(e2),
float(n2)],
[float(e3), float(n3)], [float(e4),
float(n4)]]
self.polymap.add_polygon(name, polygon)
file.close()
rospy.loginfo(rospy.get_name() +
": Loaded %ld polygons" % self.polymap.poly_total)
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
# defines
self.update_rate = 10 # set update frequency [Hz]
rospy.loginfo(rospy.get_name() + ": Start")
# get parameters
#self.debug = rospy.get_param("~print_debug_information", 'true')
#if self.debug:
# rospy.loginfo(rospy.get_name() + ": Debug enabled")
#self.status_publish_interval = rospy.get_param("~status_publish_interval", '0')
# get topic names
self.pose_topic = rospy.get_param("~pose_sub", '/fmKnowledge/pose')
self.polygon_map_topic = rospy.get_param("~polygon_map_pub",
'/fmKnowledge/polygon_map')
# setup subscription topic callbacks
rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
# setup publish topics
self.polygon_change_pub = rospy.Publisher(self.polygon_map_topic,
IntStamped)
self.polygon_change = IntStamped()
# initialize the polygon map
self.polymap = polygon_map()
self.polymap.set_nearby_threshold(5.0)
self.polymap.set_polygons_per_update(100)
# import polygons
file = open('TekInnerParcelCornersExtended.csv', 'r')
file_content = csv.reader(file, delimiter='\t')
for name, e1, n1, e2, n2, e3, n3, e4, n4 in file_content:
polygon = [[float(e1), float(n1)], [float(e2),
float(n2)],
[float(e3), float(n3)], [float(e4),
float(n4)]]
self.polymap.add_polygon(name, polygon)
file.close()
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
self.update_rate = 20 # [Hz]
# robot state
self.BHV_RC = 0
self.BHV_WPTNAV = 1
self.bhv = self.BHV_RC
# load behaviours
self.bhv_rc = behaviour_remote_control()
self.bhv_wn = behaviour_wptnav()
# get topic names
topic_rc = rospy.get_param("~remote_control_sub",
'/fmHMI/remote_control')
topic_rc_feedback = rospy.get_param("~remote_control_feedback_pub",
'/fmHMI/remote_control_feedback')
deadman_topic = rospy.get_param("~deadman_pub", "/fmSafe/deadman")
behaviour_topic = rospy.get_param("~automode_pub", "/fmPlan/automode")
# setup deadman publish topic
self.deadman_state = False
self.deadman_msg = BoolStamped()
self.deadman_pub = rospy.Publisher(deadman_topic,
BoolStamped,
queue_size=1)
# setup automode publish topic
self.behaviour_msg = IntStamped()
self.behaviour_pub = rospy.Publisher(behaviour_topic,
IntStamped,
queue_size=1)
# setup subscription topic callbacks
rospy.Subscriber(topic_rc, RemoteControl, self.on_rc_topic)
# sall 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()
#!/usr/bin/env python
import rospy
from msgs.msg import IntStamped
import serial
serial_device = '/dev/ttyUSB0'
serial_baudrate = 38400
topic_pos = '/fmInformation/encoder_pos'
update_interval = 0.1 # [s]
rospy.init_node('dmm_tech_abs_node')
pub = rospy.Publisher(topic_pos, IntStamped)
msg = IntStamped()
serial_err = False
try:
ser = serial.Serial(serial_device, serial_baudrate, timeout=1)
except Exception as e:
serial_err = True
rospy.logerr (rospy.get_name() + ': Unable to open %s' % (serial_device))
if serial_err == False:
print 'ok'
#ser.write (bytes('\0')) # request position
ser.write ('\0')
while not rospy.is_shutdown():
s = ser.read (1) # read two bytes
print len(s)
#!/usr/bin/env python
import rospy
from msgs.msg import IntStamped
import serial
serial_device = '/dev/ttyUSB0'
serial_baudrate = 38400
topic_pos = '/fmInformation/encoder_pos'
update_interval = 0.1 # [s]
rospy.init_node('dmm_tech_abs_node')
pub = rospy.Publisher(topic_pos, IntStamped)
msg = IntStamped()
serial_err = False
try:
ser = serial.Serial(serial_device, serial_baudrate, timeout=1)
except Exception as e:
serial_err = True
rospy.logerr(rospy.get_name() + ': Unable to open %s' % (serial_device))
if serial_err == False:
print 'ok'
#ser.write (bytes('\0')) # request position
ser.write('\0')
while not rospy.is_shutdown():
s = ser.read(1) # read two bytes
print len(s)
ser.write('\0')
def __init__(self):
self.update_rate = 20 # [Hz]
# robot state
self.STATE_MANUAL = 0
self.STATE_WPTNAV = 1
self.state = self.STATE_MANUAL
# keyboard interface
self.KEY_ESC = 27
self.KEY_SECOND = 91
self.KEY_SPACE = 32
self.KEY_a = 97
self.KEY_m = 109
self.KEY_e = 101
self.KEY_s = 115
self.KEY_ARROW_UP = 65
self.KEY_ARROW_DOWN = 66
self.KEY_ARROW_RIGHT = 67
self.KEY_ARROW_LEFT = 68
self.esc_key = False
self.second_key = False
# read parameters
self.vel_lin_max = rospy.get_param("~max_linear_velocity",
0.5) # [m/s]
self.vel_ang_max = rospy.get_param("~max_angular_velocity",
0.3) # [rad/s]
self.vel_lin_step = rospy.get_param("~linear_velocity_step",
0.1) # [m/s]
self.vel_ang_step = rospy.get_param("~angular_velocity_step",
0.1) # [rad/s]
# get topic names
kbd_topic = rospy.get_param("~keyboard_sub", "/fmHMI/keyboard")
deadman_topic = rospy.get_param("~deadman_pub", "/fmSafe/deadman")
automode_topic = rospy.get_param("~automode_pub", "/fmPlan/automode")
cmd_vel_topic = rospy.get_param("~cmd_vel_pub", "/fmCommand/cmd_vel")
# setup deadman publish topic
self.deadman_state = False
self.deadman_msg = BoolStamped()
self.deadman_pub = rospy.Publisher(deadman_topic,
BoolStamped,
queue_size=1)
# setup automode publish topic
self.automode_msg = IntStamped()
self.automode_pub = rospy.Publisher(automode_topic,
IntStamped,
queue_size=1)
# setup manual velocity topic
self.vel_lin = 0.0
self.vel_ang = 0.0
self.cmd_vel_msg = TwistStamped()
self.cmd_vel_pub = rospy.Publisher(cmd_vel_topic,
TwistStamped,
queue_size=1)
# setup subscription topic callbacks
rospy.Subscriber(kbd_topic, Char, self.on_kbd_topic)
# sall updater function
self.r = rospy.Rate(self.update_rate)
self.updater()