Publish planning scene at startup

[jarkenau]

Currently the planning scene, is added with the first pick action:

grasplan/src/grasplan/pick.py

Lines 319 to 333 in f628097

	for planning_scene_box in self.planning_scene_boxes:
	# add a box to the planning scene
	table_pose = PoseStamped()
	table_pose.header.frame_id = planning_scene_box['frame_id']
	box_x = planning_scene_box['box_x_dimension']
	box_y = planning_scene_box['box_y_dimension']
	box_z = planning_scene_box['box_z_dimension']
	table_pose.pose.position.x = planning_scene_box['box_position_x']
	table_pose.pose.position.y = planning_scene_box['box_position_y']
	table_pose.pose.position.z = planning_scene_box['box_position_z']
	table_pose.pose.orientation.x = planning_scene_box['box_orientation_x']
	table_pose.pose.orientation.y = planning_scene_box['box_orientation_y']
	table_pose.pose.orientation.z = planning_scene_box['box_orientation_z']
	table_pose.pose.orientation.w = planning_scene_box['box_orientation_w']
	self.scene.add_box(planning_scene_box['scene_name'], table_pose, (box_x, box_y, box_z))

I think it would be best to this as an initialization step before any action, as this only requires iterating over the yaml file with respective information.

This would also allow us to publish the static transforms for the objects once and not in each place action:

grasplan/src/grasplan/place.py

Lines 274 to 275 in f628097

	# TODO: find a better place for adding the static transforms, it shouldn't be done for each place
	self.add_planning_scene_objects(self.scene)

cc @oscar-lima

[mintar]

+1

Another advantage: This would also visualize the planning scene in RViz right from the start, without first executing a pick action.