def __init__(self):
rospy.init_node('turtlebot_state_machine', anonymous=True)
self.mode = Mode.EXPLORE
# current nav cmd
self.cmd_nav = Pose2D()
# map
self.map = OccupancyGrid()
self.map_meta_data = MapMetaData()
self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max)
self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max)
self.heading_controller = HeadingController(self.kp_th, self.om_max)
# publishers
self.map_pub = rospy.Publisher('/map', OccupancyGrid, queue_size=10)
self.map_meta_data_pub = rospy.Publisher('/map_meta_data', MapMetaData, queue_size=10)
self.cmd_nav_pub = rospy.Publisher('/cmd_nav',Pose2D, queue_size=10)
# subscribers
#rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
#rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
#rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
print "finished init"
def __init__(self):
rospy.init_node('turtlebot_navigator', anonymous=True)
self.mode = Mode.IDLE
# current state
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# goal state
self.x_g = None
self.y_g = None
self.theta_g = None
self.th_init = 0.0
# map parameters
self.map_width = 0
self.map_height = 0
self.map_resolution = 0
self.map_origin = [0, 0]
self.map_probs = []
self.occupancy = None
self.occupancy_updated = False
# plan parameters
self.plan_resolution = 0.1
self.plan_horizon = 15
# time when we started following the plan
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = 0
self.plan_start = [0., 0.]
# Robot limits
self.v_max = 0.2 # maximum velocity
self.om_max = 0.4 # maximum angular velocity
self.v_des = 0.12 # desired cruising velocity
self.theta_start_thresh = 0.05 # threshold in theta to start moving forward when path-following
self.start_pos_thresh = 0.2 # threshold to be far enough into the plan to recompute it
# threshold at which navigator switches from trajectory to pose control
self.near_thresh = 0.2
self.at_thresh = 0.02
self.at_thresh_theta = 0.05
# trajectory smoothing
self.spline_alpha = 0.15
self.traj_dt = 0.1
# trajectory tracking controller parameters
self.kpx = 0.5
self.kpy = 0.5
self.kdx = 1.5
self.kdy = 1.5
# heading controller parameters
self.kp_th = 2.
self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
self.kdy, self.v_max,
self.om_max)
self.pose_controller = PoseController(0., 0., 0., self.v_max,
self.om_max)
self.heading_controller = HeadingController(self.kp_th, self.om_max)
self.nav_planned_path_pub = rospy.Publisher('/planned_path',
Path,
queue_size=10)
self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
Path,
queue_size=10)
self.nav_smoothed_path_rej_pub = rospy.Publisher(
'/cmd_smoothed_path_rejected', Path, queue_size=10)
self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.trans_listener = tf.TransformListener()
self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
# Section 6
print "finished init"
def __init__(self):
rospy.init_node('turtlebot_navigator', anonymous=True)
self.mode = Mode.IDLE
self.mode_at_stop = None
self.x_saved = None
self.y_saved = None
self.theta_saved = None
# current state
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# history tracking of controls
self.history_cnt = 0
self.V_history = np.zeros(CMD_HISTORY_SIZE)
self.om_history = np.zeros(CMD_HISTORY_SIZE)
self.backing_cnt = 0
#laser scans for collision
self.laser_ranges = []
self.laser_angle_increment = 0.01 # this gets updated
self.chunky_radius = 0.1 #TODO: Tune this!
# goal state
self.x_g = None
self.y_g = None
self.theta_g = None
self.th_init = 0.0
self.iters = 0
# map parameters
self.map_width = 0
self.map_height = 0
self.map_resolution = 0
self.map_origin = [0, 0]
self.map_probs = []
self.occupancy = None
self.occupancy_updated = False
# plan parameters
self.plan_resolution = 0.1
self.plan_horizon = 15
# time when we started following the plan
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = 0
self.plan_start = [0., 0.]
# Robot limits
self.v_max = rospy.get_param("~v_max", 0.2) #0.2 # maximum velocity
self.om_max = rospy.get_param("~om_max",
0.4) #0.4 # maximum angular velocity
self.v_des = 0.12 # desired cruising velocity
self.theta_start_thresh = 0.05 # threshold in theta to start moving forward when path-following
self.start_pos_thresh = 0.2 # threshold to be far enough into the plan to recompute it
# threshold at which navigator switches from trajectory to pose control
self.near_thresh = 0.2
self.at_thresh = 0.02
self.at_thresh_theta = (2.0 * np.pi) / 20.0 #0.05
# trajectory smoothing
self.spline_alpha = 0.01
self.traj_dt = 0.1
# trajectory tracking controller parameters
self.kpx = 0.5
self.kpy = 0.5
self.kdx = 1.5
self.kdy = 1.5
# heading controller parameters
self.kp_th = 2.
self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
self.kdy, self.v_max,
self.om_max)
self.pose_controller = PoseController(0., 0., 0., self.v_max,
self.om_max)
self.heading_controller = HeadingController(self.kp_th, self.om_max)
# Publishers
self.nav_planned_path_pub = rospy.Publisher('/planned_path',
Path,
queue_size=10)
self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
Path,
queue_size=10)
self.nav_smoothed_path_rej_pub = rospy.Publisher(
'/cmd_smoothed_path_rejected', Path, queue_size=10)
self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.trans_listener = tf.TransformListener()
self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
#communication with squirtle_fsm to inform goal status
self.publish_squirtle = rospy.Publisher('/post/squirtle_fsm',
String,
queue_size=10)
# Subscriber Constructors
rospy.Subscriber('/post/nav_fsm', Bool,
self.post_callback) #service queue
rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
rospy.Subscriber('/scan', LaserScan, self.laser_callback)
rospy.Subscriber('debug/nav_fsm', String, self.debug_callback)
print "finished init"
def __init__(self):
rospy.init_node('turtlebot_navigator', anonymous=True)
self.mode = Mode.IDLE
self.xlist = []
self.ylist = []
self.tlist = []
self.object_dic = {}
with open('locations.json', 'r') as f:
self.object_dic = json.load(f)
for i in self.object_dic:
print i
cal_points(self.object_dic)
print(point_mat)
# current state
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# goal state
self.x_g = None
self.y_g = None
self.theta_g = None
self.th_init = 0.0
# map parameters
self.map_width = 0
self.map_height = 0
self.map_resolution = 0
self.map_origin = [0, 0]
self.map_probs = []
self.occupancy = None
self.occupancy_updated = False
# plan parameters
self.plan_resolution = 0.1
self.plan_horizon = 15
# time when we started following the plan
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = 0
self.plan_start = [0., 0.]
# Robot limits
self.v_max = 0.2
self.om_max = 0.5 #0.5
self.v_des = 0.12 # desired cruising velocity
self.theta_start_thresh = 0.05 # threshold in theta to start moving forward when path-following
self.start_pos_thresh = 0.2 # threshold to be far enough into the plan to recompute it
# threshold at which navigator switches from trajectory to pose control
self.near_thresh = 0.2
self.at_thresh = 0.02
self.at_thresh_theta = 0.05
# trajectory smoothing
self.spline_alpha = 0.15
self.traj_dt = 0.1
# trajectory tracking controller parameters
self.kpx = 0.5
self.kpy = 0.5
self.kdx = 1.5
self.kdy = 1.5
#try
self.kpx = 0.5
self.kpy = 0.5
self.kdx = 1.5
self.kdy = 1.5
# heading controller parameters
self.kp_th = 2.
self.kp_th = 5. #try
self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
self.kdy, self.v_max,
self.om_max)
self.pose_controller = PoseController(0., 0., 0., self.v_max,
self.om_max)
self.heading_controller = HeadingController(self.kp_th, self.om_max)
self.nav_planned_path_pub = rospy.Publisher('/planned_path',
Path,
queue_size=10)
self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
Path,
queue_size=10)
self.nav_smoothed_path_rej_pub = rospy.Publisher(
'/cmd_smoothed_path_rejected', Path, queue_size=10)
self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.vis_pub = rospy.Publisher('marker_topic', Marker, queue_size=10)
self.trans_listener = tf.TransformListener()
self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
self.stop_min_dist = 1.0
self.stop_time = 3.0
self.crossing_time = 10.0
rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
rospy.Subscriber('/detector/stop_sign', DetectedObject,
self.stop_sign_detected_callback)
rospy.Subscriber('/detector/broccoli', DetectedObject,
self.broccoli_callback)
rospy.Subscriber('/detector/cake', DetectedObject, self.cake_callback)
rospy.Subscriber('/detector/bowl', DetectedObject, self.bowl_callback)
rospy.Subscriber('/detector/banana', DetectedObject,
self.banana_callback)
rospy.Subscriber('/detector/donut', DetectedObject,
self.donut_callback)
rospy.Subscriber('/delivery_request', String, self.delivery_callback)
self.initPos = False
self.auto = False
self.broccoli = 0
self.cake = 1
self.bowl = 2
self.banana = 3
self.donut = 4
self.control = False
print "finished init"
def __init__(self):
rospy.init_node('turtlebot_navigator', anonymous=True)
self.mode = Mode.IDLE
# WE CHANGED THIS STUFF
# Time to stop at a stop sign
self.stop_time = rospy.get_param("~stop_time", 3.)
# Minimum distance from a stop sign to obey it (originally 0.5)
self.stop_min_dist = rospy.get_param("~stop_min_dist", 1.)
# Time taken to cross an intersection
self.crossing_time = rospy.get_param("~crossing_time", 3.)
# Stop sign detector
rospy.Subscriber('/detector/stop_sign', DetectedObject,
self.stop_sign_detected_callback)
rospy.Subscriber('/detector/hot_dog', DetectedObject,
self.hot_dog_detected_callback)
# END OF WE CHANGED THIS STUFF
# current state
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# goal state
self.x_g = None
self.y_g = None
self.theta_g = None
self.th_init = 0.0
# map parameters
self.map_width = 0
self.map_height = 0
self.map_resolution = 0
self.map_origin = [0, 0]
self.map_probs = []
self.occupancy = None
self.occupancy_updated = False
# plan parameters
self.plan_resolution = 0.1
self.plan_horizon = 15
# time when we started following the plan
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = 0
self.plan_start = [0., 0.]
# Robot limits
#self.v_max = 0.2 # maximum velocity
self.v_max = 0.5
#self.om_max = 0.4 # maximum angular velocity
self.om_max = 0.8
self.v_des = 0.12 # desired cruising velocity
self.theta_start_thresh = 0.05 # threshold in theta to start moving forward when path-following
self.start_pos_thresh = 0.2 # threshold to be far enough into the plan to recompute it
# threshold at which navigator switches from trajectory to pose control
self.near_thresh = 0.2
self.at_thresh = 0.02
self.at_thresh_theta = 0.05
# trajectory smoothing
self.spline_alpha = 0.15
self.traj_dt = 0.1
# trajectory tracking controller parameters
self.kpx = 0.5
self.kpy = 0.5
self.kdx = 1.5
self.kdy = 1.5
# heading controller parameters
self.kp_th = 2.
self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
self.kdy, self.v_max,
self.om_max)
self.pose_controller = PoseController(0., 0., 0., self.v_max,
self.om_max)
self.heading_controller = HeadingController(self.kp_th, self.om_max)
self.nav_planned_path_pub = rospy.Publisher('/planned_path',
Path,
queue_size=10)
self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
Path,
queue_size=10)
self.nav_smoothed_path_rej_pub = rospy.Publisher(
'/cmd_smoothed_path_rejected', Path, queue_size=10)
self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.trans_listener = tf.TransformListener()
self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
# ADD CURRENT STATE PUBLISHER (11/15/20 DEM)
self.current_state_pub = rospy.Publisher('/current_state',
Pose2D,
queue_size=10)
print "finished init"
def __init__(self):
rospy.init_node('turtlebot_navigator', anonymous=True)
self.mode = Mode.EXPLORE
self.delivery_locations = {}
self.requests = []
self.vendor_marker_ids = {}
# current state
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# goal state
self.x_g = None
self.y_g = None
self.theta_g = None
self.th_init = 0.0
# map parameters
self.map_width = 0
self.map_height = 0
self.map_resolution = 0
self.map_origin = [0, 0]
self.map_probs = []
self.occupancy = None
self.occupancy_updated = False
# plan parameters
self.plan_resolution = 0.1
self.plan_horizon = 20 # NOTE: Changed this to incraese the plan horizon for testing
# time when we started following the plan
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = 0
self.plan_start = [0., 0.]
# Robot limits
self.v_max = 0.2 # maximum velocity
self.om_max = 0.4 # maximum angular velocity
self.v_des = 0.12 # desired cruising velocity
self.theta_start_thresh = 0.05 # threshold in theta to start moving forward when path-following
self.start_pos_thresh = 0.2 # threshold to be far enough into the plan to recompute it
# threshold at which navigator switches from trajectory to pose control
self.near_thresh = 0.2
self.at_thresh = 0.02
self.at_thresh_theta = 0.05
#self.at_thresh_theta = 0.1
# trajectory smoothing
self.spline_alpha = 0.15
self.traj_dt = 0.1
# trajectory tracking controller parameters
self.kpx = 0.5
self.kpy = 0.5
self.kdx = 1.5
self.kdy = 1.5
# heading controller parameters
self.kp_th = 2.
self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
self.kdy, self.v_max,
self.om_max)
self.pose_controller = PoseController(0., 0., 0., self.v_max,
self.om_max)
self.heading_controller = HeadingController(self.kp_th, self.om_max)
self.nav_planned_path_pub = rospy.Publisher('/planned_path',
Path,
queue_size=10)
self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
Path,
queue_size=10)
self.nav_smoothed_path_rej_pub = rospy.Publisher(
'/cmd_smoothed_path_rejected', Path, queue_size=10)
self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.trans_listener = tf.TransformListener()
self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
#stop sign parameters
self.stop_min_dist = 2
self.stop_sign_roll_start = 0
self.stop_sign_stop_time = 4
self.wait_time = 1
self.first_seen_time = -1
rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
#For the stop sign
rospy.Subscriber('/detector/stop_sign', DetectedObject,
self.stop_sign_detected_callback)
# For ETA rviz markers
self.vis_pub = rospy.Publisher('/eta', Marker, queue_size=10)
# For landmark rviz markers
self.vis_pub = rospy.Publisher('/marker_topic', Marker, queue_size=10)
# Listen to object detector and save locations of interest
rospy.Subscriber('/detector/objects',
DetectedObjectList,
self.detected_objects_callback,
queue_size=10)
rospy.Subscriber('/delivery_request', String, self.request_callback)
print "finished init"
def __init__(self):
rospy.init_node('turtlebot_navigator', anonymous=True)
self.mode = Mode.IDLE
# current state
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# goal state
self.x_g = None
self.y_g = None
self.theta_g = None
self.vendor_queue = []
self.energy = 65
self.th_init = 0.0
# map parameters
self.map_width = 0
self.map_height = 0
self.map_resolution = 0
self.map_origin = [0,0]
self.map_probs = []
self.occupancy = None
self.occupancy_updated = False
self.vendors = {'apple': None, 'banana': None, 'broccoli': None, 'pizza': None, 'donut': None, 'home': None}
#self.vendors = {'apple': (0.27, 1), 'banana': (1, 0.27), 'broccoli': (1.85, 2.8), 'pizza': (1.15, 1.65), 'donut': (2.2, 1.85), 'home': (3.3, 1.6)}
self.intermediate_left = (2.5, 0.3)
self.intermediate_right = (2.5, 2.7)
# plan parameters
self.plan_resolution = 0.1
self.plan_horizon = 15
self.fail_count = 0
# Stop sign related parameters
# Time to stop at a stop sign
self.stop_time = rospy.get_param("~stop_time", 3.)
# Minimum distance from a stop sign to obey it
self.stop_min_dist = rospy.get_param("~stop_min_dist", 0.4)
# Time taken to cross an intersection
self.crossing_time = rospy.get_param("~crossing_time", 3.5)
self.crossing_vel = 0.125
self.vendor_detector_dist = 0.4
self.vendor_time = rospy.get_param("~vendor_stop_time", 5.)
self.vendor_stop_start_time = rospy.get_rostime()
# time when we started following the plan
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = 0
self.plan_start = [0.,0.]
# Robot limits
self.v_max = 0.2 # maximum velocity
self.om_max = 0.4 # maximum angular velocity
#self.v_max = rospy.get_param("~v_max")
#self.om_max = rospy.get_param("~om_max")
self.v_des = 0.12 # desired cruising velocity
self.theta_start_thresh = 0.05 # threshold in theta to start moving forward when path-following
self.start_pos_thresh = 0.2 # threshold to be far enough into the plan to recompute it
# threshold at which navigator switches from trajectory to pose control
self.near_thresh = 0.2
self.at_thresh = 0.02
self.at_thresh_theta = 0.05 # cfg
# trajectory smoothing
self.spline_alpha = 0.1 #need to change in cfg
self.traj_dt = 0.1
# trajectory tracking controller parameters
self.kpx = 0.5
self.kpy = 0.5
self.kdx = 1.5
self.kdy = 1.5
# heading controller parameters
self.kp_th = 2.
self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max)
self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max)
self.heading_controller = HeadingController(self.kp_th, self.om_max)
self.nav_planned_path_pub = rospy.Publisher('/planned_path', Path, queue_size=10)
self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path', Path, queue_size=10)
self.nav_smoothed_path_rej_pub = rospy.Publisher('/cmd_smoothed_path_rejected', Path, queue_size=10)
self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.marker_pub = rospy.Publisher('/vendor_loc', Pose2D, queue_size=10)
self.goal_marker_pub = rospy.Publisher('/goal_loc', Pose2D, queue_size=10)
self.hungry_pub = rospy.Publisher('/hungry', String, queue_size=10)
self.trans_listener = tf.TransformListener()
self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
# Stop sign detector
rospy.Subscriber('/detector/stop_sign', DetectedObject, self.stop_sign_detected_callback)
# Vendor detector
rospy.Subscriber('/detector/objects', DetectedObjectList, self.vendor_detected_callback)
rospy.Subscriber('/delivery_request', String, self.order_callback)
print "finished init"
def __init__(self):
rospy.init_node('turtlebot_navigator', anonymous=True)
self.mode = Mode.IDLE
# current state
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# goal state
self.x_g = None
self.y_g = None
self.theta_g = None
self.th_init = 0.0
# map parameters
self.map_width = 0
self.map_height = 0
self.map_resolution = 0
self.map_origin = [0,0]
self.map_probs = []
self.occupancy = None
self.occupancy_updated = False
# plan parameters
self.plan_resolution = 0.1
self.plan_horizon = 15
# time when we started following the plan
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = 0
self.plan_start = [0.,0.]
# Robot limits
self.v_max = rospy.get_param("~v_max", 0.2) # 0.2 # maximum velocity
self.om_max = rospy.get_param("~om_max", 0.4) # 0.4 # maximum angular velocity
self.v_des = 0.12 # desired cruising velocity
self.theta_start_thresh = 0.05 # threshold in theta to start moving forward when path-following
self.start_pos_thresh = 0.2 # threshold to be far enough into the plan to recompute it
# threshold at which navigator switches from trajectory to pose control
self.near_thresh = 0.2
self.at_thresh = 0.02
self.at_thresh_theta = 0.05
# trajectory smoothing
self.spline_alpha = 0.15
self.traj_dt = 0.1
# trajectory tracking controller parameters
self.kpx = 0.5
self.kpy = 0.5
self.kdx = 1.5
self.kdy = 1.5
# heading controller parameters
self.kp_th = 2.
self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max)
self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max)
self.heading_controller = HeadingController(self.kp_th, self.om_max)
self.nav_planned_path_pub = rospy.Publisher('/planned_path', Path, queue_size=10)
self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path', Path, queue_size=10)
self.nav_smoothed_path_rej_pub = rospy.Publisher('/cmd_smoothed_path_rejected', Path, queue_size=10)
self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.trans_listener = tf.TransformListener()
self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
self.detected_objects = {}
rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
# Food delivery
rospy.Subscriber('/delivery_request', String, self.delivery_request_callback)
rospy.Subscriber('/detector/objects', DetectedObjectList, self.detected_objects_name_callback, queue_size=10)
# Vendor state
self.vendor_start = None # Starting time of vendor state
self.vendor_time = 4.0 # 3-5s stop time
self.gotovendor = False
self.home_location = [3.15,1.6,0.0]
#############################################################
# Stop state
# Time to stop at a stop sign
self.stop_time = rospy.get_param("~stop_time", 5.)
# Minimum distance from a stop sign to obey it
self.stop_min_dist = rospy.get_param("~stop_min_dist", 0.6) # original value 0.5
# Time taken to cross an intersection
self.crossing_time = rospy.get_param("~crossing_time", 3.)
# Stop sign detector
rospy.Subscriber('/detector/stop_sign', DetectedObject, self.stop_sign_detected_callback)
self.stopped = False
self.v = 0.0
self.stop_sign_start = None
#############################################################
print "finished init"
def __init__(self):
rospack = rospkg.RosPack()
package_dir = rospack.get_path("final_project")
self.points_path = package_dir + "/scripts/points.txt"
rospy.init_node('turtlebot_navigator', anonymous=True)
if SAVE_POINTS:
with open(self.points_path, "w") as f:
f.write("x, y, theta\n")
self.mode = Mode.IDLE
# current state
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# goal state
self.x_g = None
self.y_g = None
self.theta_g = None
self.th_init = 0.0
# map parameters
self.map_width = 0
self.map_height = 0
self.map_resolution = 0
self.map_origin = [0,0]
self.map_probs = []
self.occupancy = None
self.occupancy_updated = False
# plan parameters
self.plan_resolution = 0.1
self.plan_horizon = 15
# time when we started following the plan
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = 0
self.plan_start = [0.,0.]
# Robot limits
self.v_max = 0.2 # maximum velocity
self.om_max = 0.4 # maximum angular velocity
self.v_des = 0.12 # desired cruising velocity
self.theta_start_thresh = 0.05 # threshold in theta to start moving forward when path-following
self.start_pos_thresh = 0.2 # threshold to be far enough into the plan to recompute it
# threshold at which navigator switches from trajectory to pose control
self.near_thresh = 0.2
self.at_thresh = 0.02
self.at_thresh_theta = 0.05
# trajectory smoothing
self.spline_alpha = 0.05
self.traj_dt = 0.1
# trajectory tracking controller parameters
self.kpx = 0.5
self.kpy = 0.5
self.kdx = 1.5
self.kdy = 1.5
self.stop_time = 0.0
# heading controller parameters
self.kp_th = 2.
self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max)
self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max)
self.heading_controller = HeadingController(self.kp_th, self.om_max)
self.nav_planned_path_pub = rospy.Publisher('/planned_path', Path, queue_size=10)
self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path', Path, queue_size=10)
self.nav_smoothed_path_rej_pub = rospy.Publisher('/cmd_smoothed_path_rejected', Path, queue_size=10)
self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.nav_mode_pub = rospy.Publisher('/nav_mode', Int16, queue_size=10)
# Publish robot's perception of own state
self.robot_pose_current_pub = rospy.Publisher('/robot/pose/current', Pose2D, queue_size=10)
self.trans_listener = tf.TransformListener()
self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
rospy.Subscriber('/map_inflated', OccupancyGrid, self.map_callback)
rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
rospy.Subscriber('/detector/objects', DetectedObjectList, self.object_detected_callback)
print "finished init"
