def __init__(self):
# parameters not added to launch file
self.implement_raise_time = 2.0
self.implement_lower_time = 3.0
# defines
self.update_rate = 20 # set update frequency [Hz]
self.automode = False
self.automode_prev = False
self.status = 0
self.wpt = False
self.prev_wpt = False
self.linear_speed = 0.0
self.angular_speed = 0.0
self.pos = False
self.bearing = False
self.implement_state = S_IMP_IDLE
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)
# get topic names
self.automode_topic = rospy.get_param("~automode_sub",'/fmDecisionMakers/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.wptnav_status_topic = rospy.get_param("~status_pub",'/fmData/wptnav_status')
# setup publish topics
self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
self.twist = TwistStamped()
self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status)
self.wptnav_status = waypoint_navigation_status()
self.status_publish_count = 0
# configure implement
self.implement_active_pub = rospy.Publisher('/fmCommands/implement_active', Bool)
self.implement_raise_pub = rospy.Publisher('/fmCommands/implement_raise', Bool)
self.implement_lower_pub = rospy.Publisher('/fmCommands/implement_lower', Bool)
self.implement_active = False
self.implement_raise = 0.0
self.implement_lower = 0.0
# configure waypoint navigation
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_integral_max = rospy.get_param("~drive_integral_max", 1.0)
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.0)
turn_integral_max = rospy.get_param("~turn_integral_max", 1.0)
max_linear_velocity = rospy.get_param("~max_linear_velocity", 0.4)
max_angular_velocity = rospy.get_param("~max_angular_velocity", 0.4)
self.wpt_tolerance = rospy.get_param("~wpt_tolerance", 0.5)
wpt_target_distance = rospy.get_param("~wpt_target_distance", 1.0)
wpt_turn_start_at_heading_err = rospy.get_param("~wpt_turn_start_at_heading_err", 20.0)
wpt_turn_stop_at_heading_err = rospy.get_param("~wpt_turn_stop_at_heading_err", 1.0)
self.wpt_linear_velocity = rospy.get_param("~wpt_linear_velocity", 0.5)
wpt_ramp_down_velocity_at_distance = rospy.get_param("~wpt_ramp_down_velocity_at_distance", 1.0)
wpt_ramp_down_minimum_velocity = rospy.get_param("~wpt_ramp_down_minimum_velocity", 0.3)
self.wptnav = waypoint_navigation(self.update_rate, drive_kp, drive_ki, drive_kd, drive_integral_max, turn_kp, turn_ki, turn_kd, turn_integral_max, max_linear_velocity, max_angular_velocity, self.wpt_tolerance, wpt_target_distance, wpt_turn_start_at_heading_err, wpt_turn_stop_at_heading_err, self.wpt_linear_velocity, wpt_ramp_down_velocity_at_distance, wpt_ramp_down_minimum_velocity, self.debug)
# configure casmo
casmo_width = rospy.get_param("~casmo_width", 0.8)
casmo_default_length = rospy.get_param("~casmo_default_length", 20)
self.casmo = area_coverage_casmo()
self.casmo.param_set_width(casmo_width)
self.casmo.param_set_default_length(casmo_default_length)
# 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()
from area_coverage_casmo import area_coverage_casmo
#from math import pi
def print_state(state, pos):
print '%d: %.1f %.1f' % (state, pos[0], pos[1])
# initialize casmo
casmo = area_coverage_casmo()
# set width of the implement (from launch parameter?)
casmo.param_set_width (0.8)
# optionaly set the default length to drive unless the user turns before that
casmo.param_set_default_length (100)
wptlist = []
print 'user pressed up button in auto mode (robot at 0.0 pointing East)'
wptlist.append(casmo.start([0.0, 0.0], 0)) # when switching to auto call start(pos, orientation)
print_state (casmo.state, wptlist[-1])
print 'user pressed left button while at approx (50,1) the ,1 is to illustrate inaccuracy'
wptlist.append(casmo.turn_left ([50.0, 1.0])) # when user presses left call turn_left(pos)
print_state (casmo.state, wptlist[-1])
print 'robot reached first left turn wpt thus requesting the next.'
wptlist.append(casmo.goto_next ()) # whenever robot has reached a waypoint call goto_next()
def __init__(self):
# defines
self.update_rate = 20 # set update frequency [Hz]
self.automode = False
self.automode_prev = False
self.status = 0
self.wpt = False
self.prev_wpt = False
self.linear_speed = 0.0
self.angular_speed = 0.0
self.pos = False
self.bearing = False
self.implement_state = S_IMP_IDLE
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_home = False
self.wii_home_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
# 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.automode_topic = rospy.get_param("~automode_sub",
'/fmDecisionMakers/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.wptnav_status_topic = rospy.get_param("~status_pub",
'/fmData/wptnav_status')
# 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)
# setup publish topics
self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
self.twist = TwistStamped()
self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic,
waypoint_navigation_status)
self.wptnav_status = waypoint_navigation_status()
self.status_publish_count = 0
# configure waypoint navigation
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_integral_max = rospy.get_param("~drive_integral_max", 1.0)
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.0)
turn_integral_max = rospy.get_param("~turn_integral_max", 1.0)
max_linear_velocity = rospy.get_param("~max_linear_velocity", 0.4)
max_angular_velocity = rospy.get_param("~max_angular_velocity", 0.4)
self.wpt_tolerance = rospy.get_param("~wpt_tolerance", 0.5)
wpt_target_distance = rospy.get_param("~wpt_target_distance", 1.0)
wpt_turn_start_at_heading_err = rospy.get_param(
"~wpt_turn_start_at_heading_err", 20.0)
wpt_turn_stop_at_heading_err = rospy.get_param(
"~wpt_turn_stop_at_heading_err", 1.0)
self.wpt_linear_velocity = rospy.get_param("~wpt_linear_velocity", 0.5)
wpt_ramp_down_velocity_at_distance = rospy.get_param(
"~wpt_ramp_down_velocity_at_distance", 1.0)
wpt_ramp_down_minimum_velocity = rospy.get_param(
"~wpt_ramp_down_minimum_velocity", 0.3)
self.wptnav = waypoint_navigation(
self.update_rate, drive_kp, drive_ki, drive_kd, drive_integral_max,
turn_kp, turn_ki, turn_kd, turn_integral_max, max_linear_velocity,
max_angular_velocity, self.wpt_tolerance, wpt_target_distance,
wpt_turn_start_at_heading_err, wpt_turn_stop_at_heading_err,
self.wpt_linear_velocity, wpt_ramp_down_velocity_at_distance,
wpt_ramp_down_minimum_velocity, self.debug)
# configure casmo
casmo_width = rospy.get_param("~casmo_width", 0.8)
casmo_default_length = rospy.get_param("~casmo_default_length", 20)
self.casmo = area_coverage_casmo()
self.casmo.param_set_width(casmo_width)
self.casmo.param_set_default_length(casmo_default_length)
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
# defines
self.update_rate = 20 # set update frequency [Hz]
self.update_count = 0
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)
# configure casmo
casmo_width = rospy.get_param("~casmo_width", 0.8)
casmo_default_length = rospy.get_param("~casmo_default_length", 100)
self.casmo = area_coverage_casmo()
self.casmo.param_set_width(casmo_width)
self.casmo.param_set_default_length(casmo_default_length)
# Configure wriggle
turn_angle_left = rospy.get_param("~wads_turn_angle",pi/6)
turn_angle_right = -turn_angle_left
speed_gain = rospy.get_param("~wads_turn_angular_speed_gain", 5)
sensor_penalty_distance = rospy.get_param("~wads_penalty_distance", 5)
self.wriggle = Wriggle(turn_angle_left, turn_angle_right, self.wpt_def_turn_vel, speed_gain, sensor_penalty_distance)
# Configure wads sensor
self.wads_topic = rospy.get_param("~wads_sub",'/fmData/nmea_from_wads')
self.wads_threshold = rospy.get_param("~wads_threshold", 300)
rospy.Subscriber(self.wads_topic, nmea, self.on_wads_sensor_msg)
self.wads_value = [0.0]*10 # buffer length 10
self.wads_value_updated = False
self.wads_error = True
#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):
# defines
self.update_rate = 20 # set update frequency [Hz]
self.automode = False
self.automode_prev = False
self.status = 0
self.wpt = False
self.prev_wpt = False
self.linear_speed = 0.0
self.angular_speed = 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_home = False
self.wii_home_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
# 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.automode_topic = rospy.get_param("~automode_sub",'/fmDecisionMakers/automode')
self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
self.joy_topic = rospy.get_param("~joy_sub",'/fmHMI/joy')
self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",'/fmCommand/cmd_vel')
self.wptnav_status_topic = rospy.get_param("~status_pub",'/fmData/wptnav_status')
# 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)
# setup publish topics
self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
self.twist = TwistStamped()
self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status)
self.wptnav_status = waypoint_navigation_status()
self.status_publish_count = 0
# configure waypoint navigation and casmo
casmo_width = rospy.get_param("~casmo_width", 0.8)
casmo_default_length = rospy.get_param("~casmo_default_length", 100)
self.max_linear_velocity = rospy.get_param("~max_linear_velocity", 0.5)
self.wpt_tolerance = rospy.get_param("~wpt_tolerance", 0.5)
self.wptnav = waypoint_navigation(self.update_rate, self.debug)
self.casmo = area_coverage_casmo()
self.casmo.param_set_width(casmo_width)
self.casmo.param_set_default_length(casmo_default_length)
# Configure wriggle
turn_angle_left = rospy.get_param("~turn_angle_left",pi/6)
turn_angle_right = rospy.get_param("~turn_angle_right",pi/6 * -1)
turn_max_speed = rospy.get_param("~max_angular_velocity", 0.3)
speed_gain = rospy.get_param("~turn_angular_speed_gain", 5)
sensor_penalty_distance = rospy.get_param("~sensor_penalty_distance", 5)
self.wriggle = Wriggle(turn_angle_left, turn_angle_right, turn_max_speed, speed_gain, sensor_penalty_distance)
# Configure wads sensor
self.wads_topic = rospy.get_param("~wads_sub",'/fmInformation/wads')
self.wads_threshold = rospy.get_param("~wads_threshold", 3)
rospy.Subscriber(self.wads_topic, Float64, self.on_wads_sensor_msg)
self.wads_value = 0.0
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def __init__(self):
# defines
self.update_rate = 20 # set update frequency [Hz]
self.update_count = 0
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)
# configure casmo
casmo_width = rospy.get_param("~casmo_width", 0.8)
casmo_default_length = rospy.get_param("~casmo_default_length", 100)
self.casmo = area_coverage_casmo()
self.casmo.param_set_width(casmo_width)
self.casmo.param_set_default_length(casmo_default_length)
# Configure wriggle
turn_angle_left = rospy.get_param("~wads_turn_angle", pi / 6)
turn_angle_right = -turn_angle_left
speed_gain = rospy.get_param("~wads_turn_angular_speed_gain", 5)
sensor_penalty_distance = rospy.get_param("~wads_penalty_distance", 5)
self.wriggle = Wriggle(turn_angle_left, turn_angle_right,
self.wpt_def_turn_vel, speed_gain,
sensor_penalty_distance)
# Configure wads sensor
self.wads_topic = rospy.get_param("~wads_sub",
'/fmData/nmea_from_wads')
self.wads_threshold = rospy.get_param("~wads_threshold", 300)
rospy.Subscriber(self.wads_topic, nmea, self.on_wads_sensor_msg)
self.wads_value = [0.0] * 10 # buffer length 10
self.wads_value_updated = False
self.wads_error = True
#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()
from area_coverage_casmo import area_coverage_casmo
#from math import pi
def print_state(state, pos):
print '%d: %.1f %.1f' % (state, pos[0], pos[1])
# initialize casmo
casmo = area_coverage_casmo()
# set width of the implement (from launch parameter?)
casmo.param_set_width(0.8)
# optionaly set the default length to drive unless the user turns before that
casmo.param_set_default_length(100)
wptlist = []
print 'user pressed up button in auto mode (robot at 0.0 pointing East)'
wptlist.append(casmo.start(
[0.0, 0.0], 0)) # when switching to auto call start(pos, orientation)
print_state(casmo.state, wptlist[-1])
print 'user pressed left button while at approx (50,1) the ,1 is to illustrate inaccuracy'
wptlist.append(casmo.turn_left(
[50.0, 1.0])) # when user presses left call turn_left(pos)
print_state(casmo.state, wptlist[-1])
print 'robot reached first left turn wpt thus requesting the next.'
wptlist.append(casmo.goto_next()
) # whenever robot has reached a waypoint call goto_next()
