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:
