INPUT_FILE = rospy.get_param('~input_file', 'views.json') OUTPUT_FILE_COSTS = rospy.get_param('~output_file_costs', 'view_costs.json') OUTPUT_FILE_VALUES = rospy.get_param('~output_file_values', 'view_values.json') views = [] with open(INPUT_FILE, "r") as input_file: json_data = input_file.read() views = jsonpickle.decode(json_data) rospy.loginfo("Loaded %s views" % len(views)) planner = ViewPlanner(robot) view_values = planner.compute_view_values(views) robot_poses = PoseArray() robot_poses.header.frame_id = '/map' robot_poses.poses = [] for v in views: robot_poses.poses.append(v.get_ptu_pose()) robot_poses_pub.publish(robot_poses) #view_costs = planner.compute_view_costs(views) # triangle marker markerArray = MarkerArray() idx = 0 for view in views:
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'
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) print "Best plan: ID: ", p.ID, " Value: ", plan_values[p.ID] vis.visualize_plan(p, plan_values) # frustum marker # call compute_values to calc the frustum view_values = planner.compute_view_values(views) frustum_marker = MarkerArray() idx = 0 for view in views: if view.ID in pids: val = view_values[view.ID] print idx, val if val > 0: print "Create frustum marker with value", val vis.create_frustum_marker(frustum_marker, view, view.get_ptu_pose(), view_values) idx += 1 vis.pubfrustum.publish(frustum_marker) raw_input() vis.delete(p) break
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'