rospy.loginfo("Loaded %s plans" % len(plans)) view_values = dict() with open(INPUT_FILE_VALUES, "r") as input_file: json_data = input_file.read() view_values = jsonpickle.decode(json_data) rospy.loginfo("Loaded view values") view_costs = dict() with open(INPUT_FILE_COSTS, "r") as input_file: json_data = input_file.read() view_costs = jsonpickle.decode(json_data) rospy.loginfo("Loaded view costs") planner = ViewPlanner(robot) plan_values = planner.compute_plan_values(plans, view_values, view_costs) min_cost_plan_id = planner.min_cost_plan(plan_values) vis = Vis() for p in plans: print "ID: ", p.ID, " Value: ", plan_values[p.ID] #vis.visualize_plan(p, plan_values) #raw_input() #vis.delete(p) for p in plans: if p.ID == min_cost_plan_id: pids = [] for v in p.views: pids.append(v.ID)
def execute(self, userdata): rospy.loginfo('Executing state %s', self.__class__.__name__) userdata.state = self.__class__.__name__ robot = ScitosRobot() NUM_OF_VIEWS = userdata.num_of_views planner = ViewPlanner(robot) rospy.loginfo('Generate views.') views = planner.sample_views(NUM_OF_VIEWS) rospy.loginfo('Generate views. Done. (%s views have been generated)' % len(views)) view_values = planner.compute_view_values(views) view_costs = planner.compute_view_costs(views) print view_values print view_costs current_view = self.get_current_view() if current_view == None: return 'aborted' vcosts = dict() for v in views: cost = robot.cost(current_view,v) vcosts[v.ID] = cost view_costs[current_view.ID] = vcosts NUM_OF_PLANS = rospy.get_param('~num_of_plans', 10) PLAN_LENGTH = rospy.get_param('~plan_length', 10) RHO = rospy.get_param('~rho', 1.0) rospy.loginfo("Started plan sampling.") plans = planner.sample_plans(NUM_OF_PLANS, PLAN_LENGTH, RHO, views, view_values, view_costs, current_view.ID) rospy.loginfo("Stopped plan sampling.") plan_values = planner.compute_plan_values(plans, view_values, view_costs) best_plan_id = planner.min_cost_plan(plan_values) for p in plans: if p.ID == best_plan_id: break else: p = None if p.ID != best_plan_id: print "Something bad has happend!" userdata.views = p.views # visulaize for debug robot_poses = PoseArray() robot_poses.header.frame_id = '/map' robot_poses.poses = [] for v in views: robot_poses.poses.append(v.get_robot_pose()) print len(robot_poses.poses) self.robot_poses_pub.publish(robot_poses) return 'succeeded'
rospy.loginfo("Loaded %s plans" % len(plans)) view_values = dict() with open(INPUT_FILE_VALUES, "r") as input_file: json_data = input_file.read() view_values = jsonpickle.decode(json_data) rospy.loginfo("Loaded view values") view_costs = dict() with open(INPUT_FILE_COSTS, "r") as input_file: json_data = input_file.read() view_costs = jsonpickle.decode(json_data) rospy.loginfo("Loaded view costs") planner = ViewPlanner(robot) plan_values = planner.compute_plan_values(plans, view_values, view_costs) min_cost_plan_id = planner.min_cost_plan(plan_values) vis = Vis() for p in plans: print "ID: ", p.ID, " Value: ", plan_values[p.ID] #vis.visualize_plan(p, plan_values) #raw_input() #vis.delete(p) for p in plans: if p.ID == min_cost_plan_id: pids = [] for v in p.views: pids.append(v.ID)
def execute(self, userdata): rospy.loginfo('Executing state %s', self.__class__.__name__) userdata.state = self.__class__.__name__ robot = ScitosRobot() NUM_OF_VIEWS = userdata.num_of_views planner = ViewPlanner(robot) rospy.loginfo('Generate views.') views = planner.sample_views(NUM_OF_VIEWS) rospy.loginfo('Generate views. Done. (%s views have been generated)' % len(views)) view_values = planner.compute_view_values(views) view_costs = planner.compute_view_costs(views) print view_values print view_costs current_view = self.get_current_view() if current_view == None: return 'aborted' vcosts = dict() for v in views: cost = robot.cost(current_view, v) vcosts[v.ID] = cost view_costs[current_view.ID] = vcosts NUM_OF_PLANS = rospy.get_param('~num_of_plans', 10) PLAN_LENGTH = rospy.get_param('~plan_length', 10) RHO = rospy.get_param('~rho', 1.0) rospy.loginfo("Started plan sampling.") plans = planner.sample_plans(NUM_OF_PLANS, PLAN_LENGTH, RHO, views, view_values, view_costs, current_view.ID) rospy.loginfo("Stopped plan sampling.") plan_values = planner.compute_plan_values(plans, view_values, view_costs) best_plan_id = planner.min_cost_plan(plan_values) for p in plans: if p.ID == best_plan_id: break else: p = None if p.ID != best_plan_id: print "Something bad has happend!" userdata.views = p.views # visulaize for debug robot_poses = PoseArray() robot_poses.header.frame_id = '/map' robot_poses.poses = [] for v in views: robot_poses.poses.append(v.get_robot_pose()) print len(robot_poses.poses) self.robot_poses_pub.publish(robot_poses) return 'succeeded'