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()