# using costmap_plot() to plot the background first (with resolution of (map_size/map_res)^2), then in update_plot the color(zz, as probability) is updated.
w, h = figaspect(.8)
fig = plt.figure(figsize=(w,h))
ani = animation.FuncAnimation(fig, update_plot, interval = 1)
plt.show()
if __name__ == '__main__':
rospy.init_node('costmap_plot', anonymous=True)
rospy.Subscriber('/costmap0', numpy_nd_msg(Float32MultiArray), costmap_update)
rospy.Subscriber('/car0/state', numpy_nd_msg(Float32MultiArray), car_state_update)
rospy.Subscriber('/car0/base_pose_ground_truth', Odometry, update.update_car_odom)
costmap_plot(costmap[0])
while not rospy.is_shutdown():
try:
pass
except rospy.ROSInterruptException:
pass
def main():
global car_vel
obs_list = ['car0']
# initialize the map
map_res = rospy.get_param('/car1_nav/cmap_res', 0.1) # default is 1.0
map_size = rospy.get_param('/car1_nav/cmap_size', 45) # default is 25
update.init_map(map_res, map_size)
# car's initial movement
car_init_y_vel = rospy.get_param('/car1_nav/init_vel',
0.5) # default is 0.0
car_vel[0][1] = car_init_y_vel
# Initialize the node
rospy.init_node('solabot_commands', anonymous=True)
# ROS Publishers
# publish as numpy array using numpy_msg
pub_costmap = rospy.Publisher('/costmap1',
numpy_nd_msg(Float32MultiArray),
queue_size=5)
# data of the "car"
pub_car_vel = rospy.Publisher('/car1/cmd_vel', Twist, queue_size=5)
# ROS Subscribers
rospy.Subscriber('/car1/base_pose_ground_truth', Odometry,
update.update_car_odom)
for i in range(len(obs_list)):
rospy.Subscriber('/{0}/base_pose_ground_truth'.format(obs_list[i]),
Odometry, update.update_obs_odom, (i))
rate = rospy.Rate(1 / t_res) # default is 100
while not rospy.is_shutdown():
try:
pass
finally:
# update the costmap
update.update_costmap()
costmap = update.costmap
pub_costmap.publish(data=costmap)
# NOTE different drive_mode ?
prob_thresh = 0.0 # smaller the thresh, more careful the driver is
# avoid by prediction
lh_dist = 0.5 * car_vel[0][
1]**2 / brake + 0.5 # look ahead distance: able to stop with full break
# get col_prob
car_pose = update.car_pose
lh_pose = car_pose[0] + [
0, lh_dist * t_res
] # this line is calculated in every t_res
prob = get_col_prob(0, np.array([lh_pose]))
# NOTE: Different drive mode??? now is energy-saving mode
# avoid obstacle while try to maintain the initial(target)speed
if prob != 0:
if costmap.shape[0] > 1: # prediction mode
if col_prob_diff(0, np.array([
lh_pose
])) > 0: # obstacle approaching: is it ok to accelerate?
print("Obstacle coming!")
cross_dist = get_cross_dist(np.array([lh_pose]))
time_impact = get_impact_time(np.array([lh_pose]))
# whether car will collide with obs if it keeping at this car_vel
if cross_dist - car_vel[0][1] * time_impact > 0:
# NOTE: assume const. acceleration ! a = 1m/s (0.1m/t_res)
# calculate the root of t: 1/2*a*t**2 + V_0*t - S = 0
roots = np.roots([
1.0 / 2.0 * (accele), car_vel[0][1],
-cross_dist
])
# only one positive root since S > 0
root_t = np.amax(roots)
# accelerate OR decelerate
if root_t < time_impact:
accelerate() # accelerate
else:
decelerate() # decelerate
else:
# keep moving can pass the obstacle
pass
elif col_prob_diff(0, np.array(
[lh_pose])) < 0: # obstacle leaving
accelerate() # accelerate
else: # diff = 0 (prob = 1)
decelerate() # not going to move a bit
else: #NOTE: local planner!!! we don't have this yet
if prob == 1:
decelerate() # decelerate
# NO OBSTACLE AHEAD
else:
print("no obstacle ahead!!!!!!!!!!!!")
if car_vel[0][1] + accele <= car_init_y_vel:
accelerate()
elif car_vel[0][1] - brake >= car_init_y_vel:
decelerate()
twist = Twist()
twist.linear.x = 0
twist.linear.y = car_vel[0][1]
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
if True:
pub_car_vel.publish(twist)
else:
pass
rate.sleep()
def main():
global car_vel
obs_list = ['car0']
# initialize the map
map_res = rospy.get_param('/car1_nav/cmap_res', 0.1) # default is 1.0
map_size = rospy.get_param('/car1_nav/cmap_size', 45) # default is 25
update.init_map(map_res, map_size)
# car's initial movement
car_init_y_vel = rospy.get_param('/car1_nav/init_vel',
0.5) # default is 0.0
car_vel[0][1] = car_init_y_vel
# Initialize the node
rospy.init_node('solabot_commands', anonymous=True)
# ROS Publishers
# publish as numpy array using numpy_msg
pub_costmap = rospy.Publisher('/costmap1',
numpy_nd_msg(Float32MultiArray),
queue_size=5)
# data of the "car"
pub_car_vel = rospy.Publisher('/car1/cmd_vel', Twist, queue_size=5)
# pub the state of the ego vehicle
pub_car_state = rospy.Publisher('/car1/state',
numpy_nd_msg(Float32MultiArray),
queue_size=5)
# ROS Subscribers
rospy.Subscriber('/car1/base_pose_ground_truth', Odometry,
update.update_car_odom)
for i in range(len(obs_list)):
rospy.Subscriber('/{0}/base_pose_ground_truth'.format(obs_list[i]),
Odometry, update.update_obs_odom, (i))
rate = rospy.Rate(100) # default is 100
while not rospy.is_shutdown():
try:
pass
finally:
#################################################################################
##====================== BEGIN of POS Algorithm ===============================##
#################################################################################
### update the costmap
#pdb.set_trace()
update.update_costmap()
costmap = update.costmap
pub_costmap.publish(data=costmap)
### NOTE different drive_mode ?
prob_thresh = 0.0 # smaller the thresh, more careful the driver is
### Avoid by prediction (lh_dist in meters)
lh_dist = 0.5 * car_vel[0][
1]**2 / brake + 0.5 # look ahead distance: able to stop with full break
### Get the col_prob
car_pose = update.car_pose
lh_pose = car_pose[0] + [0, lh_dist * t_res
] # this is claculated in every t_res
### Expand the lh_pose from a point to a range (from car to lh_pose), to find out the closest lh_pose to obstacle (on costmap => prob > 0 )
min_lh_pose = car_pose[0]
max_lh_pose = lh_pose
lh_y_range = np.arange(min_lh_pose[1], max_lh_pose[1], map_res)
lh_pose_range = np.zeros((len(lh_y_range), 2))
for y_val in range(len(lh_y_range)):
lh_pose_range[y_val][0] = car_pose[0][0]
lh_pose_range[y_val][1] = lh_y_range[y_val]
prob = 0
unit_lh_pose = [0, 0]
for lh_p in lh_pose_range:
cand_prob = get_col_prob(0, np.array([lh_p]))
if cand_prob > 0:
prob = cand_prob
unit_lh_pose = lh_p
break
else:
pass
#--- slow down when approaching crossroad ---#
obs_pose = update.obs_pose
obs_vel = update.obs_vel
d_node = np.sqrt(
np.sum(np.power(np.subtract(obs_pose[0], [0, 0]), 2)))
v_obs = np.sqrt(np.sum(np.power(obs_vel[0], 2)))
if d_node < 5 and v_obs > 1:
decelerate()
### NOTE: Different drive mode??? now is energy-saving mode
### To avoid obstacle while try to maintain the initial(target)speed
if prob != 0:
if costmap.shape[0] > 1: # prediction mode
if col_prob_diff(0, np.array([
unit_lh_pose
])) > 0: # obstacle approaching: is it ok to accelerate?
print("Obstacle coming!")
pos = update.POS()
if pos > 0.8:
accelerate()
else:
cross_dist = get_cross_dist(
np.array([unit_lh_pose])) # in meters
time_impact = get_impact_time(
np.array([unit_lh_pose])) # in seconds
### The car will collide with obs if it keeping at this car_vel
if cross_dist - car_vel[0][1] * time_impact > 0:
### NOTE: assume const. acceleration ! a = 1m/s (0.1m/t_res)
### calculate the root of t: 1/2*a*t**2 + V_0*t - S = 0
roots = np.roots([
1.0 / 2.0 * (accele), car_vel[0][1],
-cross_dist
])
### Only one positive root since S > 0
root_t = np.amax(roots)
### accelerate OR decelerate
if root_t < time_impact:
accelerate() # accelerate
else:
decelerate() # decelerate
else:
### Keep moving can pass the obstacle
pass
elif col_prob_diff(0, np.array(
[unit_lh_pose])) < 0: # obstacle leaving
accelerate() # accelerate
else: # diff = 0 (prob = 1)
decelerate() # not going to move a bit
else: ###NOTE: local planner!!! we don't have this yet
if prob == 1:
decelerate() # decelerate
# NO OBSTACLE AHEAD
else:
print("no obstacle ahead!!!!!!!!!!!!")
if car_vel[0][1] + accele <= car_init_y_vel:
accelerate() # accelerate
elif car_vel[0][1] - brake >= car_init_y_vel:
decelerate() # decelerate
### Get car states
# [0]: min_lh_pose; [1]: max_lh_pose; [2]: final lh_pose
# [3]: prob at that final lh_pose; [4]: probability of stoppiong
car_state = np.zeros((8, ), dtype=np.float32)
car_state[0] = min_lh_pose[0]
car_state[1] = min_lh_pose[1]
car_state[2] = max_lh_pose[0]
car_state[3] = max_lh_pose[1]
car_state[4] = unit_lh_pose[0]
car_state[5] = unit_lh_pose[1]
car_state[6] = prob # it's risk actually
car_state[7] = update.POS() # probability of stopping
###############################################################################
##====================== END of POS Algorithm ===============================##
###############################################################################
twist = Twist()
twist.linear.x = 0
twist.linear.y = car_vel[0][1]
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
if True:
pub_car_vel.publish(twist)
pub_car_state.publish(data=car_state)
else:
pass
rate.sleep()
def costmap_update(raw_arr):
global costmap, map_size, map_res
costmap = raw_arr.data
def car_state_update(raw_data):
global car_state
car_state = raw_data.data
if __name__ == '__main__':
rospy.init_node('costmap_plot', anonymous=True)
rospy.Subscriber('/costmap1', numpy_nd_msg(Float32MultiArray),
costmap_update)
rospy.Subscriber('/car1/state', numpy_nd_msg(Float32MultiArray),
car_state_update)
rospy.Subscriber('/car1/base_pose_ground_truth', Odometry,
update.update_car_odom)
#---------------------------#
#-- Initialize the figure --#
#---------------------------#
fig = plt.figure(figsize=(6, 7))
#-----------------------------#
#-- Subplot default setting --#
#-----------------------------#