concert_service_waypoint_navigation: publish

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import rospy
00003 from concert_software_farmer import SoftwareFarmClient, FailedToStartSoftwareException
00004 
00005 import world_canvas_client
00006 
00007 def publish_map(world, namespace):
00008     map_name = rospy.get_param('map_name')
00009     map_topic = rospy.get_param('map_topic')
00010     rospy.loginfo("Publish World: Map - %s"%map_name)
00011 
00012     # Get the 2D map for the given world
00013     map_ac = world_canvas_client.AnnotationCollection(world=world, types=['nav_msgs/OccupancyGrid'], srv_namespace=namespace, names=[map_name])
00014     map_ac.load_data()
00015 
00016     # Publish the map on server side; topic type is get from the annotation info
00017     map_ac.publish(map_topic, None, False, False)   # i.e. by_server=True, as_list=False
00018     
00019     return map_ac
00020 
00021 def publish_location(world, namespace):
00022     waypoints_topic = rospy.get_param('waypoints_topic')
00023     waypoints_viz_topic = rospy.get_param('waypoints_viz_topic')
00024     rospy.loginfo("Publish World: Waypoints   - %s"%waypoints_topic)
00025     
00026     waypoints_ac = world_canvas_client.AnnotationCollection(world=world, types=['yocs_msgs/Waypoint'], srv_namespace=namespace)
00027     waypoints_ac.load_data()
00028     waypoints_ac.publish(waypoints_topic, 'yocs_msgs/WaypointList', by_server=False, as_list=True)
00029     waypoints_ac.publish_markers(waypoints_viz_topic)
00030     return waypoints_ac
00031 
00032 if __name__ == '__main__':
00033     rospy.init_node('world_canvas_client')
00034 
00035     try:
00036         sfc = SoftwareFarmClient()
00037         success, namespace, _unused_parameters = sfc.allocate("concert_software_common/world_canvas_server")
00038 
00039         if not success:
00040             raise FailedToStartSoftwareException("Failed to allocate software")
00041 
00042         rospy.loginfo("Publish World : world canvas namespace : %s"%namespace)
00043         world = rospy.get_param('world')
00044         rospy.loginfo("Publish World: %s"%world)
00045         map_ac = publish_map(world, namespace)
00046         waypoint_ac = publish_location(world, namespace)
00047         rospy.loginfo("Done")
00048         rospy.spin()
00049 
00050     except (FailedToStartSoftwareException) as e:
00051         rospy.logerr("Publish World : %s"%str(e))