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()
from std_msgs.msg import Float32MultiArray # import modult for n-d array from nav_msgs.msg import Odometry from costmap_module.numpy_nd_msg import numpy_nd_msg from costmap_module import update from matplotlib.figure import figaspect import matplotlib.gridspec as gridspec # debug import pdb # initialize the map map_res = rospy.get_param('/car0_costmap_plot/cmap_res', 0.1) # default is 1.0 map_size = rospy.get_param('/car0_costmap_plot/cmap_size', 45) # default is 45 update.init_map(map_res, map_size) costmap = np.array(0.2 * np.random.randn(2, map_size/map_res, map_size/map_res) + 0.5, dtype=np.float32) car_pose_update = update.car_pose car_state = np.zeros((7,), dtype=np.float32) def update_plot(i): global car_pose_update, costmap , tryme # debug #pdb.set_trace()
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()