def GoEasy(direc):
if direc == 4: # Backward
easyGo.mvStraight(-SPEED, -1)
elif direc == 0 or direc == 1: # Go straight
easyGo.mvStraight(SPEED, -1)
elif direc == 2: # turn left
easyGo.mvRotate(ROTATE_SPEED, -1, False)
elif direc == 3: # turn right
easyGo.mvRotate(ROTATE_SPEED, -1, True)
elif direc == 5: # stop
easyGo.stop()
def dap(path, velRobot=0.1, verbose=0): # path is 2D array (x, y)
global start_position, init_path
init_path = path
global i, threshold_boundary
while (True):
print('thread re1fuck')
if rospy.get_param('/point_init'):
#closest_pt()
rospy.set_param('/point_init', False)
if i == 0 or FlagMORP: # when just running code or we should search new path(by MORP algorithm)
while (True): # at first time, get_poseXY() == (0, 0)
start_position = easyVector.get_poseXY(
)[:] # x axis == forward
#print(start_position, easyVector.get_angle())
if start_position != (0.0, 0.0):
break
if verbose: # plot current robot position
fig, ax = plt.subplots()
threshold_boundary = 0.2 #0.14
_x = [item[0] for item in path]
_y = [item[1] for item in path]
plt.scatter(_x, _y, color='b')
plt.hold(True)
plt.xlim(-1, 1.3)
plt.ylim(-2, 2)
plt.hold(True)
current_position = get_current_pose(start_position)
#(x, y) when DPoom head to right, x decreases (negatively increases)
# current_position and DAP's coordinates : straight(y), right(x)
#print('current_position :', current_position)
if (path[i][0] - current_position[0])**2 + (
path[i][1] - current_position[1])**2 < threshold_boundary**2:
i = i + 1
if i == len(path):
print('Goal!')
easyGo.stop()
if verbose:
plt.close()
break
print('i:', i)
if cv2.waitKey(1) == ord('k'):
easyGo.stop()
exit()
break
easy_drive(path[i][0], path[i][1], current_position, velRobot)
if verbose:
plt.scatter(current_position[0], current_position[1], c='r')
plt.hold(True)
plt.pause(0.05)
def main():
speed = 5
count = 0
f = keyCap.KeyPoller()
with f as poller:
start_time, end_time = 0, 0
while True:
getch = poller.poll()
if getch != None:
if getch == 'w':
easyGo.mvStraight(abs(speed), -1)
print("[Move Forward] by %f, %d" % (speed, count))
keyCap._cls()
elif getch == "a":
easyGo.mvRotate(abs(speed) * 4, -1, False)
print("[Trun CounterColckwise] by %f, %d" % (speed, count))
keyCap._cls()
elif getch == "s":
easyGo.mvStraight(-abs(speed), -1)
print("[Move Backward] by %f, %d" % (speed, count))
keyCap._cls()
elif getch == 'd':
easyGo.mvRotate(abs(speed) * 4, -1, True)
print("[Trun Colckwise] by %f, %d" % (speed, count))
keyCap._cls()
elif getch == 'e':
speed += 0.1
print('set speed :', speed)
keyCap._cls()
elif getch == "c":
speed -= 0.2
if speed < 0:
speed = 0
print('set speed :', speed)
keyCap._cls()
elif getch == "x":
speed = 5
print('set speed :', speed)
elif getch == "q":
print('Program terminated')
easyGo.stop()
return
else:
easyGo.stop()
count += 1
start_time = time.time()
else:
end_time = time.time()
if end_time - start_time < 0.1:
continue
easyGo.stop()
count = 0
def pc2obs(voxel_size=0.3, plot=False):
global points_raw, bridge, currentStatus, handle_easy
#print(points_raw)
if type(points_raw) == type(0):
print("NOT CONNECTED")
sleep(0.1)
return False
t1 = time.time()
points = np.array(list(points_raw), dtype=np.float32)
if len(points) == 0:
return False
t2 = time.time()
#print("length pre-processed points: {}".format(len(points)))
np_vox = np.ceil(
(np.max(points, axis=0) - np.min(points, axis=0)) / voxel_size)
non_empty_voxel_keys, inverse, nb_pts_per_voxel = np.unique(
((points - np.min(points, axis=0)) // voxel_size).astype(int),
axis=0,
return_inverse=True,
return_counts=True)
idx_pts_sorted = np.argsort(inverse)
voxel_grid = {}
grid_barycenter, grid_candidate_center = [], []
last_seen = int(0)
for idx, vox in enumerate(non_empty_voxel_keys):
voxel_grid[tuple(vox)] = points[
idx_pts_sorted[int(last_seen):int(last_seen +
nb_pts_per_voxel[idx])]]
grid_barycenter.append(np.mean(voxel_grid[tuple(vox)], axis=0))
grid_candidate_center.append(voxel_grid[tuple(vox)][np.linalg.norm(
voxel_grid[tuple(vox)] - np.mean(voxel_grid[tuple(vox)], axis=0),
axis=1).argmin()])
last_seen += nb_pts_per_voxel[idx]
points = np.array(filter(lambda x: x[0] != 0, list(grid_candidate_center)))
t3 = time.time()
points_layer = []
for i, p in enumerate(points):
#if -p[1] > 0.1 and -p[1] < 0.6:
#if p[2] > 0.1 and p[2] < 0.6:
#if abs(-p[1] - 0.3) < 0.06: # points are at 0.2m higher height than depth camera
# forward:x . right:y, up:z
points_layer.append([-p[0], p[1]])
#points_layer.append([p[0], p[2], -p[1]])
samples = np.array(points_layer)
if plot:
print("time took")
print(t2 - t1)
print(t3 - t2)
print(time.time() - t3)
plt.scatter(points[:, 0], points[:, 2], label='voxel grid filtering')
plt.scatter(samples[:, 0], samples[:, 1], label='height filtering')
plt.xlim(-1.5, 1.5)
plt.ylim(0, 6)
plt.legend()
plt.title("Top view points after filter processing")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.pause(0.05)
plt.cla()
plt.clf()
#cv2.imshow('RealSense_depth', depth_image)
if cv2.waitKey(1) == 27: #esc
easyGo.stop()
rospy.signal_shutdown("esc")
if args.csv:
f.close()
sys.exit(1)
return samples
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)
def Steer_Visualization(desired_steer): #steer visualization
M = cv2.getRotationMatrix2D((cols / 2, rows / 2), desired_steer*20, 1)
dst = cv2.warpAffine(steer, M, (cols, rows))
cv2.imshow("steering wheel", dst)
if cv2.waitKey(1) & 0xFF == ord('q'):
easyGo.stop()
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()
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()
'''
numpy
for intel D435i
'''
import pyrealsense2 as rs
import numpy as np
import cv2
import matplotlib.pyplot as plt
import math
import time
import easyGo as eg
global depth_scale, ROW, COL
eg.stop()
#size of images
COL= 720
ROW = 1280
VERTICAL_CORRECTION = 0.15 #0.45 #parameter of correction for parabola to linear
WARP_PARAM = 0.45 #value should be 0.0 ~ 1.0. Bigger get more warped. 0.45
GRN_ROI = 300 #The index of col that want to consider as ground ROI 400
ZOOM_PARAM = 0.15 #Force calibrating the depth image to match the color image 0.15
UNAVAILABLE_THRES = 500 #The index of col that is threshold of unavailable virtual lane
font = cv2.FONT_HERSHEY_SCRIPT_SIMPLEX
fontScale = 1.5
yellow = (0, 255, 255)
#Topview image. src, dst are numpy array.
def main():
# Configure depth and color streams
global depth_scale, ROW, COL, GRN_ROI, bridge, direc
fpsFlag = False
numFrame = 0
fps = 0.0
#easyGo.mvStraight(0, -1)
#####
#ic = image_converter()
#####
bridge = CvBridge()
realsense_listener = threading.Thread(target=listener)
realsense_listener.start()
#realsense_listener.join()
print("F**K!!")
while 1:
try:
'''
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, ROW, COL, rs.format.z16, 30)
config.enable_stream(rs.stream.color, ROW, COL, rs.format.bgr8, 30)
'''
# Start streaming
#profile = pipeline.start(config)
#depth_sensor = profile.get_device().first_depth_sensor()
#depth_scale = depth_sensor.get_depth_scale()
#print("Depth Scale is: ", depth_scale)
#frames = pipeline.wait_for_frames()
break
except:
pass
#COL=720, ROW=1280
depth_scale = 0.0010000000474974513
startTime = time.time()
abcd = 0
while True:
#print(abcd)
abcd += 1
print('MORP Wokring')
#try:
#if depth_image_raw == None: continue
#frames = pipeline.wait_for_frames()
#depth_frame = frames.get_depth_frame()
#depth_frame = cv_image
#color_frame
#color_frame = frames.get_color_frame()
#if not depth_frame or not color_frame:
# continue
# Convert images to numpy arrays
#depth_image = np.asanyarray(depth_frame.get_data(), dtype=float)
#color_image = np.asanyarray(color_frame.get_data())
# print(depth_image[360][550], len(depth_image[0]), str(depth_image[360][550] * depth_scale) + "m")
# first step
# Wait for a coherent pair of frames: depth and color
global depth_image_raw, color_image_raw, currentStatus, handle_easy
if type(depth_image_raw) == type(0) or type(color_image_raw) == type(
0):
sleep(1)
##babbanner(bad=True)
continue
print('MORP CYCLE : ', abcd)
#banner("True")
depth_image, color_image = preGroundSeg(depth_image_raw,
color_image_raw)
# last step
color_image, virtual_lane_available = GroundSeg(
depth_image, color_image)
# handling lane
cv2.line(color_image, (0, UNAVAILABLE_THRES), (ROW, UNAVAILABLE_THRES),
(0, 255, 0), 2)
direc = LaneHandling(virtual_lane_available, UNAVAILABLE_THRES, 1)
# serverSock.send(str(direc).encode('utf-8'))
if direc == 1:
currentStatus = "NO OBSTACLE"
rospy.set_param('/point_init', True)
else:
currentStatus = "YES OBSTACLE"
print(direc)
if handle_easy:
easyGo.stopper = handle_easy
GoEasy(direc)
print('Morp easyGo stoppper :: ' + str(easyGo.stopper))
#LaneHandling(virtual_lane_available, UNAVAILABLE_THRES, 1)
# Stack both images horizontally
# images = np.hstack((color_image, depth_colormap))
# Show images
cv2.namedWindow('RealSense', cv2.WINDOW_NORMAL)
cv2.imshow('RealSense', color_image)
if cv2.waitKey(1) == 27: #esc
easyGo.stop()
break
# FPS
numFrame += 1
if cv2.waitKey(1) == ord('f'):
endTime = time.time()
fps = round((float)(numFrame) / (endTime - startTime), 2)
print("###FPS = " + str(fps) + " ###")
#except Exception:
# print("Error")
#pipeline.stop()
#easyGo.stop()
# Stop streaming
#pipeline.stop()
easyGo.stop()
print "velocidad angular z = " + str (move.angular.z)
rate.sleep()
#sub=rospy.Subscriber('cmd_vel', Twist, twist)
rospy.init_node('robot_mvs', anonymous=True) # the original name sphero might be the same as other node.
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) #topic publisher that allows you to move the sphero
#sub_odom = rospy.Subscriber('/odom', Odometry, odom_callback) # the original name odom might be the same as other function.
sub_imu = rospy.Subscriber('/imu_yaw', Float32, yaw_callback)
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:
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")
