class Navigator:
"""
This node handles point to point turtlebot motion, avoiding obstacles.
It is the sole node that should publish to cmd_vel
"""
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 dyn_cfg_callback(self, config, level):
rospy.loginfo(
"Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}".format(**config))
self.pose_controller.k1 = config["k1"]
self.pose_controller.k2 = config["k2"]
self.pose_controller.k3 = config["k3"]
return config
def cmd_nav_callback(self, data):
"""
loads in goal if different from current goal, and replans
"""
if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
self.x_g = data.x
self.y_g = data.y
self.theta_g = data.theta
self.replan()
def map_md_callback(self, msg):
"""
receives maps meta data and stores it
"""
self.map_width = msg.width
self.map_height = msg.height
self.map_resolution = msg.resolution
self.map_origin = (msg.origin.position.x, msg.origin.position.y)
def map_callback(self, msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
self.occupancy = StochOccupancyGrid2D(
self.map_resolution, self.map_width, self.map_height,
self.map_origin[0], self.map_origin[1], 8, self.map_probs)
if self.x_g is not None:
# if we have a goal to plan to, replan
rospy.loginfo("replanning because of new map")
self.replan() # new map, need to replan
def shutdown_callback(self):
"""
publishes zero velocities upon rospy shutdown
"""
cmd_vel = Twist()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = 0.0
self.nav_vel_pub.publish(cmd_vel)
def near_goal(self):
"""
returns whether the robot is close enough in position to the goal to
start using the pose controller
"""
return linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g
])) < self.near_thresh
def at_goal(self):
"""
returns whether the robot has reached the goal position with enough
accuracy to return to idle state
"""
return (linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g])) <
self.near_thresh and abs(wrapToPi(self.theta - self.theta_g)) <
self.at_thresh_theta)
def aligned(self):
"""
returns whether robot is aligned with starting direction of path
(enough to switch to tracking controller)
"""
return (abs(wrapToPi(self.theta - self.th_init)) <
self.theta_start_thresh)
def close_to_plan_start(self):
return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh
and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)
def snap_to_grid(self, x):
return (self.plan_resolution * round(x[0] / self.plan_resolution),
self.plan_resolution * round(x[1] / self.plan_resolution))
def switch_mode(self, new_mode):
rospy.loginfo("Switching from %s -> %s", self.mode, new_mode)
self.mode = new_mode
def publish_planned_path(self, path, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in path:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_smoothed_path(self, traj, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for i in range(traj.shape[0]):
pose_st = PoseStamped()
pose_st.pose.position.x = traj[i, 0]
pose_st.pose.position.y = traj[i, 1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_control(self):
"""
Runs appropriate controller depending on the mode. Assumes all controllers
are all properly set up / with the correct goals loaded
"""
t = self.get_current_plan_time()
if self.mode == Mode.PARK:
V, om = self.pose_controller.compute_control(
self.x, self.y, self.theta, t)
elif self.mode == Mode.TRACK:
V, om = self.traj_controller.compute_control(
self.x, self.y, self.theta, t)
elif self.mode == Mode.ALIGN:
V, om = self.heading_controller.compute_control(
self.x, self.y, self.theta, t)
else:
V = 0.
om = 0.
cmd_vel = Twist()
cmd_vel.linear.x = V
cmd_vel.angular.z = om
self.nav_vel_pub.publish(cmd_vel)
def get_current_plan_time(self):
t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
return max(0.0, t) # clip negative time to 0
def replan(self):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo(
"Navigator: replanning canceled, waiting for occupancy map.")
self.switch_mode(Mode.IDLE)
return
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
self.plan_start = x_init
x_goal = self.snap_to_grid((self.x_g, self.y_g))
problem = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Navigator: computing navigation plan")
success = problem.solve()
if not success:
rospy.loginfo("Planning failed")
return
rospy.loginfo("Planning Succeeded")
planned_path = problem.path
# Check whether path is too short
if len(planned_path) < 4:
rospy.loginfo("Path too short to track")
self.switch_mode(Mode.PARK)
return
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
self.spline_alpha,
self.traj_dt)
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
)
# Estimate duration of new trajectory
th_init_new = traj_new[0, 2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err / self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr:
rospy.loginfo(
"New plan rejected (longer duration than current plan)")
self.publish_smoothed_path(traj_new,
self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.th_init = traj_new[0, 2]
self.heading_controller.load_goal(self.th_init)
if not self.aligned():
rospy.loginfo("Not aligned with start direction")
self.switch_mode(Mode.ALIGN)
return
rospy.loginfo("Ready to track")
self.switch_mode(Mode.TRACK)
def run(self):
rate = rospy.Rate(10) # 10 Hz
while not rospy.is_shutdown():
# try to get state information to update self.x, self.y, self.theta
try:
(translation, rotation) = self.trans_listener.lookupTransform(
'/map', '/base_footprint', rospy.Time(0))
self.x = translation[0]
self.y = translation[1]
euler = tf.transformations.euler_from_quaternion(rotation)
self.theta = euler[2]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
self.current_plan = []
rospy.loginfo("Navigator: waiting for state info")
self.switch_mode(Mode.IDLE)
print e
pass
# STATE MACHINE LOGIC
# some transitions handled by callbacks
if self.mode == Mode.IDLE:
pass
elif self.mode == Mode.ALIGN:
if self.aligned():
self.current_plan_start_time = rospy.get_rostime()
self.switch_mode(Mode.TRACK)
elif self.mode == Mode.TRACK:
if self.near_goal():
self.switch_mode(Mode.PARK)
elif not self.close_to_plan_start():
rospy.loginfo("replanning because far from start")
self.replan()
elif (rospy.get_rostime() - self.current_plan_start_time
).to_sec() > self.current_plan_duration:
rospy.loginfo("replanning because out of time")
self.replan(
) # we aren't near the goal but we thought we should have been, so replan
elif self.mode == Mode.PARK:
if self.at_goal():
# forget about goal:
self.x_g = None
self.y_g = None
self.theta_g = None
self.switch_mode(Mode.IDLE)
self.publish_control()
self.pose_controller.publish() # For section 6
rate.sleep()
class Navigator:
"""
This node handles point to point turtlebot motion, avoiding obstacles.
It is the sole node that should publish to cmd_vel
"""
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 startPlan(self):
for i in range(5):
if i == self.broccoli:
print i
print "broccoli"
if 'broccoli' in self.object_dic:
x, y, t = self.object_dic['broccoli']
self.xlist.append(x)
self.ylist.append(y)
self.tlist.append(t)
elif i == self.cake:
print i
print "cake"
if 'cake' in self.object_dic:
x, y, t = self.object_dic['cake']
self.xlist.append(x)
self.ylist.append(y)
self.tlist.append(t)
elif i == self.bowl:
print i
print "bowl"
if 'bowl' in self.object_dic:
x, y, t = self.object_dic['bowl']
self.xlist.append(x)
self.ylist.append(y)
self.tlist.append(t)
elif i == self.banana:
print i
print "banana"
if 'banana' in self.object_dic:
x, y, t = self.object_dic['banana']
self.xlist.append(x)
self.ylist.append(y)
self.tlist.append(t)
elif i == self.donut:
print i
print "donut"
if 'donut' in self.object_dic:
x, y, t = self.object_dic['donut']
self.xlist.append(x)
self.ylist.append(y)
self.tlist.append(t)
print 4
print "Destination"
x, y, t = self.object_dic['start']
self.xlist.append(x)
self.ylist.append(y)
self.tlist.append(t)
def show(self, idx, x, y, r, g, b, t):
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time()
marker.id = idx
marker.type = t # sphere
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = 0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = r
marker.color.g = g
marker.color.b = b
self.vis_pub.publish(marker)
def delivery_callback(self, msg):
obj = msg.data
destination_list = obj.split(',')
for dest in destination_list:
if dest in self.object_dic:
x, y, t = self.object_dic[dest]
if x in self.xlist or x == self.x:
print "Request already taken"
else:
print "New request: " + dest
n = len(self.xlist)
self.xlist.append(x)
self.ylist.append(y)
self.tlist.append(t)
key = self.object_dic.keys()
# for k in key:
# point_mat[(k, dest)] = np.sqrt((x - self.object_dic[k][0])**2 + (y - self.object_dic[k][1])**2)
def helper(lst):
if len(lst) == 1:
return [lst]
res = []
for i in range(len(lst)):
lst_del = lst[:i] + lst[i + 1:]
pre_res = helper(lst_del)
for r in pre_res:
res.append(r + [lst[i]])
print(res)
return res
pts_lst = destination_list[:-1]
route = helper(pts_lst) # possible route index
costs = []
for i in range(len(route)):
route[i] = [destination_list[-1]
] + route[i] + [destination_list[-1]]
cost = 0
for j in range(len(route[i]) - 1):
cost += point_mat[(route[i][j], route[i][j + 1])]
costs.append(cost)
opt_route = route[costs.index(min(costs))]
print(costs)
print(route)
#print(opt_route)
xlist, ylist, tlist = [], [], []
for i in range(len(opt_route)):
xlist.append(self.object_dic[opt_route[i]][0])
ylist.append(self.object_dic[opt_route[i]][1])
tlist.append(self.object_dic[opt_route[i]][2])
self.xlist = xlist
self.ylist = ylist
self.tlist = tlist
def broccoli_callback(self, msg):
self.object_dic['broccoli'] = (self.x, self.y, self.theta)
print 'broccoli'
print self.object_dic['broccoli']
self.show(0, self.x, self.y, 1.0, 0.0, 0.0, 2)
def cake_callback(self, msg):
self.object_dic['cake'] = (self.x, self.y, self.theta)
print 'cake'
print self.object_dic['cake']
self.show(1, self.x, self.y, 0.0, 1.0, 0.0, 2)
def bowl_callback(self, msg):
self.object_dic['bowl'] = (self.x, self.y, self.theta)
print 'bowl'
print self.object_dic['bowl']
self.show(2, self.x, self.y, 0.0, 0.0, 1.0, 2)
def banana_callback(self, msg):
self.object_dic['banana'] = (self.x, self.y, self.theta)
print 'banana'
print self.object_dic['banana']
self.show(3, self.x, self.y, 0.0, 1.0, 1.0, 2)
def donut_callback(self, msg):
self.object_dic['donut'] = (self.x, self.y, self.theta)
print 'donut'
print self.object_dic['donut']
self.show(4, self.x, self.y, 1.0, 1.0, 0.0, 2)
def stop_sign_detected_callback(self, msg):
""" callback for when the detector has found a stop sign. Note that
a distance of 0 can mean that the lidar did not pickup the stop sign at all """
# distance of the stop sign
# print "Stop Sign Destected"
dist = msg.distance
# if self.mode==Mode.TRACK:
# if close enough and in nav mode, stop
if dist > 0 and dist < self.stop_min_dist and self.mode == Mode.TRACK:
print "Stop Sign Countdown"
self.init_stop_sign()
def init_stop_sign(self):
""" initiates a stop sign maneuver """
self.stop_sign_start = rospy.get_rostime()
self.mode = Mode.STOP
# print "initial stop sign"
# print self.stop_sign_start
def has_stopped(self):
""" checks if stop sign maneuver is over """
return self.mode == Mode.STOP and \
rospy.get_rostime() - self.stop_sign_start > rospy.Duration.from_sec(self.stop_time)
def init_crossing(self):
""" initiates an intersection crossing maneuver """
self.cross_start = rospy.get_rostime()
self.mode = Mode.CROSS
# print "Start crossing"
# print self.cross_start
def has_crossed(self):
""" checks if crossing maneuver is over """
return self.mode == Mode.CROSS and \
rospy.get_rostime() - self.cross_start > rospy.Duration.from_sec(self.crossing_time)
def dyn_cfg_callback(self, config, level):
# rospy.loginfo("Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3},vm:{vm},sa:{sa},td:{td}".format(**config))
self.pose_controller.k1 = config["k1"]
self.pose_controller.k2 = config["k2"]
self.pose_controller.k3 = config["k3"]
self.v_max = config["vm"]
self.spline_alpha = config["sa"]
self.traj_dt = config["td"]
self.auto = config["auto"]
self.broccoli = config["broccoli"]
self.cake = config["cake"]
self.bowl = config["bowl"]
self.banana = config["banana"]
self.donut = config["donut"]
return config
def cmd_nav_callback(self, data):
"""
loads in goal if different from current goal, and replans
"""
if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
self.x_g = data.x
self.y_g = data.y
self.theta_g = data.theta
self.replan()
# print(self.x_g,self.y_g,self.theta_g)
def map_md_callback(self, msg):
"""
receives maps meta data and stores it
"""
self.map_width = msg.width
self.map_height = msg.height
self.map_resolution = msg.resolution
self.map_origin = (msg.origin.position.x, msg.origin.position.y)
def map_callback(self, msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
self.occupancy = StochOccupancyGrid2D(self.map_resolution,
self.map_width,
self.map_height,
self.map_origin[0],
self.map_origin[1],
16,
self.map_probs,
thresh=0.3)
if self.x_g is not None:
# if we have a goal to plan to, replan
# rospy.loginfo("replanning because of new map")
self.replan() # new map, need to replan
def shutdown_callback(self):
"""
publishes zero velocities upon rospy shutdown
"""
cmd_vel = Twist()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = 0.0
self.nav_vel_pub.publish(cmd_vel)
def near_goal(self):
"""
returns whether the robot is close enough in position to the goal to
start using the pose controller
"""
return linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g
])) < self.near_thresh
def at_goal(self):
"""
returns whether the robot has reached the goal position with enough
accuracy to return to idle state
"""
return (linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g])) <
self.near_thresh and abs(wrapToPi(self.theta - self.theta_g)) <
self.at_thresh_theta)
def aligned(self):
"""
returns whether robot is aligned with starting direction of path
(enough to switch to tracking controller)
"""
return (abs(wrapToPi(self.theta - self.th_init)) <
self.theta_start_thresh)
def close_to_plan_start(self):
return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh
and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)
def snap_to_grid(self, x):
return (self.plan_resolution * round(x[0] / self.plan_resolution),
self.plan_resolution * round(x[1] / self.plan_resolution))
def switch_mode(self, new_mode):
rospy.loginfo("Switching from %s -> %s", self.mode, new_mode)
self.mode = new_mode
def publish_planned_path(self, path, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in path:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_smoothed_path(self, traj, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for i in range(traj.shape[0]):
pose_st = PoseStamped()
pose_st.pose.position.x = traj[i, 0]
pose_st.pose.position.y = traj[i, 1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_control(self):
"""
Runs appropriate controller depending on the mode. Assumes all controllers
are all properly set up / with the correct goals loaded
"""
t = self.get_current_plan_time()
if self.mode == Mode.PARK:
V, om = self.pose_controller.compute_control(
self.x, self.y, self.theta, t)
elif self.mode == Mode.TRACK or self.mode == Mode.CROSS:
V, om = self.traj_controller.compute_control(
self.x, self.y, self.theta, t)
elif self.mode == Mode.ALIGN:
V, om = self.heading_controller.compute_control(
self.x, self.y, self.theta, t)
else:
V = 0.
om = 0.
cmd_vel = Twist()
cmd_vel.linear.x = V
cmd_vel.angular.z = om
self.nav_vel_pub.publish(cmd_vel)
def get_current_plan_time(self):
t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
return max(0.0, t) # clip negative time to 0
def replan(self):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo(
"Navigator: replanning canceled, waiting for occupancy map.")
self.switch_mode(Mode.IDLE)
return
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
self.plan_start = x_init
x_goal = self.snap_to_grid((self.x_g, self.y_g))
problem = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Navigator: computing navigation plan")
success = problem.solve()
if not success:
rospy.loginfo("Planning failed")
return
rospy.loginfo("Planning Succeeded")
planned_path = problem.path
# Check whether path is too short
if len(planned_path) < 4:
rospy.loginfo("Path too short to track")
self.switch_mode(Mode.PARK)
return
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
self.spline_alpha,
self.traj_dt)
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
)
# Estimate duration of new trajectory
th_init_new = traj_new[0, 2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err / self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr:
# rospy.loginfo("New plan rejected (longer duration than current plan)")
self.publish_smoothed_path(traj_new,
self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.th_init = traj_new[0, 2]
self.heading_controller.load_goal(self.th_init)
if self.mode == Mode.STOP or self.mode == Mode.CROSS:
return
if not self.aligned():
rospy.loginfo("Not aligned with start direction")
self.switch_mode(Mode.ALIGN)
return
rospy.loginfo("Ready to track")
self.switch_mode(Mode.TRACK)
def run(self):
rate = rospy.Rate(10) # 10 Hz
while not rospy.is_shutdown():
# try to get state information to update self.x, self.y, self.theta
try:
(translation, rotation) = self.trans_listener.lookupTransform(
'/map', '/base_footprint', rospy.Time(0))
self.x = translation[0]
self.y = translation[1]
euler = tf.transformations.euler_from_quaternion(rotation)
self.theta = euler[2]
if self.initPos == False:
self.object_dic['start'] = (self.x, self.y, self.theta)
print 'start position'
print self.object_dic['start']
self.initPos = True
self.show(5, self.x, self.y, 1.0, 0.0, 1.0, 1)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
self.current_plan = []
# rospy.loginfo("Navigator: waiting for state info")
self.switch_mode(Mode.IDLE)
print e
pass
# STATE MACHINE LOGIC
# some transitions handled by callbacks
if self.mode == Mode.IDLE:
if self.auto:
self.control = True
print "Delivery start"
self.startPlan()
self.auto = False
if self.xlist:
print "Read next goal"
time.sleep(3)
self.x_g = self.xlist.pop(0)
self.y_g = self.ylist.pop(0)
self.theta_g = self.tlist.pop(0)
self.replan()
else:
pass
elif self.mode == Mode.ALIGN:
if self.aligned():
self.current_plan_start_time = rospy.get_rostime()
self.switch_mode(Mode.TRACK)
elif self.mode == Mode.TRACK:
if self.near_goal():
self.switch_mode(Mode.PARK)
elif not self.close_to_plan_start():
# rospy.loginfo("replanning because far from start")
self.replan()
elif (rospy.get_rostime() - self.current_plan_start_time
).to_sec() > self.current_plan_duration:
# rospy.loginfo("replanning because out of time")
self.replan(
) # we aren't near the goal but we thought we should have been, so replan
elif self.mode == Mode.PARK:
if self.at_goal():
# forget about goal:
self.x_g = None
self.y_g = None
self.theta_g = None
self.switch_mode(Mode.IDLE)
elif self.mode == Mode.STOP:
if self.has_stopped():
print rospy.get_rostime()
self.init_crossing()
elif self.mode == Mode.CROSS:
# Crossing an intersection
if self.has_crossed():
print rospy.get_rostime()
self.mode = Mode.TRACK
self.publish_control()
rate.sleep()
class Navigator:
"""
This node handles point to point turtlebot motion, avoiding obstacles.
It is the sole node that should publish to cmd_vel
"""
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"
#------------------------------------------------------------------
# Subscriber Callbacks
#------------------------------------------------------------------
#for in terminal debug
def debug_callback(self, msg):
if msg.data == "query_goal":
print('[NAV DEBUG]: The goal is: %f, %f, %f' %
(self.x_g, self.y_g, self.theta_g))
elif msg.data == "query_state":
print("[NAV DEBUG]: The current state is: %s" % str(self.mode))
elif msg.data == "why_u_do_dis?":
print("[NAV DEBUG]: The current position delta is: %f, %f, %f" %
(self.x_g - self.x, self.y_g - self.y,
self.theta_g - self.theta))
else:
print("[NAV DEBUG]: Invalid debug message")
#for the interface topic between nav and supervisor
def post_callback(self, data):
rospy.loginfo("Received from interface topic")
# received true = stop
if data.data is True:
# store current goal
self.x_saved = self.x_g
self.y_saved = self.y_g
self.theta_saved = self.theta_g
# set goal to nothing
self.x_g = None
self.y_g = None
self.theta_g = None
#put machine back into idle
self.switch_mode(Mode.IDLE)
return
# for getting the laser data
def laser_callback(self, msg):
""" callback for thr laser rangefinder """
self.laser_ranges = msg.ranges
self.laser_angle_increment = msg.angle_increment
def dyn_cfg_callback(self, config, level):
rospy.loginfo(
"Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}, spline_alpha:{spline_alpha}"
.format(**config))
self.pose_controller.k1 = config["k1"]
self.pose_controller.k2 = config["k2"]
self.pose_controller.k3 = config["k3"]
self.spline_alpha = config["spline_alpha"]
self.traj_controller.kpx = config["kpx"]
self.traj_controller.kpy = config["kpy"]
self.traj_controller.kdx = config["kdx"]
self.traj_controller.kdy = config["kdy"]
self.traj_controller.V_max = config["V_max"]
self.traj_controller.om_max = config["om_max"]
self.chunky_radius = config["chunky_radius"]
return config
def cmd_nav_callback(self, data):
"""
loads in goal if different from current goal, and replans
"""
if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
self.x_g = data.x
self.y_g = data.y
self.theta_g = data.theta
self.replan()
#tell squirtle we are no longer at the goal
#msg = String()
#msg.data = "not_at_goal"
#self.publish_squirtle.publish(msg)
def map_md_callback(self, msg):
"""
receives maps meta data and stores it
"""
self.map_width = msg.width
self.map_height = msg.height
self.map_resolution = msg.resolution
self.map_origin = (msg.origin.position.x, msg.origin.position.y)
def map_callback(self, msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
self.occupancy = StochOccupancyGrid2D(
self.map_resolution, self.map_width, self.map_height,
self.map_origin[0], self.map_origin[1], 8, self.map_probs)
if self.x_g is not None:
# if we have a goal to plan to, replan
rospy.loginfo("replanning because of new map")
self.replan() # new map, need to replan
def shutdown_callback(self):
"""
publishes zero velocities upon rospy shutdown
"""
cmd_vel = Twist()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = 0.0
self.nav_vel_pub.publish(cmd_vel)
def near_goal(self):
"""
returns whether the robot is close enough in position to the goal to
start using the pose controller
"""
return linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g
])) < self.near_thresh
def at_goal(self):
"""
returns whether the robot has reached the goal position with enough
accuracy to return to idle state
"""
# if the goal is none, it's already there you dumb Winnebago
if self.x_g is None:
return True
return (linalg.norm(np.array(
[self.x - self.x_g, self.y - self.y_g])) < self.at_thresh and abs(
wrapToPi(self.theta - self.theta_g)) < self.at_thresh_theta)
def aligned(self):
"""
returns whether robot is aligned with starting direction of path
(enough to switch to tracking controller)
"""
return (abs(wrapToPi(self.theta - self.th_init)) <
self.theta_start_thresh)
def close_to_plan_start(self):
return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh
and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)
def snap_to_grid(self, x):
return (self.plan_resolution * round(x[0] / self.plan_resolution),
self.plan_resolution * round(x[1] / self.plan_resolution))
def switch_mode(self, new_mode):
rospy.loginfo("Switching from %s -> %s", self.mode, new_mode)
self.mode = new_mode
#------------------------------------------------------------------
# Publishers
#------------------------------------------------------------------
def publish_planned_path(self, path, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in path:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_smoothed_path(self, traj, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for i in range(traj.shape[0]):
pose_st = PoseStamped()
pose_st.pose.position.x = traj[i, 0]
pose_st.pose.position.y = traj[i, 1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_control(self):
"""
Runs appropriate controller depending on the mode. Assumes all controllers
are all properly set up / with the correct goals loaded
"""
#-------------------------------------
# helper functions for LIFO queue
#-------------------------------------
def enQ_buffer(V_in, om_in):
#rospy.loginfo("Enqueuing controls")
# roll right 1 [n]->[n+1]
self.V_history = np.roll(self.V_history, 1, axis=None)
self.om_history = np.roll(self.om_history, 1, axis=None)
# load in the value
self.V_history[0] = V_in
self.om_history[0] = om_in
def deQ_buffer():
#rospy.loginfo("Dequeuing controls")
# get first value out
V_out = -1.0 * self.V_history[0]
om_out = -1.0 * self.om_history[0]
# roll left 1 [n]<-[n+1]
self.V_history = np.roll(self.V_history, -1, axis=None)
self.om_history = np.roll(self.om_history, -1, axis=None)
return V_out, om_out
t = self.get_current_plan_time()
if self.mode == Mode.PARK:
V, om = self.pose_controller.compute_control(
self.x, self.y, self.theta, t)
elif self.mode == Mode.TRACK:
V, om = self.traj_controller.compute_control(
self.x, self.y, self.theta, t)
elif self.mode == Mode.ALIGN:
V, om = self.heading_controller.compute_control(
self.x, self.y, self.theta, t)
elif self.mode == Mode.BACKING_UP:
#extract elements from the queue
V, om = deQ_buffer()
else:
V = 0.
om = 0.
#enqueue for history tracking
if self.mode is not Mode.BACKING_UP:
enQ_buffer(V, om)
cmd_vel = Twist()
cmd_vel.linear.x = V
cmd_vel.angular.z = om
self.nav_vel_pub.publish(cmd_vel)
def get_current_plan_time(self):
t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
return max(0.0, t) # clip negative time to 0
def replan(self):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo(
"Navigator: replanning canceled, waiting for occupancy map.")
self.switch_mode(Mode.IDLE)
return
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
self.plan_start = x_init
x_goal = self.snap_to_grid((self.x_g, self.y_g))
problem = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Navigator: computing navigation plan")
success = problem.solve()
if not success:
rospy.loginfo("Planning failed")
return
rospy.loginfo("Planning Succeeded")
'''
self.iters = 0
success = False
while not success:
success = problem.solve()
print("Ran Solve")
if not success and not self.at_goal():
if self.iters < 5 and self.theta_g is not None:
rospy.loginfo("Planning Iteration: %d", self.iters)
rospy.loginfo("Planning failed, adjusting heading and retrying")
#increment heading
self.theta_g = wrapToPi(self.theta_g + np.pi/8.0)
rospy.loginfo("New heading: %f", self.theta_g)
#increment count
self.iters += 1
else:
rospy.loginfo("Planning failed")
return
rospy.loginfo("Planning Succeeded")
'''
planned_path = problem.path
# Check whether path is too short
if self.at_goal():
rospy.loginfo("Path already at goal pose")
self.switch_mode(Mode.IDLE)
return
elif len(planned_path) < 4:
rospy.loginfo("Path too short to track")
self.switch_mode(Mode.PARK)
return
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
self.spline_alpha,
self.traj_dt)
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
)
# Estimate duration of new trajectory
th_init_new = traj_new[0, 2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err / self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr:
rospy.loginfo(
"New plan rejected (longer duration than current plan)")
self.publish_smoothed_path(traj_new,
self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.th_init = traj_new[0, 2]
self.heading_controller.load_goal(self.th_init)
if not self.aligned():
rospy.loginfo("Not aligned with start direction")
self.switch_mode(Mode.ALIGN)
return
rospy.loginfo("Ready to track")
self.switch_mode(Mode.TRACK)
def run(self):
rate = rospy.Rate(10) # 10 Hz
while not rospy.is_shutdown():
# try to get state information to update self.x, self.y, self.theta
try:
(translation, rotation) = self.trans_listener.lookupTransform(
'/map', '/base_footprint', rospy.Time(0))
self.x = translation[0]
self.y = translation[1]
euler = tf.transformations.euler_from_quaternion(rotation)
self.theta = euler[2]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
self.current_plan = []
rospy.loginfo("Navigator: waiting for state info")
self.switch_mode(Mode.IDLE)
print e
pass
"""
def if_about_to_hit_wall():
# check if inflated turtlebot circumference will hit the wall
#th_offset = np.pi/8.0
#arr = np.arange(wrapToPi(self.theta-th_offset), wrapToPi(self.theta+th_offset), 0.02)
arr = np.arange(0, 2*np.pi, 0.02)
for i in range(len(arr)):
if not self.occupancy.is_free(np.array([self.x + np.cos(arr[i])*self.chunky_radius,self.y+np.sin(arr[i])*self.chunky_radius])):
return True
return False
#return not self.occupancy.is_free(np.array([self.x ,self.y]))
"""
def if_about_to_hit_wall(laserRanges):
#initialize return value to false
returnFlag = False
#remove the zeros
#validRanges = [i for i, dist in enumerate(laserRanges) if dist != 0]
#check the minimum scan distance
#rospy.loginfo(laserRanges)
minScanDist = min(laserRanges)
#rospy.loginfo("Minimum Scan Distance: %f", minScanDist)
#see if less that our boy's fat body
if minScanDist < self.chunky_radius:
#set flag to true
returnFlag = True
#return the flag
return returnFlag
# STATE MACHINE LOGIC
# some transitions handled by callbacks
if self.mode == Mode.IDLE:
pass
elif self.mode == Mode.ALIGN:
if self.aligned():
self.current_plan_start_time = rospy.get_rostime()
self.switch_mode(Mode.TRACK)
elif self.mode == Mode.TRACK:
if if_about_to_hit_wall(self.laser_ranges):
rospy.loginfo("About to hit a wall")
self.switch_mode(Mode.BACKING_UP)
elif self.near_goal():
self.switch_mode(Mode.PARK)
elif not self.close_to_plan_start():
rospy.loginfo("replanning because far from start")
self.replan()
elif (rospy.get_rostime() - self.current_plan_start_time
).to_sec() > self.current_plan_duration:
rospy.loginfo("replanning because out of time")
self.replan(
) # we aren't near the goal but we thought we should have been, so replan
elif self.mode == Mode.PARK:
#if if_about_to_hit_wall():
# self.switch_mode(Mode.BACKING_UP)
if self.at_goal():
# forget about goal:
self.x_g = None
self.y_g = None
self.theta_g = None
self.switch_mode(Mode.IDLE)
#tell squirtle we are at the goal
msg = String()
msg.data = "at_goal"
self.publish_squirtle.publish(msg)
elif self.mode == Mode.BACKING_UP:
# see if we have backed enough counts
if (self.backing_cnt > CMD_HISTORY_SIZE
): # or if_about_to_hit_wall(self.laser_ranges):
# NUKE EVERYTHING!!!!!!!!!
self.history_cnt = 0
self.V_history = np.zeros(CMD_HISTORY_SIZE)
self.om_history = np.zeros(CMD_HISTORY_SIZE)
self.backing_cnt = 0
self.replan() #switches mode internally
#self.switch_mode(Mode.TRACK)
else:
# increment count
self.backing_cnt += 1
self.publish_control()
rate.sleep()
class Navigator:
"""
This class handles point to point turtlebot motion, avoiding obstacles.
It is the sole node that should publish to cmd_vel
"""
def __init__(self):
# 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)
self.new_cmd_nav = False
self.new_map = False
def dyn_cfg_callback(self, config, level):
rospy.loginfo(
"Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}".format(**config))
self.pose_controller.k1 = config["k1"]
self.pose_controller.k2 = config["k2"]
self.pose_controller.k3 = config["k3"]
return config
def cmd_nav_callback(self, data):
"""
loads in goal if different from current goal, and replans
"""
if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
self.x_g = data.x
self.y_g = data.y
self.theta_g = data.theta
self.new_cmd_nav = True
def map_md_callback(self, msg):
"""
receives maps meta data and stores it
"""
self.map_width = msg.width
self.map_height = msg.height
self.map_resolution = msg.resolution
self.map_origin = (msg.origin.position.x, msg.origin.position.y)
def map_callback(self, msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
self.occupancy = StochOccupancyGrid2D(
self.map_resolution, self.map_width, self.map_height,
self.map_origin[0], self.map_origin[1], 8, self.map_probs)
self.new_map = True
def near_goal(self):
"""
returns whether the robot is close enough in position to the goal to
start using the pose controller
"""
return linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g
])) < self.near_thresh
def at_goal(self):
"""
returns whether the robot has reached the goal position with enough
accuracy to return to idle state
"""
return (linalg.norm(np.array(
[self.x - self.x_g, self.y - self.y_g])) < self.at_thresh and abs(
wrapToPi(self.theta - self.theta_g)) < self.at_thresh_theta)
def aligned(self):
"""
returns whether robot is aligned with starting direction of path
(enough to switch to tracking controller)
"""
return (abs(wrapToPi(self.theta - self.th_init)) <
self.theta_start_thresh)
def close_to_plan_start(self):
return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh
and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)
def snap_to_grid(self, x):
return (self.plan_resolution * round(x[0] / self.plan_resolution),
self.plan_resolution * round(x[1] / self.plan_resolution))
def publish_planned_path(self, path, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in path:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_smoothed_path(self, traj, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for i in range(traj.shape[0]):
pose_st = PoseStamped()
pose_st.pose.position.x = traj[i, 0]
pose_st.pose.position.y = traj[i, 1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_control(self):
"""
Runs appropriate controller depending on the mode. Assumes all controllers
are all properly set up / with the correct goals loaded
"""
t = self.get_current_plan_time()
V, om = self.traj_controller.compute_control(self.x, self.y,
self.theta, t)
cmd_vel = Twist()
cmd_vel.linear.x = V
cmd_vel.angular.z = om
self.nav_vel_pub.publish(cmd_vel)
def get_current_plan_time(self):
t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
return max(0.0, t) # clip negative time to 0
# ============Added to take in delivery request================
def detected_objects_name_callback(self, msg):
rospy.loginfo("There are %i detected objects" % len(msg.objects))
#self.detected_objects = msg
for obj in msg.objects:
rospy.loginfo("obj1: %s" % msg.objects[0])
self.detected_objects[obj] = (self.x, self.y, self.theta)
self.last_box_time = rospy.get_rostime()
def delivery_request_callback(self, msg):
if msg.data in self.detected_objects:
rospy.loginfo("New order: %s, set goal: %s" %
(msg.data, str(self.detected_objects[msg.data])))
self.x_g, self.y_g, self.theta_g = self.detected_objects[msg.data]
self.switch_mode(Mode.ALIGN)
self.replan()
else:
rospy.loginfo("New order: %s, no goal found. " % (msg.data))
self.last_box_time = rospy.get_rostime()
class Navigator:
"""
This node handles point to point turtlebot motion, avoiding obstacles.
It is the sole node that should publish to cmd_vel
"""
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 dyn_cfg_callback(self, config, level):
rospy.loginfo(
"NAVIGATOR: Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}".format(
**config))
self.pose_controller.k1 = config["k1"]
self.pose_controller.k2 = config["k2"]
self.pose_controller.k3 = config["k3"]
return config
def cmd_nav_callback(self, data):
"""
loads in goal if different from current goal, and replans
"""
if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
self.x_g = data.x
self.y_g = data.y
self.theta_g = data.theta
self.replan()
def map_md_callback(self, msg):
"""
receives maps meta data and stores it
"""
self.map_width = msg.width
self.map_height = msg.height
self.map_resolution = msg.resolution
self.map_origin = (msg.origin.position.x, msg.origin.position.y)
def map_callback(self, msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
#self.occupancy = StochOccupancyGrid2D(self.map_resolution,
#self.map_width,
#self.map_height,
#self.map_origin[0],
#self.map_origin[1],
#8,
#self.map_probs)
self.occupancy = StochOccupancyGrid2D(
self.map_resolution,
self.map_width,
self.map_height,
self.map_origin[0],
self.map_origin[1],
8, # NOTE: Made the window size larger
self.map_probs) # NOTE: Made the probability lower
if self.x_g is not None:
# if we have a goal to plan to, replan
rospy.loginfo("NAVIGATOR: replanning because of new map")
self.replan() # new map, need to replan
def shutdown_callback(self):
"""
publishes zero velocities upon rospy shutdown
"""
cmd_vel = Twist()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = 0.0
self.nav_vel_pub.publish(cmd_vel)
def stop_sign_detected_callback(self, msg):
""" callback for when the detector has found a stop sign. Note that
a distance of 0 can mean that the lidar did not pickup the stop sign at all """
# distance of the stop sign
dist = msg.distance
rospy.loginfo("Detected stop sign")
rospy.loginfo(dist)
if dist > 0 and dist < self.stop_min_dist and self.mode == Mode.TRACK and msg.confidence > OBJECT_CONFIDENCE_THESH:
if self.first_seen_time == -1:
self.first_seen_time = rospy.get_rostime()
elif rospy.get_rostime(
) - self.first_seen_time > rospy.Duration.from_sec(
self.wait_time) and self.mode == Mode.TRACK:
rospy.loginfo("Initializing roll and changing v_max and v_des")
self.init_stop_sign()
def calculate_bounding_box_area(self, corners):
ymin, xmin, ymax, xmax = corners
return (ymax - ymin) * (xmax - xmin)
def detected_objects_callback(self, msg):
# Iterate through all of the objects found by the detector
for name, obj in zip(msg.objects, msg.ob_msgs):
# Check to see if the object has not already been seen and if it is an object of interest
if name not in self.delivery_locations.keys(
) and name in OBJECTS_OF_INTEREST:
print(self.calculate_bounding_box_area(obj.corners))
# Ensure that the object detected is of high confidence and close to the robot
print('Detected object info')
print(obj.confidence)
print(obj.distance)
print(self.calculate_bounding_box_area(obj.corners))
if (obj.confidence < OBJECT_CONFIDENCE_THESH
and obj.distance < OBJECT_DISTANCE_THESH
and self.calculate_bounding_box_area(
obj.corners) > BOUNDING_BOX_AREA_THRESH):
# Add the object to the robot list
print('adding object')
currentPose = Pose2D()
currentPose.x = self.x
currentPose.y = self.y
currentPose.theta = self.theta
self.delivery_locations[name] = currentPose
print(self.delivery_locations)
# Once all objects have been found, then start the request cycle
if len(self.delivery_locations.keys()
) == NUM_LOCATIONS_EXPLORED:
rospy.loginfo(
"NAVIGATOR: Found all delivery locations.")
self.switch_mode(Mode.IDLE)
# to be tested: if object detected again, update location in case opponent moved it
elif name in self.delivery_locations.keys():
prev_loc = self.delivery_locations[name]
dist = math.sqrt((prev_loc.x - self.x)**2 +
(prev_loc.y - self.y)**2)
if dist > 8: # todo: tune this distance threshold
self.delivery_locations[name].x = self.x
self.delivery_locations[name].y = self.y
self.delivery_locations[name].theta = self.theta
rospy.loginfo(
"NAVIGATOR: Updating detected object location to current position"
)
def request_callback(self, msg):
rospy.loginfo("NAVIGATOR: Receiving request {}".format(msg.data))
if msg.data == 'clear':
self.requests = []
self.switch_mode(Mode.IDLE)
return
if len(self.requests) == 0:
for location in msg.data.split(','):
if location not in self.delivery_locations.keys():
rospy.loginfo("NAVIGATOR: Location %s invalid. Skipping.",
location)
continue
else:
rospy.loginfo("NAVIGATOR: Processing request...")
self.requests.append(location)
if len(self.requests) > 0:
self.requests.append(HOME_LOCATION)
self.go_to_next_request()
def init_stop_sign(self):
self.stop_sign_roll_start = rospy.get_rostime()
self.traj_controller.V_max = 0.05
self.pose_controller.V_max = 0.05
self.v_des = 0.04
self.switch_mode(Mode.ROLL)
def has_rolled(self):
return (self.mode == Mode.ROLL
and rospy.get_rostime() - self.stop_sign_roll_start >
rospy.Duration.from_sec(self.stop_sign_stop_time))
def near_goal(self):
"""
returns whether the robot is close enough in position to the goal to
start using the pose controller
"""
return linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g
])) < self.near_thresh
def at_goal(self):
"""
returns whether the robot has reached the goal position with enough
accuracy to return to idle state
"""
return (linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g])) <
self.near_thresh and abs(wrapToPi(self.theta - self.theta_g)) <
self.at_thresh_theta)
def aligned(self):
"""
returns whether robot is aligned with starting direction of path
(enough to switch to tracking controller)
"""
return (abs(wrapToPi(self.theta - self.th_init)) <
self.theta_start_thresh)
def close_to_plan_start(self):
return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh
and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)
def snap_to_grid(self, x):
return (self.plan_resolution * round(x[0] / self.plan_resolution),
self.plan_resolution * round(x[1] / self.plan_resolution))
def switch_mode(self, new_mode):
rospy.loginfo("NAVIGATOR: Switching from %s -> %s", self.mode,
new_mode)
self.mode = new_mode
def publish_planned_path(self, path, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in path:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_smoothed_path(self, traj, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for i in range(traj.shape[0]):
pose_st = PoseStamped()
pose_st.pose.position.x = traj[i, 0]
pose_st.pose.position.y = traj[i, 1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_control(self):
"""
Runs appropriate controller depending on the mode. Assumes all controllers
are all properly set up / with the correct goals loaded
"""
t = self.get_current_plan_time()
if self.mode == Mode.PARK:
V, om = self.pose_controller.compute_control(
self.x, self.y, self.theta, t)
elif self.mode == Mode.TRACK or self.mode == Mode.ROLL:
V, om = self.traj_controller.compute_control(
self.x, self.y, self.theta, t)
elif self.mode == Mode.ALIGN:
V, om = self.heading_controller.compute_control(
self.x, self.y, self.theta, t)
elif self.mode == Mode.EXPLORE:
return
else:
V = 0.
om = 0.
cmd_vel = Twist()
cmd_vel.linear.x = V
cmd_vel.angular.z = om
self.nav_vel_pub.publish(cmd_vel)
def get_current_plan_time(self):
t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
return max(0.0, t) # clip negative time to 0
def go_to_next_request(self):
goal_pose = self.delivery_locations[self.requests[0]]
if goal_pose.x != self.x_g or goal_pose.y != self.y_g or goal_pose.theta != self.theta_g:
rospy.loginfo("NAVIGATOR: Going to a new location")
self.x_g = goal_pose.x
self.y_g = goal_pose.y
self.theta_g = goal_pose.theta
self.replan()
def replan(self):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo(
"NAVIGATOR: replanning canceled, waiting for occupancy map.")
self.switch_mode(Mode.IDLE)
return
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
self.plan_start = x_init
x_goal = self.snap_to_grid((self.x_g, self.y_g))
problem = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("NAVIGATOR: computing navigation plan")
success = problem.solve()
if not success:
rospy.loginfo("NAVIGATOR: Planning failed")
return
rospy.loginfo("NAVIGATOR: Planning Succeeded")
planned_path = problem.path
# Check whether path is too short
try:
if len(planned_path) < 4:
rospy.loginfo("NAVIGATOR: Path too short to track")
self.switch_mode(Mode.PARK)
return
except:
rospy.loginfo(
"NAVIGATOR: len(path_planned) attempt failed. Switching to park. Try a new path."
)
self.switch_mode(Mode.PARK)
return
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
self.spline_alpha,
self.traj_dt)
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
)
# Estimate duration of new trajectory
th_init_new = traj_new[0, 2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err / self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr:
rospy.loginfo(
"NAVIGATOR: New plan rejected (longer duration than current plan)"
)
self.publish_smoothed_path(traj_new,
self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.th_init = traj_new[0, 2]
self.heading_controller.load_goal(self.th_init)
if not self.aligned():
rospy.loginfo("NAVIGATOR: Not aligned with start direction")
self.switch_mode(Mode.ALIGN)
return
rospy.loginfo("NAVIGATOR: Ready to track")
self.switch_mode(Mode.TRACK)
def run(self):
rate = rospy.Rate(10) # 10 Hz
while not rospy.is_shutdown():
# try to get state information to update self.x, self.y, self.theta
try:
(translation, rotation) = self.trans_listener.lookupTransform(
'/map', '/base_footprint', rospy.Time(0))
self.x = translation[0]
self.y = translation[1]
euler = tf.transformations.euler_from_quaternion(rotation)
self.theta = euler[2]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
self.current_plan = []
rospy.loginfo("NAVIGATOR: waiting for state info")
if (self.mode != Mode.EXPLORE):
self.switch_mode(Mode.IDLE)
print e
pass
# STATE MACHINE LOGIC
# some transitions handled by callbacks
if self.mode == Mode.IDLE:
pass
elif self.mode == Mode.ALIGN:
if self.aligned():
self.current_plan_start_time = rospy.get_rostime()
self.switch_mode(Mode.TRACK)
elif self.mode == Mode.TRACK:
if self.near_goal():
self.switch_mode(Mode.PARK)
elif not self.close_to_plan_start():
rospy.loginfo(
"NAVIGATOR: replanning because far from start")
self.replan()
elif (rospy.get_rostime() - self.current_plan_start_time
).to_sec() > self.current_plan_duration:
rospy.loginfo("NAVIGATOR: replanning because out of time")
self.replan(
) # we aren't near the goal but we thought we should have been, so replan
self.displayETA()
elif self.mode == Mode.PARK:
if self.at_goal():
# Forget about the current goal
self.requests.pop(0)
self.x_g = None
self.y_g = None
self.theta_g = None
self.switch_mode(Mode.IDLE)
# Check to see if there are more places that must be visited
if len(self.requests) > 0:
self.go_to_next_request()
elif self.mode == Mode.ROLL:
rospy.loginfo("Rolling...")
if self.has_rolled():
rospy.loginfo("Finishd rolling")
self.traj_controller.V_max = 0.2
self.pose_controller.V_max = 0.2
self.v_des = 0.12
self.mode = Mode.TRACK
self.first_seen_time = -1
# publish vendor and robot locations
self.publish_vendor_locs()
self.publish_robot_loc()
self.publish_control()
rate.sleep()
########## RVIZ VISUALIZATION ##########
def publish_vendor_locs(self):
for name, loc in self.delivery_locations.items():
# add marker
marker = Marker()
marker.header.frame_id = "/map"
marker.header.stamp = rospy.Time()
# so we don't create millions of markers over time
if name in self.vendor_marker_ids.keys():
marker.id = self.vendor_marker_ids[name]
else:
next_avail_id = len(self.vendor_marker_ids
) + 1 # robot is 0, so increment from 1
marker.id = next_avail_id
self.vendor_marker_ids[name] = next_avail_id
marker.type = 1 # cube
marker.pose.position.x = loc.x
marker.pose.position.y = loc.y
#marker.pose.position.x = loc[0]
#marker.pose.position.y = loc[1]
marker.pose.position.z = 0.1
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
if name == HOME_LOCATION:
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
else:
marker.color.a = 1.0 # Don't forget to set the alpha!
marker.color.r = 0.5
marker.color.g = 0.0
marker.color.b = 1.0
self.vis_pub.publish(marker)
def publish_robot_loc(self):
marker = Marker()
marker.header.frame_id = "base_footprint"
marker.header.stamp = rospy.Time()
marker.id = 0 # robot marker id is 0
marker.type = 0 # arrow
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.4
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0 # Don't forget to set the alpha!
marker.color.r = 0.5
marker.color.g = 1.0
marker.color.b = 0.5
self.vis_pub.publish(marker)
def displayETA(self):
marker = Marker()
marker.header.frame_id = "base_footprint"
marker.header.stamp = rospy.Time()
marker.id = 1234
marker.type = Marker.TEXT_VIEW_FACING
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.4
marker.scale.y = 0.1
marker.scale.z = 0.2
marker.color.a = 1.0 # Don't forget to set the alpha!
marker.color.r = 0.02
marker.color.g = 0.84
marker.color.b = 1.0
marker.text = "ETA to destination: {}".format(
self.current_plan_duration)
self.vis_pub.publish(marker)
class Navigator:
"""
This node handles point to point turtlebot motion, avoiding obstacles.
It is the sole node that should publish to cmd_vel
"""
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 dyn_cfg_callback(self, config, level):
rospy.loginfo("Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}, spline_alpha:{spline_alpha}".format(**config))
self.pose_controller.k1 = config["k1"] #default: 0.8
self.pose_controller.k2 = config["k2"] #default: 0.4
self.pose_controller.k3 = config["k3"] #default: 0.4
self.spline_alpha = config["spline_alpha"] #default: 0.15
self.at_thresh_theta = config["at_thresh_theta"] #default: 0.05
return config
def cmd_nav_callback(self, data):
"""
loads in goal if different from current goal, and replans
"""
if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
self.x_g = data.x
self.y_g = data.y
self.theta_g = data.theta
self.replan()
def map_md_callback(self, msg):
"""
receives maps meta data and stores it
"""
self.map_width = msg.width
self.map_height = msg.height
self.map_resolution = msg.resolution
self.map_origin = (msg.origin.position.x,msg.origin.position.y)
def map_callback(self,msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width>0 and self.map_height>0 and len(self.map_probs)>0:
self.occupancy = StochOccupancyGrid2D(self.map_resolution,
self.map_width,
self.map_height,
self.map_origin[0],
self.map_origin[1],
10,
self.map_probs)
if self.x_g is not None:
# if we have a goal to plan to, replan
rospy.loginfo("replanning because of new map")
self.replan() # new map, need to replan
def stop_sign_detected_callback(self, msg):
""" callback for when the detector has found a stop sign. Note that
a distance of 0 can mean that the lidar did not pickup the stop sign at all """
# distance of the stop sign
thetaleft = msg.thetaleft - 2*np.pi if msg.thetaleft > np.pi else msg.thetaleft
thetaright = msg.thetaright - 2*np.pi if msg.thetaright > np.pi else msg.thetaright
camera_theta = (thetaleft + thetaright)/2
dist = msg.distance*np.cos(camera_theta)
# if close enough and in nav mode, stop
if dist > 0 and dist < self.stop_min_dist and self.mode == Mode.TRACK and msg.confidence > 0.8:
self.init_stop_sign()
print('stop sign detected')
def vendor_detected_callback(self, msg):
'''
callback for when the detector has found a vendor logo it is looking for
'''
msg = msg.ob_msgs[-1]
print(msg.name, msg.distance)
if msg.name in self.vendors.keys() and msg.distance < self.vendor_detector_dist and self.vendors[msg.name] == None:
print("Vendor detected")
thetaleft = msg.thetaleft - 2*np.pi if msg.thetaleft > np.pi else msg.thetaleft
thetaright = msg.thetaright - 2*np.pi if msg.thetaright > np.pi else msg.thetaright
camera_theta = (thetaleft + thetaright)/2
dist = msg.distance
self.vendors[msg.name] = (self.x+dist*np.cos(self.theta+camera_theta), self.y+dist*np.sin(self.theta+camera_theta))
pub_msg = Pose2D()
pub_msg.x, pub_msg.y = self.vendors[msg.name]
self.marker_pub.publish(pub_msg)
self.vendors[msg.name] = self.snap_to_grid((self.x, self.y))
def order_callback(self, msg):
'''
callback for when there is an order request
'''
vendorList = str(msg)[7:-1].split(',')
if vendorList == ['']:
self.vendor_queue.append(self.vendors['home'])
else:
locations = [self.vendors['home']]
for v in vendorList:
if v != '':
locations.append(self.vendors[v])
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
edges, cost_matrix = tsp_solver(state_min, state_max, self.occupancy, self.plan_resolution, self.v_des, self.spline_alpha, self.traj_dt, locations)
print(edges, cost_matrix)
queue_temp = []
cost_list = [0]
for edge in edges[1:]:
queue_temp.append(locations[edge[1]])
cost_list.append(cost_list[-1]+cost_matrix[edge[0]][edge[1]])
print(cost_list)
energy_fill_times = 0
while cost_list[-1] > self.energy:
index = len(cost_list)-1
while index >= 0 and cost_list[index] > self.energy:
index -= 1
while index >= 0 and cost_list[index] + compute_path_cost(state_min, state_max, self.occupancy, self.plan_resolution, self.v_des, self.spline_alpha, self.traj_dt, self.vendors['donut'], locations[edges[index][1]]) > self.energy:
index -= 1
queue_temp.insert(index+energy_fill_times, self.vendors['donut'])
energy_fill_times += 1
# update cost_list
cost_list[index+1:] = cost_list[index+1:] - cost_list[index+1] + compute_path_cost(state_min, state_max, self.occupancy, self.plan_resolution, self.v_des, self.spline_alpha, self.traj_dt, self.vendors['donut'], locations[edges[index+1][1]])
print(queue_temp)
self.vendor_queue = queue_temp
def shutdown_callback(self):
"""
publishes zero velocities upon rospy shutdown
"""
cmd_vel = Twist()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = 0.0
self.nav_vel_pub.publish(cmd_vel)
def stay_idle(self):
""" sends zero velocity to stay put """
vel_g_msg = Twist()
self.nav_vel_pub.publish(vel_g_msg)
def init_stop_sign(self):
""" initiates a stop sign maneuver """
self.stop_sign_start = rospy.get_rostime()
self.mode = Mode.STOP
def has_stopped(self):
""" checks if stop sign maneuver is over """
return self.mode == Mode.STOP and \
rospy.get_rostime() - self.stop_sign_start > rospy.Duration.from_sec(self.stop_time)
def init_crossing(self):
""" initiates an intersection crossing maneuver """
self.cross_start = rospy.get_rostime()
self.mode = Mode.CROSS
print("init crossing")
def has_crossed(self):
""" checks if crossing maneuver is over """
return self.mode == Mode.CROSS and \
rospy.get_rostime() - self.cross_start > rospy.Duration.from_sec(self.crossing_time)
def near_goal(self):
"""
returns whether the robot is close enough in position to the goal to
start using the pose controller
"""
return linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])) < self.near_thresh
def at_goal(self):
"""
returns whether the robot has reached the goal position with enough
accuracy to return to idle state
"""
if self.x_g == None or self.y_g == None or self.theta_g == None:
return True
if (self.vendor_queue and linalg.norm(np.array([self.x-self.vendor_queue[0][0], self.y-self.vendor_queue[0][1]])) < self.near_thresh) or (linalg.norm(np.array([self.x-self.intermediate_left[0], self.y-self.intermediate_left[1]])) < self.near_thresh) or (linalg.norm(np.array([self.x-self.intermediate_right[0], self.y-self.intermediate_right[1]])) < self.near_thresh):
return linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])) < self.at_thresh
else:
return (linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])) < self.at_thresh and abs(wrapToPi(self.theta - self.theta_g)) < self.at_thresh_theta)
def aligned(self):
"""
returns whether robot is aligned with starting direction of path
(enough to switch to tracking controller)
"""
return (abs(wrapToPi(self.theta - self.th_init)) < self.theta_start_thresh)
def close_to_plan_start(self):
return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)
def snap_to_grid(self, x):
return (self.plan_resolution*round(x[0]/self.plan_resolution), self.plan_resolution*round(x[1]/self.plan_resolution))
def switch_mode(self, new_mode):
rospy.loginfo("Switching from %s -> %s", self.mode, new_mode)
self.mode = new_mode
def publish_planned_path(self, path, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in path:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_smoothed_path(self, traj, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for i in range(traj.shape[0]):
pose_st = PoseStamped()
pose_st.pose.position.x = traj[i,0]
pose_st.pose.position.y = traj[i,1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_control(self):
"""
Runs appropriate controller depending on the mode. Assumes all controllers
are all properly set up / with the correct goals loaded
"""
t = self.get_current_plan_time()
if self.mode == Mode.PARK:
V, om = self.pose_controller.compute_control(self.x, self.y, self.theta, t)
elif self.mode == Mode.TRACK:
V, om = self.traj_controller.compute_control(self.x, self.y, self.theta, t)
elif self.mode == Mode.ALIGN:
V, om = self.heading_controller.compute_control(self.x, self.y, self.theta, t)
elif self.mode == Mode.CROSS:
V = self.crossing_vel
om = 0.
else:
V = 0.
om = 0.
cmd_vel = Twist()
cmd_vel.linear.x = V
cmd_vel.angular.z = om
self.nav_vel_pub.publish(cmd_vel)
def get_current_plan_time(self):
t = (rospy.get_rostime()-self.current_plan_start_time).to_sec()
return max(0.0, t) # clip negative time to 0
def replan(self):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo("Navigator: replanning canceled, waiting for occupancy map.")
self.switch_mode(Mode.IDLE)
return
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
self.plan_start = x_init
x_goal = self.snap_to_grid((self.x_g, self.y_g))
problem = AStar(state_min,state_max,x_init,x_goal,self.occupancy,self.plan_resolution)
rospy.loginfo("Navigator: computing navigation plan")
success = problem.solve()
# deal with goal that is too far from where we are
if not success:
rospy.loginfo("Planning failed")
self.fail_count += 1
if self.fail_count > 3:
self.fail_count = 0
left = abs(self.y - self.intermediate_left[1])
right = abs(self.y - self.intermediate_right[1])
self.x_g = 2.5
if left < right:
self.y_g = self.intermediate_left[1]
else:
self.y_g = self.intermediate_right[1]
rospy.loginfo("Goal position reset due to failed plan")
return
rospy.loginfo("Planning Succeeded")
planned_path = problem.path
# Check whether path is too short
if len(planned_path) < 4:
rospy.loginfo("Path too short to track")
self.switch_mode(Mode.PARK)
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
return
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des, self.spline_alpha, self.traj_dt)
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time()
# Estimate duration of new trajectory
th_init_new = traj_new[0,2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err/self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr:
rospy.loginfo("New plan rejected (longer duration than current plan)")
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.th_init = traj_new[0,2]
self.heading_controller.load_goal(self.th_init)
if not self.aligned():
rospy.loginfo("Not aligned with start direction")
if not self.mode == Mode.STOP:
if not (self.mode == Mode.CROSS and not self.has_crossed()):
self.switch_mode(Mode.ALIGN)
return
if not self.mode == Mode.STOP:
if not (self.mode == Mode.CROSS and not self.has_crossed()):
rospy.loginfo("Ready to track")
self.switch_mode(Mode.TRACK)
def run(self):
rate = rospy.Rate(10) # 10 Hz
while not rospy.is_shutdown():
# try to get state information to update self.x, self.y, self.theta
try:
(translation,rotation) = self.trans_listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
self.x = translation[0]
self.y = translation[1]
euler = tf.transformations.euler_from_quaternion(rotation)
self.theta = euler[2]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
self.current_plan = []
rospy.loginfo("Navigator: waiting for state info")
self.switch_mode(Mode.IDLE)
print e
pass
# STATE MACHINE LOGIC
# some transitions handled by callbacks
if self.mode == Mode.IDLE:
if self.vendors['home'] == None and self.x != 0 and self.y != 0:
self.vendors['home'] = self.snap_to_grid((self.x + 0.2, self.y))
msg = Pose2D()
msg.x, msg.y = self.vendors['home']
self.marker_pub.publish(msg)
if self.vendor_queue and (rospy.get_rostime() - self.vendor_stop_start_time) > rospy.Duration.from_sec(self.vendor_time):
self.x_g = self.vendor_queue[0][0]
self.y_g = self.vendor_queue[0][1]
#start from home
if (linalg.norm(np.array([self.x-self.vendors['home'][0], self.y-self.vendors['home'][1]])) < self.at_thresh):
left = abs(self.y_g - self.intermediate_left[1])
right = abs(self.y_g - self.intermediate_right[1])
self.x_g = 2.5
if left < right:
self.y_g = self.intermediate_left[1]
else:
self.y_g = self.intermediate_right[1]
#return to home
elif ((self.x_g==self.vendors['home'][0]) and (self.y_g==self.vendors['home'][1])) and not (linalg.norm(np.array([self.x-self.intermediate_left[0], self.y-self.intermediate_left[1]])) < self.at_thresh or linalg.norm(np.array([self.x-self.intermediate_right[0], self.y-self.intermediate_right[1]])) < self.at_thresh) and self.x < 3:
left = abs(self.y - self.intermediate_left[1])
right = abs(self.y - self.intermediate_right[1])
self.x_g = 2.5
if left < right:
self.y_g = self.intermediate_left[1]
else:
self.y_g = self.intermediate_right[1]
self.theta_g = 0
self.replan()
elif self.mode == Mode.ALIGN:
if self.aligned():
self.current_plan_start_time = rospy.get_rostime()
self.switch_mode(Mode.TRACK)
elif self.mode == Mode.TRACK:
if self.near_goal():
self.switch_mode(Mode.PARK)
elif not self.close_to_plan_start():
rospy.loginfo("replanning because far from start")
self.replan()
elif (rospy.get_rostime() - self.current_plan_start_time).to_sec() > self.current_plan_duration:
rospy.loginfo("replanning because out of time")
self.replan() # we aren't near the goal but we thought we should have been, so replan
elif self.mode == Mode.PARK:
if self.at_goal():
if self.vendor_queue and linalg.norm(np.array([self.x-self.vendor_queue[0][0], self.y-self.vendor_queue[0][1]])) < self.at_thresh:
self.vendor_stop_start_time = rospy.get_rostime()
self.vendor_queue.pop(0)
# forget about goal:
self.x_g = None
self.y_g = None
self.theta_g = None
self.switch_mode(Mode.IDLE)
elif self.mode == Mode.STOP:
# At a stop sign
if self.has_stopped():
self.init_crossing()
else:
self.stay_idle()
elif self.mode == Mode.CROSS:
# Crossing an intersection
if self.at_goal():
self.mode = Mode.IDLE
elif self.has_crossed():
print("crossed")
self.replan()
else:
print("crossing")
cmd_vel = Twist()
cmd_vel.linear.x = self.crossing_vel
cmd_vel.angular.z = 0
self.nav_vel_pub.publish(cmd_vel)
self.publish_control()
# Publish goal marker
if self.x_g and self.y_g:
goal = Pose2D()
goal.x, goal.y = self.x_g, self.y_g
self.goal_marker_pub.publish(goal)
# Publish hungry message
if self.vendors['donut'] and self.x_g == self.vendors['donut'][0] and self.y_g == self.vendors['donut'][1]:
msg = String()
msg.data = "I am so hungry! I have to grab some donuts before I starve to die!!!"
self.hungry_pub.publish(msg)
rate.sleep()
class Navigator:
"""
This node handles point to point turtlebot motion, avoiding obstacles.
It is the sole node that should publish to cmd_vel
"""
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 stop_sign_detected_callback(self, msg):
""" callback for when the detector has found a stop sign. Note that
a distance of 0 can mean that the lidar did not pickup the stop sign at all """
#rospy.loginfo("Detected stop sign")
# distance of the stop sign
dist = msg.distance
# if close enough and in track mode, stop
#print("stop sign dust: %s, stopped: %s " % (dist, self.stopped))
if dist > 0 and dist < self.stop_min_dist and (self.mode == Mode.TRACK or self.mode == Mode.VENDOR) and not self.stopped:
#rospy.loginfo("Start stopping")
self.init_stop_sign()
def init_stop_sign(self):
""" initiates a stop sign maneuver """
self.stop_sign_start = rospy.get_rostime()
self.stopped = True
self.switch_mode(Mode.STOP)
def has_stopped(self):
""" checks if stop sign maneuver is over """
return self.mode == Mode.STOP and \
rospy.get_rostime() - self.stop_sign_start > rospy.Duration.from_sec(self.stop_time)
def stay_idle(self):
""" sends zero velocity to stay put """
vel_g_msg = Twist()
self.nav_vel_pub.publish(vel_g_msg)
def dyn_cfg_callback(self, config, level):
rospy.loginfo("Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}, spline_alpha:{spline_alpha}, traj_dt:{traj_dt}".format(**config))
self.pose_controller.k1 = config["k1"]
self.pose_controller.k2 = config["k2"]
self.pose_controller.k3 = config["k3"]
self.spline_alpha = config["spline_alpha"]
self.traj_dt = config["traj_dt"]
return config
def detected_objects_name_callback(self, msg):
#rospy.loginfo("There are %i detected objects" % len(msg.objects))
#self.detected_objects = msg
for obj in msg.objects:
#rospy.loginfo("obj1: %s" % obj)
self.detected_objects[obj] = (self.x, self.y, self.theta)
self.last_box_time = rospy.get_rostime()
def deliver_one(self):
if not self.orderlist:
self.vendor_start,self.gotovendor = None,False
return
data = self.orderlist.pop(0)
if data in self.detected_objects:
rospy.loginfo("Popping order: %s, set goal: %s" % (data, str(self.detected_objects[data])))
self.x_g, self.y_g, self.theta_g = self.detected_objects[data]
self.gotovendor = True
self.replan()
else:
rospy.loginfo("New order: %s, no goal found. " % (msg.data))
def delivery_request_callback(self, msg):
self.orderlist = msg.data.split(',')
self.deliver_one()
# plan the robot
self.last_box_time = rospy.get_rostime()
def cmd_nav_callback(self, data):
"""
loads in goal if different from current goal, and replans
"""
if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
self.x_g = data.x
self.y_g = data.y
self.theta_g = data.theta
# start over everytime we change the pose
#self.switch_mode(Mode.ALIGN)
self.replan()
def map_md_callback(self, msg):
"""
receives maps meta data and stores it
"""
self.map_width = msg.width
self.map_height = msg.height
self.map_resolution = msg.resolution
self.map_origin = (msg.origin.position.x,msg.origin.position.y)
def map_callback(self,msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width>0 and self.map_height>0 and len(self.map_probs)>0:
self.occupancy = StochOccupancyGrid2D(self.map_resolution,
self.map_width,
self.map_height,
self.map_origin[0],
self.map_origin[1],
6, # TODO: was 8
self.map_probs)
if self.x_g is not None and self.mode != Mode.STOP:
# if we have a goal to plan to, replan
rospy.loginfo("replanning because of new map")
self.replan() # new map, need to replan
def shutdown_callback(self):
"""
publishes zero velocities upon rospy shutdown
"""
cmd_vel = Twist()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = 0.0
self.nav_vel_pub.publish(cmd_vel)
def near_goal(self):
"""
returns whether the robot is close enough in position to the goal to
start using the pose controller
"""
return linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])) < self.near_thresh
def at_goal(self):
"""
returns whether the robot has reached the goal position with enough
accuracy to return to idle state
"""
#print(linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])))
return (linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])) < self.at_thresh and abs(wrapToPi(self.theta - self.theta_g)) < self.at_thresh_theta)
def aligned(self):
"""
returns whether robot is aligned with starting direction of path
(enough to switch to tracking controller)
"""
return (abs(wrapToPi(self.theta - self.th_init)) < self.theta_start_thresh)
def close_to_plan_start(self):
return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)
def snap_to_grid(self, x):
return (self.plan_resolution*round(x[0]/self.plan_resolution), self.plan_resolution*round(x[1]/self.plan_resolution))
def switch_mode(self, new_mode):
rospy.loginfo("Switching from %s -> %s", self.mode, new_mode)
self.mode = new_mode
def publish_planned_path(self, path, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in path:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_smoothed_path(self, traj, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for i in range(traj.shape[0]):
pose_st = PoseStamped()
pose_st.pose.position.x = traj[i,0]
pose_st.pose.position.y = traj[i,1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_control(self):
"""
Runs appropriate controller depending on the mode. Assumes all controllers
are all properly set up / with the correct goals loaded
"""
t = self.get_current_plan_time()
if self.mode == Mode.PARK:
V, om = self.pose_controller.compute_control(self.x, self.y, self.theta, t)
elif self.mode == Mode.TRACK:
V, om = self.traj_controller.compute_control(self.x, self.y, self.theta, t)
elif self.mode == Mode.ALIGN:
V, om = self.heading_controller.compute_control(self.x, self.y, self.theta, t)
else:
V = 0.
om = 0.
self.v = V
cmd_vel = Twist()
cmd_vel.linear.x = V
cmd_vel.angular.z = om
# rospy.loginfo("Reconfigure Request: V: %f, om:%f, mode: %s" % (V, om, self.mode))
self.nav_vel_pub.publish(cmd_vel)
def get_current_plan_time(self):
t = (rospy.get_rostime()-self.current_plan_start_time).to_sec()
return max(0.0, t) # clip negative time to 0
def replan(self):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo("Navigator: replanning canceled, waiting for occupancy map.")
self.switch_mode(Mode.IDLE)
return
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
self.plan_start = x_init
x_goal = self.snap_to_grid((self.x_g, self.y_g))
problem = AStar(state_min,state_max,x_init,x_goal,self.occupancy,self.plan_resolution)
rospy.loginfo("Navigator: computing navigation plan")
success = problem.solve()
if success:
rospy.loginfo("Navigator: computing navigation plan SUCCESS!!!!!!!!")
else:
rospy.loginfo("Navigator: computing navigation plan FAILLLLL!!")
if not success:
self.gotovendor = False
rospy.loginfo("Planning failed")
return
rospy.loginfo("Planning Succeeded")
planned_path = problem.path
# Check whether path is too short
if len(planned_path) < 4:
rospy.loginfo("Path too short to track")
self.switch_mode(Mode.PARK)
return
# Smooth and generate a trajectory
traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des, self.spline_alpha, self.traj_dt)
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time()
# Estimate duration of new trajectory
th_init_new = traj_new[0,2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err/self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr:
rospy.loginfo("New plan rejected (longer duration than current plan)")
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.th_init = traj_new[0,2]
self.heading_controller.load_goal(self.th_init)
if not self.aligned():
rospy.loginfo("Not aligned with start direction")
self.switch_mode(Mode.ALIGN)
return
rospy.loginfo("Ready to track")
self.switch_mode(Mode.TRACK)
def run(self):
rate = rospy.Rate(10) # 10 Hz
while not rospy.is_shutdown():
# try to get state information to update self.x, self.y, self.theta
try:
(translation,rotation) = self.trans_listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
self.x = translation[0]
self.y = translation[1]
euler = tf.transformations.euler_from_quaternion(rotation)
self.theta = euler[2]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
self.current_plan = []
rospy.loginfo("Navigator: waiting for state info")
self.switch_mode(Mode.IDLE)
print e
pass
# STATE MACHINE LOGIC
# some transitions handled by callbacks
if self.mode == Mode.IDLE:
self.stopped = False
elif self.mode == Mode.ALIGN:
if self.aligned():
self.current_plan_start_time = rospy.get_rostime()
self.switch_mode(Mode.TRACK)
elif self.mode == Mode.TRACK:
if self.near_goal():
self.switch_mode(Mode.PARK)
elif not self.close_to_plan_start():
rospy.loginfo("replanning because far from start")
self.replan()
elif (rospy.get_rostime() - self.current_plan_start_time).to_sec() > self.current_plan_duration:
rospy.loginfo("replanning because out of time")
self.replan() # we aren't near the goal but we thought we should have been, so replan
elif self.mode == Mode.PARK:
if self.at_goal():
# forget about goal:
self.x_g = None
self.y_g = None
self.theta_g = None
if self.gotovendor:
self.switch_mode(Mode.VENDOR)
else:
self.switch_mode(Mode.IDLE)
elif self.mode == Mode.VENDOR:
if self.vendor_start == None:
self.vendor_start = rospy.get_rostime()
t_vendor = (rospy.get_rostime()-self.vendor_start).to_sec()
if t_vendor > self.vendor_time:
if not self.orderlist:
self.x_g,self.y_g,self.theta_g = self.home_location
self.vendor_start,self.gotovendor = None,False
self.replan()
else:
self.deliver_one()
elif self.mode == Mode.STOP:
# At a stop sign
if self.has_stopped():
self.replan()
self.stop_sign_start = None
else:
self.stay_idle()
self.publish_control()
rate.sleep()
class Navigator:
"""
This node handles point to point turtlebot motion, avoiding obstacles.
It is the sole node that should publish to cmd_vel
"""
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", 0.7)
# Time taken to cross an intersection
self.crossing_time = rospy.get_param("~crossing_time", 3.)
# Number of stop sign detections in a row
self.stop_detections = 0
# Stop sign detector
rospy.Subscriber('/detector/stop_sign', DetectedObject,
self.stop_sign_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.om_max = 0.4 # maximum angular velocity
self.v_des = 0.12 # desired cruising velocity
#self.v_des = 0.2
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
# goal state threshold
self.at_thresh = 0.09 #0.05
self.at_thresh_theta = 0.2 #0.1
# trajectory smoothing
self.spline_alpha = 0.005 #0.01 #0.05 # 0.15
self.traj_dt = 0.1
# trajectory tracking controller parameters
self.kpx = 0.5 # 0.5
self.kpy = 0.5 # 0.5
self.kdx = 1.5 # 1.5
self.kdy = 1.5 # 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"
######## CALLBACK FUNCTIONS ########
def stop_sign_detected_callback(self, msg):
""" callback for when the detector has found a stop sign. Note that
a distance of 0 can mean that the lidar did not pickup the stop sign at all """
# distance of the stop sign
dist = msg.distance
# confidence of stop sign
conf = msg.confidence
# if close enough and in nav mode, stop
if dist > 0 and dist < self.stop_min_dist and self.mode == Mode.TRACK and conf > 0.8:
self.stop_detections = self.stop_detections + 1
rospy.loginfo("Our stop sign distance is " + str(dist))
rospy.loginfo("Our stop sign confidence is " + str(conf))
else:
self.stop_detections = 0 # reset (might want to delete this??)
# if we've made a detection more than twice in a row
if self.stop_detections > 2:
self.stop_detections = 0
self.init_stop_sign()
def dyn_cfg_callback(self, config, level):
rospy.loginfo(
"Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}".format(**config))
self.pose_controller.k1 = config["k1"]
self.pose_controller.k2 = config["k2"]
self.pose_controller.k3 = config["k3"]
return config
def cmd_nav_callback(self, data):
"""
loads in goal if different from current goal, and replans
"""
if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
self.x_g = data.x
self.y_g = data.y
self.theta_g = data.theta
rospy.loginfo("Replanning because of new cmd nav goal")
self.replan()
def map_md_callback(self, msg):
"""
receives maps meta data and stores it
"""
self.map_width = msg.width
self.map_height = msg.height
self.map_resolution = msg.resolution
self.map_origin = (msg.origin.position.x, msg.origin.position.y)
def map_callback(self, msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
self.occupancy = StochOccupancyGrid2D(
self.map_resolution, self.map_width, self.map_height,
self.map_origin[0], self.map_origin[1], 8, self.map_probs)
if self.x_g is not None:
# if we have a goal to plan to, replan
# EREZ added condition 11/11/20
if self.mode == Mode.TRACK:
rospy.loginfo("replanning because of new map")
self.replan() # new map, need to replan
rospy.loginfo("replanning at state:" + str(self.mode))
def shutdown_callback(self):
"""
publishes zero velocities upon rospy shutdown
"""
cmd_vel = Twist()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = 0.0
self.nav_vel_pub.publish(cmd_vel)
######## STATE MACHINE ACTIONS ########
def init_stop_sign(self):
""" initiates a stop sign maneuver """
self.stop_sign_start = rospy.get_rostime()
self.switch_mode(Mode.STOP)
def has_stopped(self):
""" checks if stop sign maneuver is over """
return self.mode == Mode.STOP and \
rospy.get_rostime() - self.stop_sign_start > rospy.Duration.from_sec(self.stop_time)
def init_crossing(self):
""" initiates an intersection crossing maneuver """
self.cross_start = rospy.get_rostime()
self.switch_mode(Mode.CROSS)
def has_crossed(self):
""" checks if crossing maneuver is over """
return self.mode == Mode.CROSS and \
rospy.get_rostime() - self.cross_start > rospy.Duration.from_sec(self.crossing_time)
def near_goal(self):
"""
returns whether the robot is close enough in position to the goal to
start using the pose controller
"""
return linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g
])) < self.near_thresh
def at_goal(self):
"""
returns whether the robot has reached the goal position with enough
accuracy to return to idle state
"""
# CHANGED FROM NEAR_THRESH TO AT_THRESH
return (linalg.norm(np.array(
[self.x - self.x_g, self.y - self.y_g])) < self.at_thresh and abs(
wrapToPi(self.theta - self.theta_g)) < self.at_thresh_theta)
def aligned(self):
"""
returns whether robot is aligned with starting direction of path
(enough to switch to tracking controller)
"""
return (abs(wrapToPi(self.theta - self.th_init)) <
self.theta_start_thresh)
def close_to_plan_start(self):
return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh
and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)
def snap_to_grid(self, x):
return (self.plan_resolution * round(x[0] / self.plan_resolution),
self.plan_resolution * round(x[1] / self.plan_resolution))
def switch_mode(self, new_mode):
rospy.loginfo("NAVIGatOR: switching from %s -> %s", self.mode,
new_mode)
self.mode = new_mode
def publish_planned_path(self, path, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in path:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_smoothed_path(self, traj, publisher):
# publish planned plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for i in range(traj.shape[0]):
pose_st = PoseStamped()
pose_st.pose.position.x = traj[i, 0]
pose_st.pose.position.y = traj[i, 1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
publisher.publish(path_msg)
def publish_control(self):
"""
Runs appropriate controller depending on the mode. Assumes all controllers
are all properly set up / with the correct goals loaded
"""
t = self.get_current_plan_time()
if self.mode == Mode.PARK:
V, om = self.pose_controller.compute_control(
self.x, self.y, self.theta, t)
elif self.mode == Mode.TRACK or self.mode == Mode.CROSS:
V, om = self.traj_controller.compute_control(
self.x, self.y, self.theta, t)
elif self.mode == Mode.ALIGN:
V, om = self.heading_controller.compute_control(
self.x, self.y, self.theta, t)
else: # STOP/IDLE
V = 0.
om = 0.
cmd_vel = Twist()
cmd_vel.linear.x = V
cmd_vel.angular.z = om
self.nav_vel_pub.publish(cmd_vel)
def get_current_plan_time(self):
t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
return max(0.0, t) # clip negative time to 0
def replan(self):
"""
loads goal into pose controller
runs planner based on current pose
if plan long enough to track:
smooths resulting traj, loads it into traj_controller
sets self.current_plan_start_time
sets mode to ALIGN
else:
sets mode to PARK
"""
# Make sure we have a map
if not self.occupancy:
rospy.loginfo(
"Navigator: replanning canceled, waiting for occupancy map.")
self.switch_mode(Mode.IDLE)
return
# Attempt to plan a path
state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
self.plan_start = x_init
x_goal = self.snap_to_grid((self.x_g, self.y_g))
astar_problem = AStar(state_min, state_max, x_init, x_goal,
self.occupancy, self.plan_resolution)
rrt_problem = GeometricRRTConnect(state_min, state_max, x_init, x_goal,
self.occupancy, self.plan_resolution)
rospy.loginfo("Navigator: computing navigation plan")
rospy.loginfo("Current mode: " + str(self.mode))
astar_success = astar_problem.solve()
astar_path = astar_problem.path
rrt_success = rrt_problem.solve()
rrt_path = rrt_problem.path
success = (astar_success or rrt_success)
if not success:
rospy.loginfo("Planning failed")
return
else:
rospy.loginfo("Planning Succeeded")
if astar_success:
traj_new_astar, t_new_astar, path_length_astar = compute_smoothed_traj(
astar_path, self.v_des, self.spline_alpha, self.traj_dt)
else:
path_length_astar = np.Inf
if rrt_success:
traj_new_rrt, t_new_rrt, path_length_rrt = compute_smoothed_traj(
rrt_path, self.v_des, self.spline_alpha, self.traj_dt)
else:
path_length_rrt = np.Inf
if path_length_astar <= path_length_rrt:
traj_new = traj_new_astar
t_new = t_new_astar
planned_path = astar_path
rospy.loginfo("AStar Gave Shortest Path")
else:
traj_new = traj_new_rrt
t_new = t_new_rrt
planned_path = rrt_path
rospy.loginfo("RRT Gave Shortest Path")
# load goal into pose controller
self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
# Check whether path is too short
'''
if len(planned_path) < 4:
rospy.loginfo("Path too short to track")
self.switch_mode(Mode.PARK)
return
'''
# Smooth and generate a trajectory
# If currently tracking a trajectory, check whether new trajectory will take more time to follow
if self.mode == Mode.TRACK:
t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
)
# Estimate duration of new trajectory
th_init_new = traj_new[0, 2]
th_err = wrapToPi(th_init_new - self.theta)
t_init_align = abs(th_err / self.om_max)
t_remaining_new = t_init_align + t_new[-1]
if t_remaining_new > t_remaining_curr:
rospy.loginfo(
"New plan rejected (longer duration than current plan)")
self.publish_smoothed_path(traj_new,
self.nav_smoothed_path_rej_pub)
return
# Otherwise follow the new plan
self.publish_planned_path(planned_path, self.nav_planned_path_pub)
self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)
self.traj_controller.load_traj(t_new, traj_new)
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_duration = t_new[-1]
self.th_init = traj_new[0, 2]
self.heading_controller.load_goal(self.th_init)
if (not self.aligned() and self.mode != Mode.CROSS and self.mode != Mode.STOP) \
or (self.mode == Mode.IDLE):
rospy.loginfo("Not aligned with start direction")
self.switch_mode(Mode.ALIGN)
return
# COMMENTED OUT
#rospy.loginfo("Ready to track")
#self.switch_mode(Mode.TRACK)
def run(self):
rate = rospy.Rate(10) # 10 Hz
while not rospy.is_shutdown():
# try to get state information to update self.x, self.y, self.theta
try:
(translation, rotation) = self.trans_listener.lookupTransform(
'/map', '/base_footprint', rospy.Time(0))
self.x = translation[0]
self.y = translation[1]
euler = tf.transformations.euler_from_quaternion(rotation)
self.theta = euler[2]
# PUBLISH CURRENT STATE (11/15/20 DEM)
current_state = Pose2D()
current_state.x = self.x
current_state.y = self.y
current_state.theta = self.theta
self.current_state_pub.publish(current_state)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
self.current_plan = []
rospy.loginfo("Navigator: waiting for state info")
self.switch_mode(Mode.IDLE)
print e
pass
# STATE MACHINE LOGIC
# some transitions handled by callbacks
if self.mode == Mode.IDLE:
pass
elif self.mode == Mode.ALIGN:
if self.aligned():
self.current_plan_start_time = rospy.get_rostime()
self.switch_mode(Mode.TRACK)
elif self.mode == Mode.TRACK:
# if near goal switch to PARK mode
if self.near_goal():
self.switch_mode(Mode.PARK)
elif not self.close_to_plan_start():
rospy.loginfo("replanning because far from start")
self.replan()
elif (rospy.get_rostime() - self.current_plan_start_time
).to_sec() > self.current_plan_duration:
rospy.loginfo("replanning because out of time")
self.replan(
) # we aren't near the goal but we thought we should have been, so replan
elif self.mode == Mode.PARK:
if self.at_goal():
# forget about goal:
# EREZ COMMENTED OUT 11/18/20
#self.x_g = None
#self.y_g = None
#self.theta_g = None
self.switch_mode(Mode.IDLE)
elif self.mode == Mode.STOP:
if self.has_stopped():
self.init_crossing()
# otherwise we will be commanding 0 velocity
elif self.mode == Mode.CROSS:
# crossing the intersection
if self.has_crossed():
self.switch_mode(Mode.ALIGN)
else:
raise Exception("This mode is not supported: {}".format(
str(self.mode)))
self.publish_control()
rate.sleep()
