def GoEasy(direc):
if direc == 0:
easyGo.mvCurve(-SPEED, 0)
elif direc == 1:
#print("COME HERE")
easyGo.mvCurve(SPEED, 0)
elif direc == 2:
#print("COME HERE2")
easyGo.mvRotate(ROTATE_SPEED, -1, False)
elif direc == 3:
easyGo.mvRotate(ROTATE_SPEED, -1, True)
def easy_drive(goal_x, goal_y, realposition, velRobot): #realposition == current_position(x, y)
Kp = 1.7375 / 10
current_angle = easyVector.get_angle() ##y_axis == 0 degree, CW -> 0 ~ +180 degree
#print('current_angle :', current_angle)
if goal_y-realposition[1]>=0 : # pi/2 <= desired_angle <= pi/2
desired_angle = -math.atan2(goal_x-realposition[0],
goal_y-realposition[1])*180/PI
#print('desired_angle :', desired_angle)
desired_steer = easyVector.get_steer_Value(desired_angle, velRobot)
else : #abs(desired_angle) >= pi/2
desired_angle = -math.atan2(goal_x-realposition[0],
goal_y-realposition[1])*180/PI
#print('desired_angle :', desired_angle)
desired_steer = easyVector.get_steer_Value(desired_angle, velRobot)
#print('desired_angle :', desired_angle)
if rospy.get_param('/dap_handle'):
easyGo.mvCurve(velRobot, desired_steer)
###############Steer visualization###############
Steer_Visualization(desired_steer)
def easy_drive(goal_x, goal_y, realposition):
desired_vector = easyVector.main(
math.atan2(goal_y - realposition[0], goal_x - realposition[1]))
easyGo.mvCurve(Robot_speed, desired_vector)
def main():
parser = argparse.ArgumentParser('Parse configuration file')
parser.add_argument('--env_config', type=str, default='configs/env.config')
parser.add_argument('--policy_config',
type=str,
default='configs/policy.config')
parser.add_argument('--policy', type=str, default='orca')
parser.add_argument('--model_dir', type=str, default=None)
parser.add_argument('--il', default=False, action='store_true')
parser.add_argument('--gpu', default=False, action='store_true')
parser.add_argument('--visualize', default=True, action='store_true')
parser.add_argument('--phase', type=str, default='test')
parser.add_argument('--test_case', type=int, default=0)
parser.add_argument('--square', default=False, action='store_true')
parser.add_argument('--circle', default=False, action='store_true')
parser.add_argument('--video_file', type=str, default=None)
parser.add_argument('--traj', default=False, action='store_true')
parser.add_argument('--plot', default=False, action='store_true')
args = parser.parse_args()
if args.model_dir is not None:
env_config_file = os.path.join(args.model_dir,
os.path.basename(args.env_config))
policy_config_file = os.path.join(args.model_dir,
os.path.basename(args.policy_config))
if args.il:
model_weights = os.path.join(args.model_dir, 'il_model.pth')
else:
if os.path.exists(
os.path.join(args.model_dir, 'resumed_rl_model.pth')):
model_weights = os.path.join(args.model_dir,
'resumed_rl_model.pth')
else:
model_weights = os.path.join(args.model_dir, 'rl_model.pth')
else:
env_config_file = args.env_config
policy_config_file = args.env_config
# configure logging and device
logging.basicConfig(level=logging.INFO,
format='%(asctime)s, %(levelname)s: %(message)s',
datefmt="%Y-%m-%d %H:%M:%S")
device = torch.device(
"cuda:0" if torch.cuda.is_available() and args.gpu else "cpu")
logging.info('Using device: %s', device)
# configure policy
policy = policy_factory[args.policy]()
policy_config = configparser.RawConfigParser()
policy_config.read(policy_config_file)
policy.configure(policy_config)
if policy.trainable:
if args.model_dir is None:
parser.error(
'Trainable policy must be specified with a model weights directory'
)
policy.get_model().load_state_dict(
torch.load(model_weights, map_location=torch.device('cpu')))
# configure environment
env_config = configparser.RawConfigParser()
env_config.read(env_config_file)
env = gym.make('CrowdSim-v0')
env.configure(env_config)
if args.square:
env.test_sim = 'square_crossing'
if args.circle:
env.test_sim = 'circle_crossing'
robot = Robot(env_config, 'robot')
robot.set_policy(policy)
env.set_robot(robot)
explorer = Explorer(env, robot, device, gamma=0.9)
policy.set_phase(args.phase)
policy.set_device(device)
# set safety space for ORCA in non-cooperative simulation
if isinstance(robot.policy, ORCA):
if robot.visible:
robot.policy.safety_space = 0
else:
# because invisible case breaks the reciprocal assumption
# adding some safety space improves ORCA performance. Tune this value based on your need.
robot.policy.safety_space = 0
logging.info('ORCA agent buffer: %f', robot.policy.safety_space)
policy.set_env(env)
robot.print_info()
if args.visualize:
obs = env.reset(args.phase, args.test_case)
done = False
last_pos = np.array(robot.get_position())
policy_time = 0.0
numFrame = 0
dist = 20.0
obs_flg = 0
sim_time = False
while sim_time == False:
samples, robot_state, sim_time = pc2obs.pc2obs(
voxel_size=voxel_size)
t1 = float(sim_time)
while (dist > 0.8):
#t1 = time.time()
env.humans = []
samples, robot_state, sim_time = pc2obs.pc2obs(
voxel_size=voxel_size)
if type(samples) == type(False):
print("Not Connect")
continue
dist = math.sqrt((GOAL_X - robot_state[0])**2 +
(GOAL_Y - robot_state[1])**2)
if obs_flg == 0 and dist < 10:
os.system('sh ./init.sh')
obs_flg = 1
print("dist: {}".format(dist))
# rotate and shift obs position
t2 = time.time()
yaw = robot_state[2]
rot_matrix = np.array([[math.cos(yaw),
math.sin(yaw)],
[-math.sin(yaw),
math.cos(yaw)]])
#samples = np.array([sample + robot_state[0:2] for sample in samples[:,0:2]])
if len(samples) == 1:
samples = np.array(
[np.dot(rot_matrix, samples[0][0:2]) + robot_state[0:2]])
print(samples)
elif len(samples) > 1:
samples = np.array([
np.dot(rot_matrix, sample) + robot_state[0:2]
for sample in samples[:, 0:2]
])
obs_position_list = samples
obs = []
for ob in obs_position_list:
human = Human(env.config, 'humans')
# px, py, gx, gy, vx, vy, theta
human.set(ob[0], ob[1], ob[0], ob[1], 0, 0, 0)
env.humans.append(human)
# px, py, vx, vy, radius
obs.append(ObservableState(ob[0], ob[1], 0, 0, voxel_size / 2))
if len(obs_position_list) == 0:
human = Human(env.config, 'humans')
# SARL, CADRL
human.set(0, -10, 0, -10, 0, 0, 0)
# LSTM
#human.set(robot_state[0]+10, robot_state[1]+10, robot_state[0]+10, robot_state[1]+10 , 0, 0, 0)
env.humans.append(human)
# SARL, CADRL
obs.append(ObservableState(0, -10, 0, 0, voxel_size / 2))
# LSTM
#obs.append(ObservableState(robot_state[0]+10, robot_state[1]+10, 0, 0, voxel_size/2))
robot.set_position(robot_state)
robot.set_velocity([math.sin(yaw), math.cos(yaw)])
#print(obs)
action = robot.act(obs)
obs, _, done, info = env.step(action)
current_pos = np.array(robot.get_position())
current_vel = np.array(robot.get_velocity())
#print("Velocity: {}, {}".format(current_vel[0], current_vel[1]))
logging.debug(
'Speed: %.2f',
np.linalg.norm(current_pos - last_pos) / robot.time_step)
last_pos = current_pos
policy_time += time.time() - t1
numFrame += 1
#print(t2-t1, time.time() - t2)
diff_angle = (-yaw + math.atan2(current_vel[0], current_vel[1]))
if diff_angle > math.pi:
diff_angle = diff_angle - 2 * math.pi
elif diff_angle < -math.pi:
diff_angle = diff_angle + 2 * math.pi
#print("diff_angle: {}, {}, {}".format(diff_angle, yaw ,-math.atan2(current_vel[0], current_vel[1])))
if diff_angle < -0.7:
direc = 2 # turn left
elif diff_angle > 0.7:
direc = 3 # turn right
else:
direc = 1 # go straight
# GoEasy(direc)
vx = SPEED * math.sqrt(current_vel[0]**2 + current_vel[1]**2)
if diff_angle > 0:
v_ang = ANGULAR_SPEED * min(diff_angle / (math.pi / 2), 1)
else:
v_ang = ANGULAR_SPEED * max(diff_angle / (math.pi / 2), -1)
print(vx, -v_ang)
easyGo.mvCurve(vx, -v_ang)
if args.plot:
plt.scatter(current_pos[0], current_pos[1], label='robot')
plt.arrow(current_pos[0],
current_pos[1],
current_vel[0],
current_vel[1],
width=0.05,
fc='g',
ec='g')
plt.arrow(current_pos[0],
current_pos[1],
math.sin(yaw),
math.cos(yaw),
width=0.05,
fc='r',
ec='r')
if len(samples) == 1:
plt.scatter(samples[0][0],
samples[0][1],
label='obstacles')
elif len(samples) > 1:
plt.scatter(samples[:, 0],
samples[:, 1],
label='obstacles')
plt.xlim(-6.5, 6.5)
plt.ylim(-9, 4)
plt.legend()
plt.title("gazebo test")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.pause(0.001)
plt.cla()
plt.clf()
print("NAV TIME {}".format(float(sim_time) - t1))
time.sleep(0.5)
print("NAV TIME {}".format(float(sim_time) - t1))
easyGo.stop()
plt.close()
print("Average took {} sec per iteration, {} Frame".format(
policy_time / numFrame, numFrame))
else:
explorer.run_k_episodes(env.case_size[args.phase],
args.phase,
print_failure=True)
rate = rospy.Rate(5000)
Kp = 1.7375 / 45
while not rospy.is_shutdown():
cv2.imshow("asdf",0)
k = cv2.waitKey(1)
if k == ord('k'):
easyGo.stop()
print('FUCKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKK')
cv2.waitKey(0)
break
print('cu', current_Angle, 'dr', desired_Angle)
print(type(current_Angle), type(desired_Angle))
tempInput = (float(desired_Angle) - float(current_Angle)) * Kp
if -1*Magic_Value > tempInput:
tempInput = -1*Magic_Value
elif Magic_Value < tempInput:
tempInput = Magic_Value
print(tempInput)
if abs(tempInput) <= 0.08:
tempInput = 0
print('!!!!!!!!!tempInput 0')
easyGo.mvCurve(1,tempInput)
rate.sleep()
# Instead of using rospy.spin(), we should use rate.sleep because we are in a loop
direc = 3 # turn right
elif velocity[0][1] > 0.02:
direc = 1 # go straight
elif velocity[0][1] < -0.02:
direc = 4 # backward
else:
direc = 5 # stop
if direc == 1:
diff_angle = (
-robot_state[2] +
math.atan2(GOAL_X - robot_state[0], GOAL_Y - robot_state[1]))
if diff_angle > 0:
v_ang = ANGULAR_SPEED * min(diff_angle / (math.pi / 2), 1)
else:
v_ang = ANGULAR_SPEED * max(diff_angle / (math.pi / 2), -1)
easyGo.mvCurve(SPEED * speed_ratio, -v_ang)
else:
GoEasy(direc, speed_ratio)
t3 = time.time()
pc2obs_time += t2 - t1
lpp_time += t3 - t2
print("pc2obs took: {} sec".format(t2 - t1))
print("OCRA took: {} sec".format(t3 - t2))
print("Average took: {} sec, {} sec".format(pc2obs_time / step,
lpp_time / step))
print("Distance to the Goal: {}".format(dist))
plt.arrow(positions[0][0],
positions[0][1],
def callback(data):
global totalTime, count, csv_flag, DRIVE_INDEX
'''
print()
print("Car speed: " + str(car_speed.data * 3.6) + " km/h")
print("Car Yaw: " + str(car_yaw.data))
print("Steering Angle: " + str(steering_angle.data))
print("Simulation status: " + ("Running" if sim_status.data >=
0 else cm.status_dic.get(sim_status.data)))
'''
'''
if longitudinal_speed.data == 0:
print(slip_angle.data, 0)
else:
print(slip_angle.data, math.atan2(lateral_speed.data, longitudinal_speed.data))
floatmsg = Float32()
floatmsg.data = car_speed.data * 3.6
speedpub.publish(floatmsg)
floatmsg.data = car_yaw.data
yawpub.publish(floatmsg)
floatmsg.data = steering_angle.data
steerpub.publish(floatmsg)
'''
# csv_save = data.buttons[0] # A button
## CM vehicle control
control_speed = (data.axes[5] + 1) / 2 # Right trigger
control_speed_back = (data.axes[2] + 1) / 2 # Left trigger
#control_steer = - data.axes[3] # Right stick
control_steer = -data.axes[0] # Left stick
control_brake = data.buttons[5] # Right Button RB
#if abs(control_steer) < 0.15: # give a threshold due to the poor joystick
# control_steer = 0
if control_speed_back:
control_speed = -control_speed_back
control_speed *= MAX_SPEED
# control_brake *= MAX_BRAKE
control_steer *= MAX_STEER
start = data.buttons[4]
control_steer = (int)(round(control_steer))
control_speed = (int)(round(control_speed))
if control_brake:
easyGo.stop()
else:
easyGo.mvCurve(control_speed, control_steer)
'''
elif control_speed != 0:
easyGo.mvStraight(control_speed, -1)
elif control_steer > 0:
easyGo.mvRotate(control_steer, -1, True)
elif control_steer < 0:
easyGo.mvRotate(-control_steer, -1, False)
else:
easyGo.stop()
'''
print("speed: ", control_speed, " , brake: ", control_brake, ", steer: ",
control_steer)
#rate.sleep()
'''
####################################
'''
if -1*Magic_Value > steer:
steer = -1*Magic_Value
elif Magic_Value < steer:
steer = Magic_Value
'''
steer = get_steer_Value(desired_Angle, 0.1)
if abs(steer) <= 0.08:
steer = 0
print('Reach at Desired Angle')
##############
print('steer :', steer)
##############
easyGo.mvCurve(0.5, steer)
rate.sleep()
# Instead of using rospy.spin(), we should use rate.sleep because we are in a loop
else: #For Dependent Running
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
th = threading.Thread(target=imuThread, name="imuthread")
th.setDaemon(True)
th.start()
th2 = threading.Thread(target=subThread, name="Subthread")
th2.setDaemon(True)
th2.start()
def main():
# Configure depth and color streams
global depth_scale, ROW, COL, GRN_ROI, bridge, sim_time
fpsFlag = False
numFrame = 1
fps = 0.0
bridge = CvBridge()
realsense_listener = threading.Thread(target=listener)
realsense_listener.start()
depth_scale = 1.0
startTime = time.time()
ground_seg_time = 0.0
lpp_time = 0.0
dist = 10.0
obs_flg = 0
#while sim_time == 0.0:
# continue
t0 = sim_time
print(dist)
while (dist > 0.8):
#t1 = time.time()
# global depth_image_raw, color_image_raw, robot_state, sim_time
if type(depth_image_raw) == type(0) or type(color_image_raw) == type(
0):
sleep(0.1)
continue
dist = math.sqrt((GOAL_X - robot_state[1])**2 +
(-GOAL_Y - robot_state[0])**2)
if obs_flg == 0 and dist < 10:
# os.system("sh ./init.sh")
obs_flg = 1
depth_image, color_image = preGroundSeg(depth_image_raw,
color_image_raw)
# last step
color_image, virtual_lane_available = GroundSeg(
depth_image, color_image)
t2 = time.time()
# handling lane
cv2.line(color_image, (0, UNAVAILABLE_THRES), (ROW, UNAVAILABLE_THRES),
(0, 255, 0), 2)
if args.csv:
virtual_lane_available = np.array(virtual_lane_available)
# virtual_lane_available = UNAVAILABLE_THRES - virtual_lane_available # normalize. 0 means top of the image
virtual_lane_available = COL - virtual_lane_available
print(robot_state[1], robot_state[0])
print((GOAL_X - robot_state[1]), (GOAL_Y + robot_state[0]))
temp = [(time.time() - t), (GOAL_X - robot_state[1]),
(GOAL_Y + robot_state[0]), cmd_vel.linear.x,
cmd_vel.angular.z]
temp.extend([x for x in virtual_lane_available])
wr.writerow(temp)
t3 = time.time()
direc = LaneHandling(virtual_lane_available, UNAVAILABLE_THRES, 1)
if args.control:
if direc == 0:
diff_angle = (-robot_state[2] + math.atan2(
GOAL_X - robot_state[1], -GOAL_Y - robot_state[0]))
if diff_angle > 0:
v_ang = ANGULAR_SPEED * min(diff_angle / (math.pi / 2), 1)
else:
v_ang = ANGULAR_SPEED * max(diff_angle / (math.pi / 2), -1)
easyGo.mvCurve(SPEED, -v_ang)
else:
GoEasy(direc) # FIXME
# t4 = time.time()
# ground_seg_time += t2-t1
# lpp_time += t4-t3
#
# print("ground_seg took: {} sec".format(t2-t1))
# print("MORP took: {} sec".format(t4-t3))
# print("Average took: {} sec, {} sec, numFrame {}".format(ground_seg_time/numFrame, lpp_time/numFrame, numFrame))
# print("Distance to the Goal: {}".format(dist))
# print("flg: {}".format(obs_flg))
# print("NAV TIME {}".format(float(sim_time) - t0))
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
cv2.imshow('RealSense', color_image)
print("NAV TIME {}".format(float(sim_time) - t0))
#cv2.imshow('RealSense_depth', depth_image)
if cv2.waitKey(1) == 27: #esc
easyGo.stop()
cv2.destroyAllWindows()
rospy.signal_shutdown("esc")
if args.csv:
f.close()
sys.exit(1)
break
# FPS
numFrame += 1
print("TOTAL TIME {}".format(float(sim_time) - t0))
easyGo.stop()
rospy.signal_shutdown("esc")
def main():
# Configure depth and color streams
global depth_scale, ROW, COL, GRN_ROI, bridge, sim_time
fpsFlag = False
numFrame = 1
fps = 0.0
bridge = CvBridge()
realsense_listener = threading.Thread(target=listener)
realsense_listener.start()
depth_scale = 1.0
startTime = time.time()
ground_seg_time = 0.0
lpp_time = 0.0
dist = 10.0
obs_flg = 0
#while sim_time == 0.0:
# continue
t0 = sim_time
print(dist)
history_data = []
model.eval()
control_speed = 0
control_steer = 0
PI = 3.1415926535897
while (dist > 0.8):
#t1 = time.time()
# global depth_image_raw, color_image_raw, robot_state, sim_time
if type(depth_image_raw) == type(0) or type(color_image_raw) == type(
0):
sleep(0.1)
continue
dist = math.sqrt((GOAL_X - robot_state[1])**2 +
(-GOAL_Y - robot_state[0])**2)
if obs_flg == 0 and dist < 10:
# os.system("sh ./init.sh")
obs_flg = 1
depth_image, color_image = preGroundSeg(depth_image_raw,
color_image_raw)
# last step
color_image, virtual_lane_available = GroundSeg(
depth_image, color_image)
t2 = time.time()
# handling lane
cv2.line(color_image, (0, UNAVAILABLE_THRES), (ROW, UNAVAILABLE_THRES),
(0, 255, 0), 2)
virtual_lane_available = np.array(virtual_lane_available)
# virtual_lane_available = UNAVAILABLE_THRES - virtual_lane_available # normalize. 0 means top of the image
virtual_lane_available = COL - virtual_lane_available
temp = [(time.time() - t), (GOAL_X - robot_state[1]),
(GOAL_Y + robot_state[0]), control_speed, control_steer]
temp.extend([x for x in virtual_lane_available])
if history_data == []:
history_data = np.array(temp).reshape((1, -1))
history_data = np.repeat(history_data, HISTORY, axis=0)
else:
history_data = np.roll(history_data, -1,
axis=0) # data in the front is oldest
history_data[-1] = np.array(temp)
goal_x = history_data[-1][1]
goal_y = history_data[-1][2]
deadends = history_data[:, 5:] / 360.0
commands = history_data[:, 3:5]
dead_ends = np.hstack(deadends)
commands = np.hstack(commands)
model_input = list(dead_ends)
model_input.extend(list(commands))
model_input.extend([goal_x, goal_y])
with torch.no_grad():
model_command = model(torch.FloatTensor(model_input))
model_command = np.array(model_command)
control_speed = model_command[0]
control_steer = model_command[1]
print(control_speed, control_steer)
easyGo.mvCurve(control_speed / (2 * PI / 360),
control_steer / (2 * PI / 360))
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
cv2.imshow('RealSense', color_image)
#print("NAV TIME {}".format(float(sim_time)-t0))
#cv2.imshow('RealSense_depth', depth_image)
if cv2.waitKey(1) == 27: #esc
easyGo.stop()
cv2.destroyAllWindows()
rospy.signal_shutdown("esc")
if args.csv:
f.close()
sys.exit(1)
break
# FPS
numFrame += 1
print("TOTAL TIME {}".format(float(sim_time) - t0))
easyGo.stop()
rospy.signal_shutdown("esc")