def __init__(self):
# defines
self.update_rate = 20 # set update frequency [Hz]
self.IMPLEMENT_INVALID = -10000000.0
self.STATE_IDLE = 0
self.STATE_NAVIGATE = 1
self.STATE_WAIT = 2
self.state = self.STATE_IDLE
self.state_prev = self.state
self.automode_warn = False
self.wait_after_arrival = 0.0
self.wait_timeout = 0.0
self.status = 0
self.wpt = False
self.prev_wpt = False
self.linear_vel = 0.0
self.angular_vel = 0.0
self.pos = False
self.bearing = False
rospy.loginfo(rospy.get_name() + ": Start")
self.quaternion = np.empty((4, ), dtype=np.float64)
self.wii_a = False
self.wii_a_changed = False
self.wii_plus = False
self.wii_plus_changed = False
self.wii_minus = False
self.wii_minus_changed = False
self.wii_up = False
self.wii_up_changed = False
self.wii_down = False
self.wii_down_changed = False
self.wii_left = False
self.wii_left_changed = False
self.wii_right = False
self.wii_right_changed = False
self.wii_home = False
self.wii_home_changed = False
# 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)
self.pid_publish_interval = rospy.get_param("~pid_publish_interval", 0)
# get topic names
self.automode_topic = rospy.get_param("~automode_sub",
'/fmDecision/automode')
self.pose_topic = rospy.get_param("~pose_sub", '/fmKnowledge/pose')
self.joy_topic = rospy.get_param("~joy_sub", '/fmLib/joy')
self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",
'/fmCommand/cmd_vel')
self.implement_topic = rospy.get_param("~implement_pub",
'/fmCommand/implement')
self.wptnav_status_topic = rospy.get_param(
"~status_pub", '/fmInformation/wptnav_status')
self.pid_topic = rospy.get_param("~pid_pub",
'/fmInformation/wptnav_pid')
# setup publish topics
self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
self.twist = TwistStamped()
self.implement_pub = rospy.Publisher(self.implement_topic,
FloatStamped)
self.implement = FloatStamped()
self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic,
waypoint_navigation_status)
self.wptnav_status = waypoint_navigation_status()
self.status_publish_count = 0
self.pid_pub = rospy.Publisher(self.pid_topic, FloatArrayStamped)
self.pid = FloatArrayStamped()
self.pid_publish_count = 0
# configure waypoint navigation
self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2) # [m]
drive_kp = rospy.get_param("~drive_kp", 1.0)
drive_ki = rospy.get_param("~drive_ki", 0.0)
drive_kd = rospy.get_param("~drive_kd", 0.0)
drive_ff = rospy.get_param("~drive_feed_forward", 0.0)
drive_max_output = rospy.get_param("~drive_max_output", 0.3)
turn_kp = rospy.get_param("~turn_kp", 1.0)
turn_ki = rospy.get_param("~turn_ki", 0.0)
turn_kd = rospy.get_param("~turn_kd", 0.2)
turn_ff = rospy.get_param("~turn_feed_forward", 0.0)
turn_max_output = rospy.get_param("~turn_max_output", 0.5)
max_linear_vel = rospy.get_param("~max_linear_velocity", 0.4)
max_angular_vel = rospy.get_param("~max_angular_velocity", 0.4)
self.wpt_def_tolerance = rospy.get_param("~wpt_default_tolerance", 0.5)
self.wpt_def_drive_vel = rospy.get_param("~wpt_default_drive_velocity",
0.5)
self.wpt_def_turn_vel = rospy.get_param("~wpt_default_turn_velocity",
0.3)
self.wpt_def_wait_after_arrival = rospy.get_param(
"~wpt_default_wait_after_arrival", 0.0)
self.wpt_def_implement = rospy.get_param(
"~wpt_default_implement_command", 0.0)
turn_start_at_heading_err = rospy.get_param(
"~turn_start_at_heading_err", 20.0)
turn_stop_at_heading_err = rospy.get_param("~turn_stop_at_heading_err",
2.0)
ramp_drive_vel_at_dist = rospy.get_param(
"~ramp_drive_velocity_at_distance", 1.0)
ramp_min_drive_vel = rospy.get_param("~ramp_min_drive_velocity", 0.1)
ramp_turn_vel_at_angle = rospy.get_param(
"~ramp_turn_velocity_at_angle", 25.0)
ramp_min_turn_vel = rospy.get_param("~ramp_min_turn_velocity", 0.05)
stop_nav_at_dist = rospy.get_param("~stop_navigating_at_distance", 0.1)
self.wptnav = waypoint_navigation(
self.update_rate, self.w_dist, drive_kp, drive_ki, drive_kd,
drive_ff, drive_max_output, turn_kp, turn_ki, turn_kd, turn_ff,
turn_max_output, max_linear_vel, max_angular_vel,
self.wpt_def_tolerance, self.wpt_def_drive_vel,
self.wpt_def_turn_vel, turn_start_at_heading_err,
turn_stop_at_heading_err, ramp_drive_vel_at_dist,
ramp_min_drive_vel, ramp_turn_vel_at_angle, ramp_min_turn_vel,
stop_nav_at_dist, self.debug)
self.wptlist = waypoint_list()
self.wptlist_loaded = False
# setup subscription topic callbacks
rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
rospy.Subscriber(self.joy_topic, Joy, self.on_joy_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()
reset_timeout = 1.0
# launch node
rospy.init_node('differential_odometry_reset')
# variables
reset_sent = False
reset_time = rospy.get_time() + reset_timeout
# get parameters
easting = rospy.get_param("~easting", '0.0')
northing = rospy.get_param("~northing", '0.0')
heading = rospy.get_param("~heading", '0.0')
# create a publisher
topic_odom_reset = rospy.get_param("~odom_reset_pub", '/fmKnowledge/odometry_reset')
pub = rospy.Publisher(topic_odom_reset, FloatArrayStamped)
# define the reset message
reset_msg = FloatArrayStamped()
reset_msg.header.stamp = rospy.Time.now()
reset_msg.data = [easting, northing, heading*pi/180.0]
# loop until shutdown
while not rospy.is_shutdown() and rospy.get_time() < reset_time:
pub.publish(reset_msg)
rospy.sleep(update_interval)
rospy.loginfo(rospy.get_name() + ": Odometry has been reset to [%.3f %.3f %.1f]" % (easting, northing, heading))
reset_timeout = 1.0
# launch node
rospy.init_node('differential_odometry_reset')
# variables
reset_sent = False
reset_time = rospy.get_time() + reset_timeout
# get parameters
easting = rospy.get_param("~easting", '0.0')
northing = rospy.get_param("~northing", '0.0')
heading = rospy.get_param("~heading", '0.0')
# create a publisher
topic_odom_reset = rospy.get_param("~odom_reset_pub", '/fmInformation/odom_reset')
pub = rospy.Publisher(topic_odom_reset, FloatArrayStamped)
# define the reset message
reset_msg = FloatArrayStamped()
reset_msg.header.stamp = rospy.Time.now()
reset_msg.data = [easting, northing, heading*pi/180.0]
# loop until shutdown
while not rospy.is_shutdown() and rospy.get_time() < reset_time:
pub.publish(reset_msg)
rospy.sleep(update_interval)
rospy.loginfo(rospy.get_name() + ": Odometry has been reset to [%.3f %.3f %.1f]" % (easting, northing, heading))
def __init__(self):
# defines
self.update_rate = 20 # set update frequency [Hz]
self.IMPLEMENT_INVALID = -10000000.0
self.STATE_IDLE = 0
self.STATE_NAVIGATE = 1
self.STATE_WAIT = 2
self.state = self.STATE_IDLE
self.state_prev = self.state
self.automode_warn = False
self.no_route_plan_warn = False
self.wait_after_arrival = 0.0
self.wait_timeout = 0.0
self.status = 0
self.wpt = False
self.prev_wpt = False
self.linear_vel = 0.0
self.angular_vel = 0.0
self.pos = False
self.bearing = False
# HMI defines
self.HMI_ID_DEADMAN = 0
self.HMI_ID_MODE = 1
self.HMI_ID_GOTO_WAYPOINT = 2
self.HMI_MODE_MANUAL = 0
self.HMI_MODE_AUTO = 1
# route point defines
self.ROUTEPT_CMD_DELETE = 0
self.ROUTEPT_CMD_DELETE_THEN_APPEND = 1
self.ROUTEPT_CMD_APPEND = 2
self.ROUTEPT_MODE_PP = 0
self.ROUTEPT_MODE_MCTE = 1
self.ROUTEPT_INVALID_DATA = -1000000
rospy.loginfo(rospy.get_name() + ": Start")
self.quaternion = np.empty((4, ), dtype=np.float64)
# 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)
self.pid_publish_interval = rospy.get_param("~pid_publish_interval", 0)
# get topic names
self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
self.automode_topic = rospy.get_param("~automode_sub",'/fmDecision/automode')
self.hmi_sub_topic = rospy.get_param("~hmi_sub",'/fmDecision/hmi')
self.routept_topic = rospy.get_param("~routept_sub",'/fmPlan/route_point')
self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",'/fmCommand/cmd_vel')
self.implement_topic = rospy.get_param("~implement_pub",'/fmCommand/implement')
self.wptnav_status_topic = rospy.get_param("~status_pub",'/fmInformation/wptnav_status')
self.pid_topic = rospy.get_param("~pid_pub",'/fmInformation/wptnav_pid')
# setup publish topics
self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
self.twist = TwistStamped()
self.implement_pub = rospy.Publisher(self.implement_topic, FloatStamped)
self.implement = FloatStamped()
self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status)
self.wptnav_status = waypoint_navigation_status()
self.status_publish_count = 0
self.pid_pub = rospy.Publisher(self.pid_topic, FloatArrayStamped)
self.pid = FloatArrayStamped()
self.pid_publish_count = 0
# configure waypoint navigation
self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2) # [m]
drive_kp = rospy.get_param("~drive_kp", 1.0)
drive_ki = rospy.get_param("~drive_ki", 0.0)
drive_kd = rospy.get_param("~drive_kd", 0.0)
drive_ff = rospy.get_param("~drive_feed_forward", 0.0)
drive_max_output = rospy.get_param("~drive_max_output", 0.3)
turn_kp = rospy.get_param("~turn_kp", 1.0)
turn_ki = rospy.get_param("~turn_ki", 0.0)
turn_kd = rospy.get_param("~turn_kd", 0.2)
turn_ff = rospy.get_param("~turn_feed_forward", 0.0)
turn_max_output = rospy.get_param("~turn_max_output", 0.5)
max_linear_vel = rospy.get_param("~max_linear_velocity", 0.4)
max_angular_vel = rospy.get_param("~max_angular_velocity", 0.4)
self.wpt_def_tolerance = rospy.get_param("~wpt_default_tolerance", 0.5)
self.wpt_def_drive_vel = rospy.get_param("~wpt_default_drive_velocity", 0.5)
self.wpt_def_turn_vel = rospy.get_param("~wpt_default_turn_velocity", 0.3)
self.wpt_def_wait_after_arrival = rospy.get_param("~wpt_default_wait_after_arrival", 0.0)
self.wpt_def_implement = rospy.get_param("~wpt_default_implement_command", 0.0)
target_ahead = rospy.get_param("~target_ahead", 1.0)
turn_start_at_heading_err = rospy.get_param("~turn_start_at_heading_err", 20.0)
turn_stop_at_heading_err = rospy.get_param("~turn_stop_at_heading_err", 2.0)
ramp_drive_vel_at_dist = rospy.get_param("~ramp_drive_velocity_at_distance", 1.0)
ramp_min_drive_vel = rospy.get_param("~ramp_min_drive_velocity", 0.1)
ramp_turn_vel_at_angle = rospy.get_param("~ramp_turn_velocity_at_angle", 25.0)
ramp_min_turn_vel = rospy.get_param("~ramp_min_turn_velocity", 0.05)
stop_nav_at_dist = rospy.get_param("~stop_navigating_at_distance", 0.1)
self.wptnav = waypoint_navigation(self.update_rate, self.w_dist, drive_kp, drive_ki, drive_kd, drive_ff, drive_max_output, turn_kp, turn_ki, turn_kd, turn_ff, turn_max_output, max_linear_vel, max_angular_vel, self.wpt_def_tolerance, self.wpt_def_drive_vel, self.wpt_def_turn_vel, target_ahead, turn_start_at_heading_err, turn_stop_at_heading_err, ramp_drive_vel_at_dist, ramp_min_drive_vel, ramp_turn_vel_at_angle, ramp_min_turn_vel, stop_nav_at_dist, self.debug)
self.wptlist = waypoint_list()
self. load_wpt_list() # load locally saved waypoint list if it exist
# setup subscription topic callbacks
rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
rospy.Subscriber(self.hmi_sub_topic, StringArrayStamped, self.on_hmi_message)
rospy.Subscriber(self.routept_topic, RoutePt, self.on_routept_message)
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()