Python ViewPlanner示例

示例#1

文件： eval_plans.py 项目： kunzel/viper

    plans = jsonpickle.decode(json_data)
    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:

示例#2

文件： eval_views.py 项目： kunzel/viper

import viper.robots.scitos
robot = viper.robots.scitos.ScitosRobot()

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

示例#3

文件： eval_plans.py 项目： nilsbore/viper

    plans = jsonpickle.decode(json_data)
    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:

示例#4

文件： gen_plans.py 项目： nilsbore/viper

    views = jsonpickle.decode(json_data)
    rospy.loginfo("Loaded %s views" % len(views))

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)

try:
    rospy.loginfo("Wait for /robot_pose")
    robotpose_msg = rospy.wait_for_message("/robot_pose", Pose, timeout=10.0)
except rospy.ROSException, e:
    rospy.logwarn("Failed to get /robot_pose")

try:
    rospy.loginfo("Wait for /ptu/state")
    jointstate_msg = rospy.wait_for_message("/ptu/state",
                                            JointState,
                                            timeout=10.0)
except rospy.ROSException, e:
    rospy.logwarn("Failed to get /ptu/state")

示例#5

rospy.init_node('view_evaluation')

import viper.robots.scitos
robot = viper.robots.scitos.ScitosRobot()

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

示例#6

文件： view_planning.py 项目： ferdianjovan/viper_ros

    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'

示例#7

文件： gen_plans.py 项目： kunzel/viper

    views = jsonpickle.decode(json_data)
    rospy.loginfo("Loaded %s views"  % len(views))

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)

try:
    rospy.loginfo("Wait for /robot_pose")
    robotpose_msg = rospy.wait_for_message("/robot_pose", Pose, timeout=10.0)
except rospy.ROSException, e:
    rospy.logwarn("Failed to get /robot_pose")

try:
    rospy.loginfo("Wait for /ptu/state")
    jointstate_msg = rospy.wait_for_message("/ptu/state", JointState, timeout=10.0)
except rospy.ROSException, e:
    rospy.logwarn("Failed to get /ptu/state")

    
# Add current pose to view_costs (key: '-1')

示例#8

    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'

示例#9

import rospy
import jsonpickle
from viper.core.planner import ViewPlanner
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()