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'
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'
from geometry_msgs.msg import PoseArray robot_poses_pub = rospy.Publisher('robot_poses', PoseArray) ptu_poses_pub = rospy.Publisher('ptu_poses', PoseArray) rospy.init_node('view_generation') import viper.robots.scitos robot = viper.robots.scitos.ScitosRobot() NUM_OF_VIEWS = rospy.get_param('~num_of_views', 100) FILENAME = rospy.get_param('~output_file', 'views.json') planner = ViewPlanner(robot) rospy.loginfo('Generate views.') views = planner.sample_views(NUM_OF_VIEWS) rospy.loginfo('Generate views. Done.') 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) robot_poses_pub.publish(robot_poses) ptu_poses = PoseArray() ptu_poses.header.frame_id = '/map' ptu_poses.poses = [] for v in views: