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'
