def callback_mocap(odometry_msg): global distance_list # Information from the lidar if not len(traj_x) == 0 and not len( distance_list ) == 0: # Computes position of the car only if trajectory and lidar data are available x_pos = odometry_msg.pose.pose.position.x y_pos = odometry_msg.pose.pose.position.y yaw = odometry_msg.pose.pose.orientation.z v = odometry_msg.twist.twist.linear.x state_m = State(x_pos, y_pos, yaw, v) ind = calc_target_index(state_m, traj_x, traj_y) if ind < len(traj_x) - 1: print("### RUNNING TRAJECTORY") dist_len = len(distance_list) true_distance_list = [ ] # To detect obstacles only a a portion of the lidar information is necessary for i in range(len(distance_list)): if i > dist_len / 6 and i < 5 * dist_len / 6: # Select the lidar information the car needs true_distance_list.append(distance_list[i]) min_dist = min( true_distance_list ) # Among the selected data, find the minimum distance which means the closest obstacle if min_dist < 0.60: # Condition to detect obstacles control_request = lli_ctrl_request() control_request.velocity = 0 # Stop the car if an obstacle has been detected ctrl_pub.publish( control_request ) # publish to control request, but only if near an obstacle print("obstacle in way") else: delta, ind = pure_pursuit_control(state_m, traj_x, traj_y, ind) target_pose = PointStamped() target_pose.header.stamp = rospy.Time.now() target_pose.header.frame_id = '/qualisys' target_pose.point.x = traj_x[ind] target_pose.point.y = traj_y[ind] target_pub.publish(target_pose) target_angle = max(-80, min(delta / (math.pi / 4) * 100, 80)) control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = target_angle else: print("### DONE WITH TRAJECTORY") print(len(distance_list)) control_request = lli_ctrl_request() control_request.velocity = 0 control_request.steering = 0 ctrl_pub.publish(control_request)
def callback_mocap(odometry_msg): # ask Frank # global pind #print("mocap") if not len(traj_x) == 0 and not len(distance_list) == 0: #while pind<len(traj_x): x_pos = odometry_msg.pose.pose.position.x y_pos = odometry_msg.pose.pose.position.y #print(x_pos) yaw = odometry_msg.pose.pose.orientation.z v = odometry_msg.twist.twist.linear.x #v=30 state_m = State(x_pos, y_pos, yaw, v) ind = calc_target_index(state_m, traj_x, traj_y) if ind < len(traj_x) - 1: print("### RUNNING TRAJECTORY") for i in range(len(distance_list)): distance = distance_list[i] print(distance) if distance < 1: control_request = lli_ctrl_request() control_request.velocity = 0 # put this in a controller node ctrl_pub.publish( control_request ) # publish to control request, but only if near an obstacle print("obstacle in way") else: delta, ind = pure_pursuit_control(state_m, traj_x, traj_y, ind) # pind = ind target_pose = PointStamped() target_pose.header.stamp = rospy.Time.now() target_pose.header.frame_id = 'qualisys' # target_pose.point.x = traj_x[pind] # target_pose.point.y = traj_y[pind] target_pose.point.x = traj_x[ind] target_pose.point.y = traj_y[ind] target_pub.publish(target_pose) target_angle = max(-80, min(delta / (math.pi / 4) * 100, 80)) #print(target_angle) control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = target_angle else: print("### DONE WITH TRAJECTORY") control_request = lli_ctrl_request() control_request.velocity = 0 control_request.steering = 0 ctrl_pub.publish(control_request)
def two_obstacles(ranges, angle_list): global a global a1 global b global b1 a = 0 a1 = 0 b1 = 0 b = 0 j = 0 left = ranges[ 0:len(ranges) / 2] # splitting list in half, not sure yet what is left and right part of list right = ranges[len(ranges) / 2:] left_angle = angle_list[0:len(angle_list) / 2] right_angle = angle_list[len(angle_list) / 2:] #for i in range(len(ranges)): if -(40 * math.pi / 180 ) <= left_angle[j] <= 0 and 0 < right_angle[j] <= 40 * math.pi / 180: if ranges[i] < 0.6: a = left[j] a1 = left_angle[j] #elif ranges[i]<0.6 and 0 < angle_list[i] <= 40*math.pi/180: b = right[j] b1 = right_angle[j] j += 1 w = math.sqrt( math.pow(a, 2) + math.pow(b, 2) - 2 * a * b * math.cos(a1 - b1)) w_c = 0.28 # width of car if w > w_c: control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = 0 ctrl_pub.publish(control_request) else: a_min = min(a1, b1) if a_min <= 0: control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = 40 * math.pi / 180 * 100 ctrl_pub.publish(control_request) else: control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = -40 * math.pi / 180 * 100 ctrl_pub.publish(control_request) print("TWO OBSTACLES IN WAY")
def __pure_pursuit(self): #savetxt("/home/nvidia/catkin_ws/planned_path.csv", array(self.path), delimiter = ",") rate = rospy.Rate(50) lli_msg = lli_ctrl_request() while len(self.path) > 0: if hasattr(self, 'car_pose'): while not (rospy.is_shutdown() or len(self.path) == 0): goal = self.choose_point() lli_msg.velocity, lli_msg.steering = self.controller(goal) # if not self.has_parking_spot: self.car_control_pub.publish(lli_msg) rate.sleep() # else: # return if self.going_backwards: self.__check_backwards_done() if self.doing_forward_parking: self.__check_backwards_done(forwards=True) # goal = self.path[0] lli_msg.velocity = 0 self.car_control_pub.publish(lli_msg) pose_arr = array([self.xs, self.ys]) print(self.xs[-1]) print(self.ys[-1])
def __init__(self): self.path = path_points('circle') # self.path = [[-1.439,-1.3683],[0.245,-1.88676]] # path = self.path # Subscribe to the topics self.car_pose_sub = rospy.Subscriber("SVEA1/odom", Odometry, self.car_pose_cb) rospy.loginfo(self.car_pose_sub) # init Publisher self.car_control_pub = rospy.Publisher("lli/ctrl_request", lli_ctrl_request, queue_size=10) rate = rospy.Rate(10) # goal = self.path[0] lli_msg = lli_ctrl_request() lli_msg.velocity = speed while self.path != []: goal = self.path[0] if hasattr(self, 'car_pose'): while not (rospy.is_shutdown() or self.reach_goal(goal)): lli_msg.steering = -self.controller(goal) self.car_control_pub.publish(lli_msg) rate.sleep() self.path.remove(goal) # goal = self.path[0] lli_msg.velocity = 0 self.car_control_pub.publish(lli_msg)
def callback_lidar(scan): if not len(pos_x) == 0: #both subscribers dont start same time for i in range(len(scan.ranges)): robot_yaw = yaw_list[-1] distance = scan.ranges[i] angle = scan.angle_min + robot_yaw + i * scan.angle_increment if distance > scan.range_min and distance < scan.range_max: # dont accept scans that are outside ranges # print(scan.range_max) # print(scan.range_min) x_o = np.cos(angle) * distance y_o = np.sin(angle) * distance x_robot = pos_x[-1] y_robot = pos_y[-1] dx = x_o - x_robot dy = y_o - y_robot dist = math.sqrt(math.pow(dx, 2) + math.pow(dy, 2)) if dist<1: target_speed = 0 # speed 0 if too close to obstacle target_angle=0 # not needeed control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = target_angle pub.publish(control_request) #publish to control request, but only if near an obstacle
def callback(vel_msg): linear_vel = vel_msg.linear.x angular_vel = vel_msg.angular.z ctrl_request_msg = lli_ctrl_request() ctrl_request_msg.velocity = int(linear_vel) ctrl_request_msg.steering = int(angular_vel) pub.publish(ctrl_request_msg)
def __init__(self): self.path = path_points('ellipse') # self.path = [[-1.439,-1.3683],[0.245,-1.88676]] # path = self.path # Subscribe to the topics self.car_pose_sub = rospy.Subscriber("SVEA1/odom", Odometry, self.car_pose_cb) rospy.loginfo(self.car_pose_sub) # init Publisher self.car_control_pub = rospy.Publisher("lli/ctrl_request", lli_ctrl_request, queue_size=10) rate = rospy.Rate(10) # goal = self.path[0] lli_msg = lli_ctrl_request() lli_msg.velocity = speed self.ld = 0.5 self.xs = [] self.ys = [] while len(self.path) > 0: if hasattr(self, 'car_pose'): while not (rospy.is_shutdown() or len(self.path) == 0): goal = self.choose_point() lli_msg.steering = -self.controller(goal) self.car_control_pub.publish(lli_msg) rate.sleep() # goal = self.path[0] lli_msg.velocity = 0 self.car_control_pub.publish(lli_msg) pose_arr = array([self.xs, self.ys]) savetxt("/home/nvidia/catkin_ws/real_path.csv", pose_arr, delimiter=",")
def callback(twist_msg): linear_x = twist_msg.linear.x angular_z = twist_msg.angular.z control_request = lli_ctrl_request() control_request.steering = angular_z control_request.velocity = linear_x pub.publish(control_request)
def publish_control(self, steering_degree, velocity): linear_control = velocity #check the map to vel angular_control = steering_degree * ( 400 / math.pi) * 0.2 + 0.8 * self.start_steering self.start_steering = angular_control ctrl_request_msg = lli_ctrl_request() ctrl_request_msg.velocity = int(linear_control) ctrl_request_msg.steering = int(angular_control) self.pub_steer_control.publish(ctrl_request_msg)
def callback_mocap(odometry_msg): if not len( traj_x ) == 0: # Hold callback for mocap until callback for trajectory finishes x_pos = odometry_msg.pose.pose.position.x # Mocap data about Car y_pos = odometry_msg.pose.pose.position.y yaw = odometry_msg.pose.pose.orientation.z v = odometry_msg.twist.twist.linear.x state_m = State(x_pos, y_pos, yaw, v) ind = calc_target_index( state_m, traj_x, traj_y) # Get index from current Mocap data about car if ind < len(traj_x) - 1: print("### RUNNING TRAJECTORY") delta, ind = pure_pursuit_control(state_m, traj_x, traj_y, ind) target_pose = PointStamped( ) # Pure pursuit target pose Re-publishing target_pose.header.stamp = rospy.Time.now() target_pose.header.frame_id = 'qualisys' target_pose.point.x = traj_x[ind] target_pose.point.y = traj_y[ind] target_pub.publish(target_pose) target_angle = max(-80, min(delta / (math.pi / 4) * 100, 80)) control_request = lli_ctrl_request( ) # Low level interface settings of speed and angle while running control_request.velocity = target_speed control_request.steering = target_angle else: print("### DONE WITH TRAJECTORY") control_request = lli_ctrl_request( ) # Low level interface settings while done control_request.velocity = 0 control_request.steering = 0 ctrl_pub.publish(control_request)
def callback(data): global speed steering = controller(data) if steering > 100: steering = 100 if steering < -100: steering = -100 lli_msg = lli_ctrl_request() lli_msg.velocity = speed lli_msg.steering = steering control.publish(lli_msg)
def change_to_reversed(self): lli_msg = lli_ctrl_request() lli_msg.velocity = - 10 self.car_control_pub.publish(lli_msg) rospy.sleep(0.1) lli_msg.velocity = 0 self.car_control_pub.publish(lli_msg) rospy.sleep(0.1) lli_msg.velocity = - 10 self.car_control_pub.publish(lli_msg) rospy.sleep(0.1) self.reversed = True
def publish_control(self, steering_degree, target_ind, velocity): linear_control = velocity #check the map to vel angular_control = steering_degree * ( 400 / math.pi) * 0.9 + 0.1 * self.start_steering self.start_steering = angular_control ctrl_request_msg = lli_ctrl_request() ctrl_request_msg.velocity = int(linear_control) ctrl_request_msg.steering = int(angular_control) self.pub_steer_control.publish(ctrl_request_msg) msg = Float32() msg.data = angular_control self.pub_steer_debug.publish(msg)
def steer_from_lists(self, steerings, times): self.change_to_reversed() start = time.time() lli_msg = lli_ctrl_request() lli_msg.velocity = -17 time_elapsed = 0 i = 0 while time_elapsed < times[-1]: while times[i] < time_elapsed: i += 1 lli_msg.steering = -int(100 * steerings[i]/(pi/4)) self.car_control_pub.publish(lli_msg) rospy.sleep(0.05) time_elapsed = time.time() - start
def lidar_cb(data): speed = 10 msg = lli_ctrl_request() msg.velocity = speed angles = arange(data.angle_min, data.angle_max + data.angle_increment, data.angle_increment) #print(angles) ranges = data.ranges threshold_dist = 1 for i in range(len(angles)): if abs(angles[i]) > pi - pi / 4: if ranges[i] < threshold_dist: speed = -10 msg.velocity = speed car_speed.publish(msg)
def __pure_pursuit(self): lli_msg = lli_ctrl_request() lli_msg.velocity = speed savetxt("/home/nvidia/El2425_Automatic_Control_Group1/planned_path.csv", array(self.path), delimiter=",") while len(self.path) > 0: if hasattr(self, 'car_pose'): while not (rospy.is_shutdown() or len(self.path) == 0): goal = self.choose_point() lli_msg.velocity,lli_msg.steering = self.controller(goal) self.car_control_pub.publish(lli_msg) self.rate.sleep() # goal = self.path[0] lli_msg.velocity = 0 self.car_control_pub.publish(lli_msg) pose_arr = array([self.xs, self.ys]) savetxt("/home/nvidia/El2425_Automatic_Control_Group1/real_path.csv", pose_arr, delimiter=",")
def __init__(self, vehicle_name=""): self.vehicle_name = vehicle_name self.ctrl_request = lli_ctrl_request() # Stores the last controls sent self.ctrl_msg = lli_ctrl() # Message to send to ROS self.last_ctrl_update = rospy.get_time() self.is_stop = False self.is_emergency = False self.is_reverse = False self.is_ready = False # log of control requests and control actuated self.ctrl_request_log = [] self.ctrl_actuated_log = [] self.remote_log = []
def __pure_pursuit(self): rate = rospy.Rate(50) lli_msg = lli_ctrl_request() while len(self.path) > 0: if hasattr(self, 'car_pose'): while not (rospy.is_shutdown() or len(self.path) == 0): goal = self.choose_point() lli_msg.velocity, lli_msg.steering = self.controller(goal) self.car_control_pub.publish(lli_msg) rate.sleep() if self.going_backwards: self.__check_backwards_done() if self.doing_forward_parking: self.__check_backwards_done(forwards=True) lli_msg.velocity = 0 self.car_control_pub.publish(lli_msg) pose_arr = array([self.xs, self.ys])
def __init__(self): self.path = path_points('linear') self.Estop = 0 self.parking = 0 self.parking_lot_dist = 0 self.car_heading = 0 # Subscribe to the topics self.car_pose_sub = rospy.Subscriber("SVEA1/odom", Odometry, self.car_pose_cb) self.Lidar_sub = rospy.Subscriber("/scan", LaserScan, self.lidar_cb) self.has_parking_spot = False rospy.loginfo(self.car_pose_sub) # init Publisher self.car_control_pub = rospy.Publisher("lli/ctrl_request", lli_ctrl_request, queue_size=10) rate = rospy.Rate(10) # goal = self.path[0] lli_msg = lli_ctrl_request() lli_msg.velocity = speed self.ld = 0.5 self.xs = [] self.ys = [] while len(self.path) > 0: if hasattr(self, 'car_pose'): while not (rospy.is_shutdown() or len(self.path) == 0): if not self.has_parking_spot: goal = self.choose_point() lli_msg.velocity,lli_msg.steering = self.controller(goal) self.car_control_pub.publish(lli_msg) else: lli_msg.velocity, lli_msg.steering = self.parallell_parking_start() self.car_control_pub.publish(lli_msg) break rate.sleep() # goal = self.path[0] lli_msg.velocity = 0 self.car_control_pub.publish(lli_msg) pose_arr = array([self.xs, self.ys]) savetxt("/home/nvidia/catkin_ws/real_path.csv", pose_arr, delimiter=",")
def callback_mocap(odometry_msg): global ranges global d_min if not len(traj_x) == 0 and not len(ranges) == 0: x_pos = odometry_msg.pose.pose.position.x y_pos = odometry_msg.pose.pose.position.y yaw = odometry_msg.pose.pose.orientation.z v = odometry_msg.twist.twist.linear.x state_m = State(x_pos, y_pos, yaw, v) ind = calc_target_index(state_m, traj_x, traj_y) min_dist = min(ranges) dist_tg = dist_target(state_m, traj_x, traj_y) if min_dist < 0.6 and dist_tg > d_min: angle_list = [] for i in range( len(ranges) ): # the program might be checking in each increment angle if there is obstacle in the zone angle = angle_min + i * increment angle_list.append(angle) target_speed = 95 abs_angle = 180 * (abs(angle_list[i])) / math.pi range_limit = -0.004751131 + 0.6423077 # Want the car to turn 45[deg] so here it calculates [rad] since LidarScan and car uses [rad] then mutliply by 100 because the car takes in percentage to steer if -(90 * math.pi / 180) <= angle_list[i] <= -(70 * math.pi / 180): if ranges[i] < range_limit: y_steering = 180 * 0.4751131 * (abs( angle_list[i])) / (math.pi) - 59.23077 if angle_list[i] < 0: y_steering = -y_steering control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = (y_steering * math.pi / 180) * 100 ctrl_pub.publish(control_request) #control_request.steering = (15*math.pi/180)*100 if -(70 * math.pi / 180) < angle_list[i] <= -(50 * math.pi / 180): if ranges[i] < range_limit: y_steering = 180 * 0.4751131 * (abs( angle_list[i])) / (math.pi) - 59.23077 if angle_list[i] < 0: y_steering = -y_steering control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = (y_steering * math.pi / 180) * 100 ctrl_pub.publish(control_request) #control_request.steering = (25*math.pi/180)*100 if -(50 * math.pi / 180) < angle_list[i] <= -(30 * math.pi / 180): if ranges[i] < range_limit: y_steering = 180 * 0.4751131 * (abs( angle_list[i])) / (math.pi) - 59.23077 if angle_list[i] < 0: y_steering = -y_steering control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = (y_steering * math.pi / 180) * 100 ctrl_pub.publish(control_request) #control_request.steering = (35*math.pi/180)*100 if -(30 * math.pi / 180) < angle_list[i] <= -(10 * math.pi / 180): if ranges[i] < range_limit: y_steering = 180 * 0.4751131 * (abs( angle_list[i])) / (math.pi) - 59.23077 if angle_list[i] < 0: y_steering = -y_steering control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = (y_steering * math.pi / 180) * 100 ctrl_pub.publish(control_request) #control_request.steering = (45*math.pi/180)*100 if -(10 * math.pi / 180) < angle_list[i] <= (10 * math.pi / 180): if ranges[i] < range_limit: y_steering = 180 * 0.4751131 * (abs( angle_list[i])) / (math.pi) - 59.23077 # if angle_list[i] < 0: #y_steering = -y_steering control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = (y_steering * math.pi / 180) * 100 ctrl_pub.publish(control_request) #control_request.steering = -(55*math.pi/180)*100 #else: #if ranges[i] < 0.15: # while min_dist < 0.15: # control_request = lli_ctrl_request() #control_request.velocity = 0 #control_request.steering = 0 #ctrl_pub.publish(control_request) #print ("Too close to steer!") if (10 * math.pi / 180) < angle_list[i] <= (30 * math.pi / 180): if ranges[i] < range_limit: y_steering = 180 * 0.4751131 * (abs( angle_list[i])) / (math.pi) - 59.23077 control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = (y_steering * math.pi / 180) * 100 #control_request.steering = -(45*math.pi/180)*100 ctrl_pub.publish(control_request) if (30 * math.pi / 180) < angle_list[i] <= (50 * math.pi / 180): if ranges[i] < range_limit: y_steering = 180 * 0.4751131 * (abs( angle_list[i])) / (math.pi) - 59.23077 control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = (y_steering * math.pi / 180) * 100 #control_request.steering = -(35*math.pi/180)*100 ctrl_pub.publish(control_request) if (50 * math.pi / 180) < angle_list[i] <= (70 * math.pi / 180): if ranges[i] < range_limit: y_steering = 180 * 0.4751131 * (abs( angle_list[i])) / (math.pi) - 59.23077 control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = (y_steering * math.pi / 180) * 100 #control_request.steering = -(25*math.pi/180)*100 ctrl_pub.publish(control_request) if (70 * math.pi / 180) <= angle_list[i] <= (90 * math.pi / 180): if ranges[i] < range_limit: y_steering = 180 * 0.4751131 * (abs( angle_list[i])) / (math.pi) - 59.23077 control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = (y_steering * math.pi / 180) * 100 #control_request.steering = -(15*math.pi/180)*100 ctrl_pub.publish(control_request) #elif min_dist < 0.15: # while min_dist < 0.15: # control_request = lli_ctrl_request() # control_request.velocity = 0 # control_request.steering = 0 #ctrl_pub.publish(control_request) # print ("emergency stop!") else: if ind < len(traj_x) - 1: print('Running Trajectory') ind = calc_target_index(state_m, traj_x, traj_y) delta, ind = pure_pursuit_control(state_m, traj_x, traj_y, ind) target_pose = PointStamped() target_pose.header.stamp = rospy.Time.now() target_pose.header.frame_id = '/qualisys' target_pose.point.x = traj_x[ind] target_pose.point.y = traj_y[ind] target_pub.publish(target_pose) target_speed = 100 target_angle = max(-80, min(delta / (math.pi / 4) * 100, 80)) control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = target_angle ctrl_pub.publish(control_request) #elif min_dist < 0.15: #while min_dist < 0.2: #control_request = lli_ctrl_request() #control_request.velocity = 0 #control_request.steering = 0 #ctrl_pub.publish(control_request) #print ("emergency stop!") else: print("### DONE WITH TRAJECTORY") control_request = lli_ctrl_request() control_request.velocity = 0 control_request.steering = 0 ctrl_pub.publish(control_request)
def callback_mocap(odometry_msg): global ranges global d_min if not len(traj_x) == 0 and not len(ranges) == 0: x_pos = odometry_msg.pose.pose.position.x y_pos = odometry_msg.pose.pose.position.y yaw = odometry_msg.pose.pose.orientation.z v = odometry_msg.twist.twist.linear.x state_m = State(x_pos, y_pos, yaw, v) ind = calc_target_index(state_m, traj_x, traj_y) min_dist = min(ranges) dist_tg = dist_target(state_m, traj_x, traj_y) if min_dist < 0.6 and dist_tg > d_min: angle_list = [] for i in range( len(ranges) ): # the program might be checking in each increment angle if there is obstacle in the zone angle = angle_min + i * increment angle_list.append(angle) #animation=animation.FuncAnimation(fig,update_hist, 720, fargs =(angles_list,)) #plt.show() control_request = lli_ctrl_request() control_request.velocity = 25 # Want the car to turn 45[deg] so here it calculates [rad] since LidarScan and car uses [rad] then mutliply by 100 because the car takes in percentage to steer if -(95 * math.pi / 180) <= angle_list[i] <= -(70 * math.pi / 180): if ranges[i] < 0.2: control_request.steering = (15 * math.pi / 180) * 100 ctrl_pub.publish(control_request) #elif 0.2 < ranges[i] < 0.4: # control_request.steering = 0 #else: # control_request.steering = target_angle if -(70 * math.pi / 180) < angle_list[i] <= -(50 * math.pi / 180): if ranges[i] < 0.3: control_request.steering = (25 * math.pi / 180) * 100 ctrl_pub.publish(control_request) #else: # control_request.steering = target_angle if -(50 * math.pi / 180) < angle_list[i] <= -(30 * math.pi / 180): if ranges[i] < 0.4: control_request.steering = (35 * math.pi / 180) * 100 ctrl_pub.publish(control_request) #else: # control_request.steering = target _angle if -(30 * math.pi / 180) < angle_list[i] <= -(10 * math.pi / 180): if ranges[i] < 0.5: control_request.steering = (45 * math.pi / 180) * 100 ctrl_pub.publish(control_request) #else: # control_request.steering = -(15*math.py/180)*100 if -(10 * math.pi / 180) < angle_list[i] <= (10 * math.pi / 180): if ranges[i] < 0.6: control_request.steering = -(55 * math.pi / 180) * 100 ctrl_pub.publish(control_request) #else: # control_request.steering = target_angle if (10 * math.pi / 180) < angle_list[i] <= (30 * math.pi / 180): if ranges[i] < 0.5: control_request.steering = -(45 * math.pi / 180) * 100 ctrl_pub.publish(control_request) #else: # control_request.steering = target_angle if (30 * math.pi / 180) < angle_list[i] <= (50 * math.pi / 180): if ranges[i] < 0.4: control_request.steering = -(35 * math.pi / 180) * 100 ctrl_pub.publish(control_request) #else: # control_request.steering = target_angle if (50 * math.pi / 180) < angle_list[i] <= (70 * math.pi / 180): if ranges[i] < 0.3: control_request.steering = -(25 * math.pi / 180) * 100 ctrl_pub.publish(control_request) #else: # control_request.steering = target _angle if (70 * math.pi / 180) <= angle_list[i] <= (95 * math.pi / 180): if ranges[i] < 0.2: control_request.steering = -(15 * math.pi / 180) * 100 ctrl_pub.publish(control_request) #elif 0.2 < ranges[i] < 0.4: # control_request.steering = 0 #else: # control_request.steering = (15*math.py/180)*100 # else: # control_request_steering = 0 #ctrl_pub.publish(control_request) else: if ind < len(traj_x) - 1: print('Running Trajectory') ind = calc_target_index(state_m, traj_x, traj_y) delta, ind = pure_pursuit_control(state_m, traj_x, traj_y, ind) target_pose = PointStamped() target_pose.header.stamp = rospy.Time.now() target_pose.header.frame_id = '/qualisys' target_pose.point.x = traj_x[ind] target_pose.point.y = traj_y[ind] target_pub.publish(target_pose) target_angle = max(-80, min(delta / (math.pi / 4) * 100, 80)) control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = target_angle ctrl_pub.publish(control_request) else: print("### DONE WITH TRAJECTORY") control_request = lli_ctrl_request() control_request.velocity = 0 control_request.steering = 0 ctrl_pub.publish(control_request)
#! /usr/bin/env python import rospy from low_level_interface.msg import lli_ctrl_request rospy.init_node('cs_publisher') pub = rospy.Publisher('/lli/ctrl_request',lli_ctrl_request,queue_size=1) rate = rospy.Rate(50) control_request = lli_ctrl_request() control_request.velocity = 30 theta=50 while not rospy.is_shutdown control_request.steering = theta pub.publish(control_request) rate.sleep()
def callback_mocap(odometry_msg): global ranges global d_min global i if not len(traj_x) == 0 and not len(ranges) == 0: x_pos = odometry_msg.pose.pose.position.x y_pos = odometry_msg.pose.pose.position.y yaw = odometry_msg.pose.pose.orientation.z v = odometry_msg.twist.twist.linear.x state_m = State(x_pos, y_pos, yaw, v) print("stuck1") ind = calc_target_index(state_m, traj_x, traj_y) min_dist = min(ranges) dist_tg = dist_target(state_m, traj_x, traj_y) if min_dist < 0.6 and dist_tg > d_min: angle_list = [] control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = 0 ctrl_pub.publish(control_request) for i in range( len(ranges) ): # the program might be checking in each increment angle if there is obstacle in the zone angle = angle_min + i * increment angle_list.append(angle) two_obstacles(ranges, angle_list) if -(90 * math.pi / 180) <= angle_list[i] <= -(70 * math.pi / 180): print("-90 to -70") if ranges[i] < 0.2: print("-90 to -70 and range less than 0.2") control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = (15 * math.pi / 180) * 100 ctrl_pub.publish(control_request) if -(70 * math.pi / 180) < angle_list[i] <= -(50 * math.pi / 180): print("-70 to -50") if ranges[i] < 0.3: print("-70 to -50 and range less than 0.3") control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = (25 * math.pi / 180) * 100 ctrl_pub.publish(control_request)\ if -(50 * math.pi / 180) < angle_list[i] <= -(30 * math.pi / 180): print("-50 to -30") if ranges[i] < 0.4: print("-50 to -30 and range less than 0.4") control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = (35 * math.pi / 180) * 100 ctrl_pub.publish(control_request) if -(30 * math.pi / 180) < angle_list[i] <= -(10 * math.pi / 180): print("-30 to -10") if ranges[i] < 0.5: print("-30 to -10 and range less than 0.5") control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = (45 * math.pi / 180) * 100 ctrl_pub.publish(control_request) if -(10 * math.pi / 180) < angle_list[i] <= (10 * math.pi / 180): print("-10 to 10") if ranges[i] < 0.6: print("-10 to 10 and range less than 0.6") control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = -(55 * math.pi / 180) * 100 ctrl_pub.publish(control_request) if (10 * math.pi / 180) < angle_list[i] <= (30 * math.pi / 180): print("10 to 30") if ranges[i] < 0.5: print("10 to 30 and range less than 0.5") control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = -(45 * math.pi / 180) * 100 ctrl_pub.publish(control_request) if (30 * math.pi / 180) < angle_list[i] <= (50 * math.pi / 180): print("30 to 50") if ranges[i] < 0.4: print("30 to 50 and range less than 0.4") control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = -(35 * math.pi / 180) * 100 ctrl_pub.publish(control_request) if (50 * math.pi / 180) < angle_list[i] <= (70 * math.pi / 180): print("50 to 70") if ranges[i] < 0.3: print("50 to 70 and range less than 0.3") control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = -(25 * math.pi / 180) * 100 ctrl_pub.publish(control_request) if (70 * math.pi / 180) <= angle_list[i] <= (90 * math.pi / 180): print("70 to 90") if ranges[i] < 0.2: print("70 to 90 and range less than 0.2") control_request = lli_ctrl_request() control_request.velocity = 25 control_request.steering = -(15 * math.pi / 180) * 100 ctrl_pub.publish(control_request) elif min_dist < 0.15: while min_dist < 0.2: control_request = lli_ctrl_request() control_request.velocity = 0 control_request.steering = 0 ctrl_pub.publish(control_request) print("emergency stop!") #two_obstacles(ranges, angle_list) else: #control_request = lli_ctrl_request() # think the car stopped when it #control_request.velocity = 20 # passed all obstacles, so this #control_request.steering = 0 # should bump it a bit forward #ctrl_pub.publish(control_request) # and prevent it to be stopped if ind < len(traj_x) - 1: print('Running Trajectory') ind = calc_target_index(state_m, traj_x, traj_y) print("stuck2") delta, ind = pure_pursuit_control(state_m, traj_x, traj_y, ind) target_pose = PointStamped() target_pose.header.stamp = rospy.Time.now() target_pose.header.frame_id = '/qualisys' target_pose.point.x = traj_x[ind] target_pose.point.y = traj_y[ind] target_pub.publish(target_pose) target_angle = max(-80, min(delta / (math.pi / 4) * 100, 80)) control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = target_angle ctrl_pub.publish(control_request) elif min_dist < 0.15: while min_dist < 0.2: control_request = lli_ctrl_request() control_request.velocity = 0 control_request.steering = 0 ctrl_pub.publish(control_request) print("emergency stop!") else: print("### DONE WITH TRAJECTORY") control_request = lli_ctrl_request() control_request.velocity = 0 control_request.steering = 0 ctrl_pub.publish(control_request)
def callback_mocap(odometry_msg): global ranges global d_min global emergency_threshold if not len(traj_x) == 0 and not len(ranges) == 0: x_pos = odometry_msg.pose.pose.position.x y_pos = odometry_msg.pose.pose.position.y yaw = odometry_msg.pose.pose.orientation.z v = odometry_msg.twist.twist.linear.x state_m = State(x_pos, y_pos, yaw, v) ind = calc_target_index(state_m, traj_x, traj_y) clipped_range = ranges[len(ranges) / 3:2 * len(ranges) / 3] min_dist = min(clipped_range) # min_index = ranges.index(min(ranges)) min_index = ranges.index(min_dist) dist_tg = dist_target(state_m, traj_x, traj_y) left_obs, right_obs = 0, 0 for i, dist in enumerate(clipped_range): if i < len(clipped_range) / 2 and dist < dist_threshold: left_obs += 1 if i > len(ranges) / 2 and dist < dist_threshold: right_obs += 1 # else: # if ind < len(traj_x)-1 or dist_tg > d_min: if dist_tg > d_min: # if min_dist < dist_threshold and dist_tg > d_min: if min_dist < dist_threshold: if min_dist < emergency_threshold: rospy.loginfo_throttle(2, "Emergency Stop") control_request = lli_ctrl_request() control_request.velocity = 0 control_request.steering = 0 ctrl_pub.publish(control_request) # break else: rospy.loginfo_throttle( 0.5, "Avoiding; " + "{0} left pts, {1} right pts".format( left_obs, right_obs)) angle_list = [] for i in range( len(ranges) ): # the program might be checking in each increment angle if there is obstacle in the zone angle = angle_min + i * increment angle_list.append(angle) #animation=animation.FuncAnimation(fig,update_hist, 720, fargs =(angles_list,)) #plt.show() control_request = lli_ctrl_request() control_request.velocity = 35 min_angle_abs = 180 * (abs( angle_list[min_index])) / (math.pi) if left_obs <= right_obs: y_steering = 0.4751131 * min_angle_abs - 59.23077 if left_obs > right_obs: y_steering = 0.4751131 * min_angle_abs - 59.23077 y_steering = -y_steering range_limit = -0.004751131 * min_angle_abs + 0.6423077 # min_angle_abs = 180*(abs(angle_list[min_index]))/(math.pi) # y_steering = 0.4751131*min_angle_abs - 59.23077 # if angle_list[min_index]<-0.1: #negative values # y_steering = -y_steering # range_limit = -0.004751131*min_angle_abs + 0.6423077 if ranges[min_index] < range_limit: control_request.steering = (y_steering * (math.pi) / 180) * 100 ctrl_pub.publish(control_request) else: rospy.loginfo_throttle( 2, "Running Trajectory; " + "{0} meters from target".format(dist_tg)) # print('Running Trajectory') ind = calc_target_index(state_m, traj_x, traj_y) delta, ind = pure_pursuit_control(state_m, traj_x, traj_y, ind) if ind < len(traj_x): target_pose = PointStamped() target_pose.header.stamp = rospy.Time.now() target_pose.header.frame_id = '/qualisys' target_pose.point.x = traj_x[ind] target_pose.point.y = traj_y[ind] target_pub.publish(target_pose) target_angle = max(-80, min(delta / (math.pi / 4) * 100, 80)) control_request = lli_ctrl_request() control_request.velocity = target_speed control_request.steering = target_angle ctrl_pub.publish(control_request) else: control_request = lli_ctrl_request() control_request.velocity = 0 control_request.steering = 0 ctrl_pub.publish(control_request) else: # print("### DONE WITH TRAJECTORY") rospy.loginfo_throttle(2, "### DONE WITH TRAJECTORY") control_request = lli_ctrl_request() control_request.velocity = 0 control_request.steering = 0 ctrl_pub.publish(control_request)