def draw_points(self,
path,
model_group_name,
point_group_name,
point_type,
rgb,
display_density,
point_radius=0.03):
"""
Draws the points of a given path in RViz.
Args:
path (list of list of float): The path to draw.
model_group_name (str): The name of the joint group in question.
For the PR2 arms, this would be "right_arm" or "left_arm".
point_group_name (str): The namespace under which the points will
show up in RViz.
point_type (str): Type of point, ANGLES or POSES.
rgb (tuple of float): Color of points being drawn. Some colors are
pre-defined as members of this class.
display_density (float): The fraction of the path to be displayed.
For instance, if display_density = 0.25, one in four points will
be shown.
point_radius (float): Size of the individual points to be drawn.
"""
draw_message = DrawPoints()
draw_message.points = [Float64Array(p) for p in path]
draw_message.model_group_name = model_group_name
draw_message.point_group_name = point_group_name
draw_message.point_type = draw_message.POINT_TYPE_ANGLES if point_type == DrawPointsWrapper.ANGLES else draw_message.POINT_TYPE_POSES
draw_message.display_density = display_density
draw_message.red, draw_message.green, draw_message.blue = rgb
draw_message.action = draw_message.ACTION_ADD
draw_message.point_radius = point_radius
self.display_points_publisher.publish(draw_message)
def get_invalid_sections_for_paths(self, orig_paths, group_name):
"""
Returns the invalid sections for a set of paths.
Args:
orig_paths (list of paths): The paths to collision check,
represnted by a list of individual joint configurations.
group_name (str): The joint group for which the paths were created.
Return:
list of list of pairs of indicies, where each index in a pair is the
start and end of an invalid section.
"""
collision_check_client = rospy.ServiceProxy(COLLISION_CHECK,
CollisionCheck)
cc_req = CollisionCheckRequest()
cc_req.paths = [
Float64Array2D([Float64Array(point) for point in path])
for path in orig_paths
]
cc_req.group_name = group_name
rospy.loginfo(
"Plan Trajectory Wrapper: sending request to collision checker")
rospy.wait_for_service(COLLISION_CHECK)
response = collision_check_client(cc_req)
return [[sec.values for sec in individualPathSections.points]
for individualPathSections in response.invalid_sections]
def _get_path(self, action_goal):
"""
Callback which retrieves a path given a goal.
"""
self._set_stop_value(False)
rospy.loginfo("PFS action server: PFS got an action goal")
res = PFSResult()
s, g = action_goal.start, action_goal.goal
res.status.status = res.status.FAILURE
self.current_joint_names = action_goal.joint_names
self.current_group_name = action_goal.group_name
if not self._get_stop_value():
unfiltered = self._call_planner(
s, g, action_goal.allowed_planning_time.to_sec())
if unfiltered is None:
self.pfs_server.set_succeeded(res)
return
else:
rospy.loginfo(
"PFS action server: PFS was stopped before it started planning"
)
self.pfs_server.set_succeeded(res)
return
if not self._get_stop_value():
# The planner succeeded; actually return the path in the result.
res.status.status = res.status.SUCCESS
res.path = [Float64Array(p) for p in unfiltered]
self.pfs_server.set_succeeded(res)
return
else:
rospy.loginfo(
"PFS action server: PFS found a path but RR succeeded first")
self.pfs_server.set_succeeded(res)
return
def shortcut_path(self, original_path, group_name):
"""
Shortcuts a path, where the path is for a given group name.
Args:
original_path (list of list of float): The path, represented by
a list of individual joint configurations.
group_name (str): The group for which the path was created.
Return:
list of list of float: The shortcutted version of the path.
"""
shortcut_path_client = rospy.ServiceProxy(SHORTCUT_PATH_NAME, PathShortcut)
shortcut_req = PathShortcutRequest()
shortcut_req.path = [Float64Array(p) for p in original_path]
shortcut_req.group_name = group_name
rospy.wait_for_service(SHORTCUT_PATH_NAME)
response = shortcut_path_client(shortcut_req)
return [p.values for p in response.new_path]
def _retrieve_repair(self, action_goal):
"""
Callback which performs the full Retrieve and Repair for the path.
"""
self.working_lock.acquire()
self.start_time = time.time()
self.stats_msg = RRStats()
self._set_stop_value(False)
if self.draw_points:
self.draw_points_wrapper.clear_points()
rospy.loginfo("RR action server: RR got an action goal")
s, g = action_goal.start, action_goal.goal
res = RRResult()
res.status.status = res.status.FAILURE
self.current_joint_names = action_goal.joint_names
self.current_group_name = action_goal.group_name
projected, retrieved, invalid = [], [], []
repair_state = STATE_RETRIEVE
self.stats_msg.init_time = time.time() - self.start_time
# Go through the retrieve, repair, and return stages of the planning.
# The while loop should only ever go through 3 iterations, one for each
# stage.
while not self._need_to_stop() and repair_state != STATE_FINISHED:
if repair_state == STATE_RETRIEVE:
start_retrieve = time.time()
projected, retrieved, invalid, retrieved_planner_type = self.path_library.retrieve_path(
s, g, self.num_paths_checked, self.robot_name,
self.current_group_name, self.current_joint_names)
self.stats_msg.retrieve_time.append(time.time() -
start_retrieve)
if len(projected) == 0:
rospy.loginfo(
"RR action server: got an empty path for retrieve state"
)
repair_state = STATE_FINISHED
else:
start_draw = time.time()
if self.draw_points:
self.do_retrieved_path_drawing(projected, retrieved,
invalid)
self.stats_msg.draw_time.append(time.time() - start_draw)
repair_state = STATE_REPAIR
elif repair_state == STATE_REPAIR:
start_repair = time.time()
repaired = self._path_repair(
projected,
action_goal.allowed_planning_time.to_sec(),
invalid_sections=invalid)
self.stats_msg.repair_time.append(time.time() - start_repair)
if repaired is None:
rospy.loginfo(
"RR action server: path repair didn't finish")
repair_state = STATE_FINISHED
else:
repair_state = STATE_RETURN_PATH
elif repair_state == STATE_RETURN_PATH:
start_return = time.time()
res.status.status = res.status.SUCCESS
res.retrieved_path = [Float64Array(p) for p in retrieved]
res.repaired_path = [Float64Array(p) for p in repaired]
# added more information of the planner
rospy.loginfo("RR action server: returning a path")
repair_state = STATE_FINISHED
self.stats_msg.return_time = time.time() - start_return
if repair_state == STATE_RETRIEVE:
rospy.loginfo(
"RR action server: stopped before it retrieved a path")
elif repair_state == STATE_REPAIR:
rospy.loginfo(
"RR action server: stopped before it could repair a retrieved path"
)
elif repair_state == STATE_RETURN_PATH:
rospy.loginfo(
"RR action server: stopped before it could return a repaired path"
)
self.rr_server.set_succeeded(res)
self.stats_msg.total_time = time.time() - self.start_time
self.stats_pub.publish(self.stats_msg)
self.working_lock.release()
def _retrieve_repair(self, action_goal):
"""
Callback which performs the full Retrieve and Repair for the path.
"""
self.working_lock.acquire()
self.start_time = time.time()
# obtain the obstacle id
obs_i = rospy.wait_for_message('obstacles/obs_i', Int32)
obs_i = obs_i.data
# if obstacle id is a new id, then load a new library
if obs_i != self.obs_i:
# new id
self.obs_i = obs_i
# new robot name: [robot_name]_[obs_id]
# first try loading the existing library, if not found, then create a new one
res = self.path_library._load_library(
self.robot_name + '_%d' % (obs_i), self.current_joint_names)
if res == False:
self.path_library._create_and_load_new_library(
self.robot_name + '_%d' % (obs_i),
self.current_joint_names)
# otherwise continue using current library
self.stats_msg = RRStats()
self._set_stop_value(False)
if self.draw_points:
self.draw_points_wrapper.clear_points()
rospy.loginfo("RR action server: RR got an action goal")
s, g = action_goal.start, action_goal.goal
res = RRResult()
res.status.status = res.status.FAILURE
self.current_joint_names = action_goal.joint_names
self.current_group_name = action_goal.group_name
projected, retrieved, invalid = [], [], []
repair_state = STATE_RETRIEVE
self.stats_msg.init_time = time.time() - self.start_time
# Go through the retrieve, repair, and return stages of the planning.
# The while loop should only ever go through 3 iterations, one for each
# stage.
while not self._need_to_stop() and repair_state != STATE_FINISHED:
if repair_state == STATE_RETRIEVE:
start_retrieve = time.time()
projected, retrieved, invalid, retrieved_planner_type = self.path_library.retrieve_path(
s, g, self.num_paths_checked, self.robot_name,
self.current_group_name, self.current_joint_names)
self.stats_msg.retrieve_time.append(time.time() -
start_retrieve)
if len(projected) == 0:
rospy.loginfo(
"RR action server: got an empty path for retrieve state"
)
repair_state = STATE_FINISHED
else:
start_draw = time.time()
if self.draw_points:
self.do_retrieved_path_drawing(projected, retrieved,
invalid)
self.stats_msg.draw_time.append(time.time() - start_draw)
repair_state = STATE_REPAIR
elif repair_state == STATE_REPAIR:
start_repair = time.time()
#print('RR action server: retrieved path:')
#print(retrieved)
#print('RR action server: projected path:')
#print(projected)
#print('RR action server: invalid:')
#print(invalid)
repaired_planner_type, repaired, total_num_paths, total_num_paths_NN, \
total_new_node, total_new_node_NN = \
self._path_repair(projected, action_goal.allowed_planning_time.to_sec(), invalid_sections=invalid)
self.stats_msg.repair_time.append(time.time() - start_repair)
if repaired is None:
rospy.loginfo(
"RR action server: path repair didn't finish")
repair_state = STATE_FINISHED
else:
repair_state = STATE_RETURN_PATH
elif repair_state == STATE_RETURN_PATH:
start_return = time.time()
res.status.status = res.status.SUCCESS
res.retrieved_path = [Float64Array(p) for p in retrieved]
res.repaired_path = [Float64Array(p) for p in repaired]
# if total newly generated nodes are 0, it means the library path is not in collision
# then we don't need to train on them
if total_new_node == 0:
retrieved_planner_type = PlannerType.NEURAL
repaired_planner_type = PlannerType.NEURAL
res.retrieved_planner_type.planner_type = retrieved_planner_type
res.repaired_planner_type.planner_type = repaired_planner_type
# added more information of the planner
res.total_num_paths = total_num_paths
res.total_num_paths_NN = total_num_paths_NN
res.total_new_node = total_new_node
res.total_new_node_NN = total_new_node_NN
rospy.loginfo('RR action server: total_new_node: %d' %
(total_new_node))
rospy.loginfo('RR action server: total_new_node_NN: %d' %
(total_new_node_NN))
rospy.loginfo(
"RR action server: total_num_paths_NN before returning: %d"
% (total_num_paths_NN))
rospy.loginfo("RR action server: returning a path")
repair_state = STATE_FINISHED
self.stats_msg.return_time = time.time() - start_return
if repair_state == STATE_RETRIEVE:
rospy.loginfo(
"RR action server: stopped before it retrieved a path")
elif repair_state == STATE_REPAIR:
rospy.loginfo(
"RR action server: stopped before it could repair a retrieved path"
)
elif repair_state == STATE_RETURN_PATH:
rospy.loginfo(
"RR action server: stopped before it could return a repaired path"
)
self.rr_server.set_succeeded(res)
self.stats_msg.total_time = time.time() - self.start_time
self.stats_pub.publish(self.stats_msg)
self.working_lock.release()
def plan(args):
global responded, exception
rospy.init_node('lightning_client')
print('loading...')
if args.env_type == 's2d':
IsInCollision = plan_s2d.IsInCollision
data_loader = data_loader_2d
# create an SE2 state space
time_limit = 15.
ratio = 1.
elif args.env_type == 'c2d':
IsInCollision = plan_c2d.IsInCollision
data_loader = data_loader_2d
# create an SE2 state space
time_limit = 60.
ratio = 1.
elif args.env_type == 'r2d':
IsInCollision = plan_r2d.IsInCollision
data_loader = data_loader_r2d
# create an SE2 state space
time_limit = 60.
ratio = 1.05
elif args.env_type == 'r3d':
IsInCollision = plan_r3d.IsInCollision
data_loader = data_loader_r3d
# create an SE2 state space
time_limit = 15.
ratio = 1.
test_data = data_loader.load_test_dataset(N=args.N,
NP=args.NP,
s=args.env_idx,
sp=args.path_idx,
folder=args.data_path)
obcs, obs, paths, path_lengths = test_data
obcs = obcs.tolist()
obs = obs.tolist()
#paths = paths
path_lengths = path_lengths.tolist()
time_env = []
time_total = []
fes_env = [] # list of list
valid_env = []
# setup publisher
obc_pub = rospy.Publisher('lightning/obstacles/obc',
Float64Array2D,
queue_size=10)
obs_pub = rospy.Publisher('lightning/obstacles/obs',
Float64Array,
queue_size=10)
obs_i_pub = rospy.Publisher('lightning/obstacles/obs_i',
Int32,
queue_size=10)
length_pub = rospy.Publisher('lightning/planning/path_length_threshold',
Float64,
queue_size=10)
for i in xrange(len(paths)):
time_path = []
fes_path = [] # 1 for feasible, 0 for not feasible
valid_path = [] # if the feasibility is valid or not
# save paths to different files, indicated by i
# feasible paths for each env
obc = obcs[i]
# publishing to ROS topic
obc_msg = Float64Array2D([Float64Array(obci) for obci in obc])
obs_msg = Float64Array(obs[i])
obs_i_msg = Int32(i)
for j in xrange(len(paths[0])):
# check if the start and goal are in collision
# if so, then continue
fp = 0 # indicator for feasibility
print("step: i=" + str(i) + " j=" + str(j))
if path_lengths[i][j] == 0:
# invalid, feasible = 0, and path count = 0
fp = 0
valid_path.append(0)
elif IsInCollision(paths[i][j][0], obc) or IsInCollision(
paths[i][j][path_lengths[i][j] - 1], obc):
# invalid, feasible = 0, and path count = 0
fp = 0
valid_path.append(0)
else:
valid_path.append(1)
# obtaining the length threshold for planning
data_length = 0.
for k in xrange(path_lengths[i][j] - 1):
data_length += np.linalg.norm(paths[i][j][k + 1] -
paths[i][j][k])
length_msg = Float64(data_length * ratio)
# call lightning service
request = GetMotionPlanRequest()
request.motion_plan_request.group_name = 'base'
for k in xrange(len(paths[i][j][0])):
request.motion_plan_request.start_state.joint_state.name.append(
'%d' % (k))
request.motion_plan_request.start_state.joint_state.position.append(
paths[i][j][0][k])
request.motion_plan_request.goal_constraints.append(
Constraints())
for k in xrange(len(paths[i][j][0])):
request.motion_plan_request.goal_constraints[
0].joint_constraints.append(JointConstraint())
request.motion_plan_request.goal_constraints[
0].joint_constraints[k].position = paths[i][j][
path_lengths[i][j] - 1][k]
request.motion_plan_request.allowed_planning_time = time_limit
responded = False
exception = False
def publisher():
global responded, exception
while not responded and not exception:
print('sending obstacle message...')
obc_pub.publish(obc_msg)
obs_pub.publish(obs_msg)
obs_i_pub.publish(obs_i_msg)
length_pub.publish(length_msg)
rospy.sleep(0.5)
pub_thread = threading.Thread(target=publisher, args=())
pub_thread.start()
print('waiting for lightning service...')
try:
# to make sure when we CTRL+C we can exit
rospy.wait_for_service(LIGHTNING_SERVICE)
except:
exception = True
pub_thread.join()
# exit because there is error
print(
'exception occurred when waiting for lightning service...'
)
raise
print('acquired lightning service')
lightning = rospy.ServiceProxy(LIGHTNING_SERVICE,
GetMotionPlan)
try:
respond = lightning(request)
except:
exception = True
responded = True
pub_thread.join()
if respond.motion_plan_response.error_code.val == respond.motion_plan_response.error_code.SUCCESS:
# succeed
time = respond.motion_plan_response.planning_time
time_path.append(time)
path = respond.motion_plan_response.trajectory.joint_trajectory.points
path = [p.positions for p in path]
path = np.array(path)
print(path)
# feasibility check this path
if plan_general.feasibility_check(path,
obc,
IsInCollision,
step_sz=0.01):
fp = 1
print('feasible')
else:
fp = 0
print('not feasible')
fes_path.append(fp)
time_env.append(time_path)
time_total += time_path
print('average test time up to now: %f' % (np.mean(time_total)))
fes_env.append(fes_path)
valid_env.append(valid_path)
print(np.sum(np.array(fes_env)))
print('accuracy up to now: %f' %
(float(np.sum(np.array(fes_env))) / np.sum(np.array(valid_env))))
pickle.dump(time_env, open(args.res_path + 'time.p', "wb"))
f = open(os.path.join(args.res_path, 'accuracy.txt'), 'w')
valid_env = np.array(valid_env).flatten()
fes_env = np.array(
fes_env).flatten() # notice different environments are involved
suc_rate = float(fes_env.sum()) / valid_env.sum()
f.write(str(suc_rate))
f.close()