def run(self, request):
#make sure the request is valid
start_and_goal = self._is_valid_motion_plan_request(request)
if start_and_goal is None:
response = GetMotionPlanResponse()
response.motion_plan_response.error_code.val = response.motion_plan_response.error_code.PLANNING_FAILED
return response
s, g = start_and_goal
self.rr_returned, self.pfs_returned = False, False
self.lightning_response = None
self.lightning_response_ready_event.clear()
self.current_joint_names = request.motion_plan_request.start_state.joint_state.name
self.current_group_name = request.motion_plan_request.group_name
if self.draw_points:
self.draw_points_wrapper.clear_points()
#start a timer that stops planners if they take too long
timer = threading.Thread(
target=self._lightning_timeout,
args=(request.motion_plan_request.allowed_planning_time, ))
timer.start()
self.start_time = time.time() # Used for timing stats.
# Send action requests to RR and PFS nodes.
if self.use_rr:
rr_client_goal = RRGoal()
rr_client_goal.start = s
rr_client_goal.goal = g
rr_client_goal.joint_names = self.current_joint_names
rr_client_goal.group_name = self.current_group_name
rr_client_goal.allowed_planning_time = rospy.Duration(
request.motion_plan_request.allowed_planning_time)
self.rr_client.wait_for_server()
rospy.loginfo("Lightning: Sending goal to RR")
self.rr_client.send_goal(rr_client_goal, done_cb=self._rr_done_cb)
if self.use_pfs:
pfs_client_goal = PFSGoal()
pfs_client_goal.start = s
pfs_client_goal.goal = g
pfs_client_goal.joint_names = self.current_joint_names
pfs_client_goal.group_name = self.current_group_name
pfs_client_goal.allowed_planning_time = rospy.Duration(
request.motion_plan_request.allowed_planning_time)
self.pfs_client.wait_for_server()
rospy.loginfo("Lightning: Sending goal to PFS")
self.pfs_client.send_goal(pfs_client_goal,
done_cb=self._pfs_done_cb)
self.lightning_response_ready_event.wait()
if self.lightning_response.motion_plan_response.error_code.val != self.lightning_response.motion_plan_response.error_code.SUCCESS:
rospy.loginfo("Lightning: did not find a path")
# no need to do train MPNet
return self.lightning_response
rospy.loginfo("Lightning: Lightning is responding with a path")
return self.lightning_response
def _rr_done_cb(self, state, result):
self.done_lock.acquire()
self.rr_returned = True
if self.publish_stats:
stat_msg = Stats()
stat_msg.plan_time = time.time() - self.start_time
if result.status.status == result.status.SUCCESS:
if not self.pfs_returned or self.lightning_response is None:
self._send_stop_pfs_planning()
rr_path = [p.values for p in result.repaired_path]
retrieved_path = [p.values for p in result.retrieved_path]
shortcut_start = time.time()
shortcut = self.shortcut_path_wrapper.shortcut_path(
rr_path, self.current_group_name)
if self.publish_stats:
stat_msg.shortcut_time = time.time() - shortcut_start
self.lightning_response = self._create_get_motion_plan_response(
shortcut)
# set planning time to be the total time up to now
self.lightning_response.motion_plan_response.planning_time = time.time(
) - self.start_time
self.lightning_response_ready_event.set()
self.done_lock.release()
#display new path in rviz
if self.draw_points:
self.draw_points_wrapper.draw_points(
rr_path, self.current_group_name, "final",
DrawPointsWrapper.ANGLES, DrawPointsWrapper.GREEN, 0.1)
if self.store_paths:
#store_response = self._store_path(rr_path, rr_path) # this original version seems to be wrong because retrieved is the same as repaired
store_response = self._store_path(
rr_path, retrieved_path,
result.repaired_planner_type.CLASSIC)
self._special_print(
"Lightning: Got a path from RR, path stored = %s, number of library paths = %i"
% (store_response))
else:
self._special_print("Lightning: Got a path from RR")
# record the planning time
self.plan_time = time.time() - self.start_time
if self.publish_stats:
stat_msg.time = time.time() - self.start_time
stat_msg.rr_won = True
self.stat_pub.publish(stat_msg)
return
else:
rospy.loginfo("Lightning: Call to RR did not return a path")
if not self.use_pfs or (self.pfs_returned
and self.lightning_response is None):
self.lightning_response = GetMotionPlanResponse()
self.lightning_response.motion_plan_response.error_code.val = self.lightning_response.motion_plan_response.error_code.PLANNING_FAILED
self.lightning_response_ready_event.set()
self.done_lock.release()
def _create_get_motion_plan_response(self, path):
response = GetMotionPlanResponse()
response.motion_plan_response.error_code.val = response.motion_plan_response.error_code.SUCCESS
response.motion_plan_response.trajectory.joint_trajectory.points = []
for pt in path:
jtp = JointTrajectoryPoint()
jtp.positions = pt
response.motion_plan_response.trajectory.joint_trajectory.points.append(
jtp)
response.motion_plan_response.trajectory.joint_trajectory.joint_names = self.current_joint_names
return response
def _pfs_done_cb(self, state, result):
self.done_lock.acquire()
self.pfs_returned = True
if self.publish_stats:
stat_msg = Stats()
stat_msg.rr_won = False
stat_msg.plan_time = time.time() - self.start_time
if result.status.status == result.status.SUCCESS:
if not self.rr_returned or self.lightning_response is None:
self._send_stop_rr_planning()
pfsPath = [p.values for p in result.path]
shortcut_start = time.time()
shortcut = self.shortcut_path_wrapper.shortcut_path(
pfsPath, self.current_group_name)
if self.publish_stats:
stat_msg.shortcut_time = time.time() - shortcut_start
self.lightning_response = self._create_get_motion_plan_response(
shortcut)
self.lightning_response_ready_event.set()
self.done_lock.release()
#display new path in rviz
if self.draw_points:
self.draw_points_wrapper.draw_points(
pfsPath, self.current_group_name, "final",
DrawPointsWrapper.ANGLES, DrawPointsWrapper.GREEN, 0.1)
if self.store_paths:
store_response = self._store_path(pfsPath, [])
self._special_print(
"Lightning: Got a path from PFS, path stored = %s, number of library paths = %i"
% (store_response))
else:
self._special_print("Lightning: Got a path from PFS")
if self.publish_stats:
stat_msg.time = time.time() - self.start_time
self.stat_pub.publish(stat_msg)
return
else:
rospy.loginfo("Lightning: Call to PFS did not return a path")
if not self.use_rr or (self.rr_returned
and self.lightning_response is None):
self.lightning_response = GetMotionPlanResponse()
self.lightning_response.motion_plan_response.error_code.val = self.lightning_response.motion_plan_response.error_code.PLANNING_FAILED
self.lightning_response_ready_event.set()
self.done_lock.release()
def _pfs_done_cb(self, state, result):
print('pfs done')
start = time.time()
self.done_lock.acquire()
print('done lock acquired')
print('done lock acquire time: %f' % (time.time() - start))
self.pfs_returned = True
if self.publish_stats:
stat_msg = Stats()
stat_msg.rr_won = False
stat_msg.plan_time = time.time() - self.start_time
print('handling done function: %f' % (time.time() - start))
print('pfs_done_cb: plan time: %f' % (time.time() - self.start_time))
if result.status.status == result.status.SUCCESS:
if not self.rr_returned or self.lightning_response is None:
self._send_stop_rr_planning()
pfsPath = [p.values for p in result.path]
shortcut_start = time.time()
shortcut = self.shortcut_path_wrapper.shortcut_path(
pfsPath, self.current_group_name)
if self.publish_stats:
stat_msg.shortcut_time = time.time() - shortcut_start
print('pfs_done_cb: shortcut time: %f' %
(time.time() - shortcut_start))
self.lightning_response = self._create_get_motion_plan_response(
shortcut)
# set planning time to be the total time up to now
self.lightning_response.motion_plan_response.planning_time = time.time(
) - self.start_time
# record the planned path and planner
self.retrieved_and_final_path = [
None, None, result.planner_type.planner_type, pfsPath
]
if result.planner_type.planner_type == PlannerType.CLASSIC:
self.retrieved_and_final_path += [1, 0]
# store the number of newly generated nodes
self.total_new_node = len(pfsPath)
else:
self.retrieved_and_final_path += [1, 1]
# store the number of newly generated nodes
self.total_new_node = len(pfsPath)
self.total_new_node_NN = len(pfsPath)
self.plan_time = time.time() - self.start_time
print('pfs_done_cb: total time: %f' %
(time.time() - self.start_time))
self.lightning_response_ready_event.set()
self.done_lock.release()
#display new path in rviz
if self.draw_points:
self.draw_points_wrapper.draw_points(
pfsPath, self.current_group_name, "final",
self.DrawPointsWrapper.ANGLES,
self.DrawPointsWrapper.GREEN, 0.1)
if self.store_paths:
store_response = self._store_path(
pfsPath, [], result.planner_type.planner_type)
self._special_print(
"Lightning: Got a path from PFS, path stored = %s, number of library paths = %i"
% (store_response))
else:
self._special_print("Lightning: Got a path from PFS")
if self.publish_stats:
stat_msg.time = time.time() - self.start_time
self.stat_pub.publish(stat_msg)
return
else:
rospy.loginfo("Lightning: Call to PFS did not return a path")
if not self.use_rr or (self.rr_returned
and self.lightning_response is None):
self.lightning_response = GetMotionPlanResponse()
self.lightning_response.motion_plan_response.error_code.val = self.lightning_response.motion_plan_response.error_code.PLANNING_FAILED
self.lightning_response_ready_event.set()
self.done_lock.release()
def run(self, request):
#make sure the request is valid
self.retrieved_and_final_path = [None, None, None, None, None, None]
self.total_new_node = 0
self.total_new_node_NN = 0
start_and_goal = self._is_valid_motion_plan_request(request)
if start_and_goal is None:
response = GetMotionPlanResponse()
response.motion_plan_response.error_code.val = response.motion_plan_response.error_code.PLANNING_FAILED
return response
s, g = start_and_goal
self.rr_returned, self.pfs_returned = False, False
self.lightning_response = None
self.lightning_response_ready_event.clear()
self.current_joint_names = request.motion_plan_request.start_state.joint_state.name
self.current_group_name = request.motion_plan_request.group_name
#if self.draw_points:
# self.draw_points_wrapper.clear_points()
#start a timer that stops planners if they take too long
timer = threading.Thread(
target=self._lightning_timeout,
args=(request.motion_plan_request.allowed_planning_time, ))
timer.start()
self.start_time = time.time() # Used for timing stats.
# Send action requests to RR and PFS nodes.
if self.use_rr:
rr_client_goal = RRGoal()
rr_client_goal.start = s
rr_client_goal.goal = g
rr_client_goal.joint_names = self.current_joint_names
rr_client_goal.group_name = self.current_group_name
# allow less planning time due to intercommunication
rr_client_goal.allowed_planning_time = rospy.Duration(
request.motion_plan_request.allowed_planning_time - 5)
self.rr_client.wait_for_server()
rospy.loginfo("Lightning: Sending goal to RR")
self.rr_client.send_goal(rr_client_goal, done_cb=self._rr_done_cb)
if self.use_pfs:
pfs_client_goal = PFSGoal()
pfs_client_goal.start = s
pfs_client_goal.goal = g
pfs_client_goal.joint_names = self.current_joint_names
pfs_client_goal.group_name = self.current_group_name
pfs_client_goal.allowed_planning_time = rospy.Duration(
request.motion_plan_request.allowed_planning_time - 5)
self.pfs_client.wait_for_server()
rospy.loginfo("Lightning: Sending goal to PFS")
self.pfs_client.send_goal(pfs_client_goal,
done_cb=self._pfs_done_cb)
self.lightning_response_ready_event.wait()
if self.lightning_response.motion_plan_response.error_code.val != self.lightning_response.motion_plan_response.error_code.SUCCESS:
rospy.loginfo("Lightning: did not find a path")
# no need to do train MPNet
return self.lightning_response
rospy.loginfo("Lightning: Lightning is responding with a path")
"""
depending on the path property, train MPNet or not.
If the path is planned by Classical planner before or now, train it with MPNet.
1. transform path data into pytorch version
2. train Continual MPNet with the new path
3. save the new model weights to file
4. notify planners to update the model
"""
# transform path into pytorch version
self.train_model()
return self.lightning_response