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)
