def main(): global static_map, inflation_radius, cost_radius print "PRACTICE 02 - " + NAME rospy.init_node("practice02") rospy.Service('/inflated_map', GetMap, callback_inflated_map) rospy.Service('/cost_map', GetMap, callback_cost_map) pub_inflated = rospy.Publisher("/inflated_map", OccupancyGrid, queue_size=10) rospy.wait_for_service('/static_map') clt_static_map = rospy.ServiceProxy("/static_map", GetMap) static_map = clt_static_map() static_map = static_map.map loop = rospy.Rate(10) counter = 0 cost_radius = 0.5 inflation_radius = 0.3 while not rospy.is_shutdown(): if counter == 0: if rospy.has_param("/navigation/path_planning/cost_radius"): cost_radius = rospy.get_param( "/navigation/path_planning/cost_radius") if rospy.has_param("/navigation/path_planning/inflation_radius"): new_inflation_radius = rospy.get_param( "/navigation/path_planning/inflation_radius") if new_inflation_radius != inflation_radius: inflation_radius = new_inflation_radius pub_inflated.publish( callback_inflated_map(GetMapRequest()).map) counter = (counter + 1) % 10 loop.sleep()
def task(map_name): save_map = rospy.ServiceProxy(name='/map_manager/add_map', service_class=AddMap) get_map = rospy.ServiceProxy(name='modular_gmapping/get_map', service_class=GetMap) save_map.wait_for_service(timeout=30) get_map.wait_for_service(timeout=30) # Get the OccupancyGrid try: get_map_response = get_map.call( GetMapRequest()) # type: GetMapResponse except rospy.ServiceException as e: logger.error('Failed to get map from gmapping: {}'.format(e)) return FAILED # Save the map save_map_response = save_map.call( AddMapRequest( map=Map(info=MapInfo(name=map_name, meta_data=get_map_response.map.info)), occupancy_grid=CompressedImage( format='raw', data=get_map_response.map.data))) # type: AddMapResponse if save_map_response.success is False: logger.error('Failed to save map: {}'.format( save_map_response.message)) return FAILED return COMPLETED
def request_occupancy_grid(self): # Make request to map_loader service rospy.wait_for_service('static_map') try: get_map_service = rospy.ServiceProxy('static_map', GetMap) req = GetMapRequest() resp = get_map_service(req) rospy.loginfo("Successfully loaded occupancy grid from map_server") return resp.map except rospy.ServiceException, e: rospy.loginfo("Service call failed: %s" % e)
def get_static_map(self): srv_request = GetMapRequest() rospy.wait_for_service('%s/static_map' % self.NS) try: self._map = self._service_client_get_map(srv_request).map nmap = np.array(self._map.data) self._freespace = np.where(nmap == 0)[0] except error as e: print('Get static map error:', e) return
def main(): rospy.init_node("map_call_client_service_node") rospy.wait_for_service("/static_map") try: rospy.loginfo("Execiting client") mapClient = rospy.ServiceProxy('/static_map', GetMap) mapRequest = GetMapRequest() result = mapClient(mapRequest) if result: rospy.loginfo("EVETHING IS FINE") mapResolution = "Resolution: " + str(result.map.info.resolution) mapDimensions = "width: " + str( result.map.info.width) + ", height: " + str( result.map.info.height) mapData = mapResolution + " " + mapDimensions rospy.loginfo(mapData) except Exception as e: rospy.logerr("An error haas occurred on client server.")
#! /usr/bin/env python """ This is a service client. The aim of this file is to access the provided map through a service """ import rospy from nav_msgs.srv import GetMap, GetMapRequest rospy.init_node('map_requester') # initialise node # wait for service to be available rospy.wait_for_service("/static_map") conn_map_provider = rospy.ServiceProxy("/static_map", GetMap) map_data = GetMapRequest() result = conn_map_provider(map_data) print result
def __init__(self): self._static_map_req = GetMapRequest() self._scan_msg = LaserScan() self._map_msg = OccupancyGrid() self._initialpose_msg = PoseWithCovarianceStamped() # parameters self._use_static_map = rospy.get_param('~use_static_map', default=True) self._init_x = rospy.get_param('~init_x', default=0.0) self._init_y = rospy.get_param('~init_y', default=0.0) self._init_phi = rospy.get_param('~init_phi', default=0.0) # service clients try: rospy.wait_for_service('/static_map', timeout=3) except ROSException as e: print(e.message + '. Make sure that service node is running!') self._static_map_sc = rospy.ServiceProxy('/static_map', GetMap) # tf broadcasters self._map_odom_tfb = tf.TransformBroadcaster() # tf listeners self._odom_base_link_tfl = tf.TransformListener() self._base_link_base_laser_tfl = tf.TransformListener() # user init # processing queue self._scan_queue = PriorityQueue(maxsize=2) # get static map if self._use_static_map: static_map_res = self._static_map_sc(self._static_map_req) static_map_res.map.data = list(static_map_res.map.data) # initialize map with received one self._map = pymrpt.maps.COccupancyGridMap2D() self._map.from_ROS_OccupancyGrid_msg(static_map_res.map) # print output print 'received initial STATIC map!' print static_map_res.map.info else: # initialize empty map and wait for published ones self._map = pymrpt.maps.COccupancyGridMap2D() # setup transformations self._tf = tf.TransformerROS() self._map_to_odom = pymrpt.poses.CPose2D() self._map_to_base = pymrpt.poses.CPose2D() self._odom_to_base = pymrpt.poses.CPose2D() # setup initial map to base tf ("/odom -> /base_link" also should be (0,0,0)) self._map_to_base.x = self._init_x self._map_to_base.y = self._init_y self._map_to_base.phi = self._init_phi # setup scanmatcher lock self._scanmatcher_lock = Lock() # end of user init # subscribers rospy.Subscriber('/scan', LaserScan, self._scan_callback, queue_size=10) rospy.Subscriber('/map', OccupancyGrid, self._map_callback, queue_size=10) rospy.Subscriber('/initialpose', PoseWithCovarianceStamped, self._initialpose_callback, queue_size=10)
#!/usr/bin/env python import rospy from std_msgs.msg import Empty from nav_msgs.srv import GetMap, GetMapRequest rospy.init_node("my_caller") rospy.wait_for_service("/static_map") my_connector = rospy.ServiceProxy("/static_map", GetMap) my_call = GetMapRequest() result = my_connector(my_call) print(result)
#! /usr/bin/env python import rospy from nav_msgs.srv import GetMap, GetMapRequest rospy.init_node('service_clien') rospy.wait_for_service('/static_map') call_the_ser = rospy.ServiceProxy('/static_map', GetMap) serv_object = GetMapRequest() result = call_the_ser(serv_object) print(result)
#! /usr/bin/env python import rospy from nav_msgs.srv import GetMap, GetMapRequest import sys rospy.init_node( 'service_client') # Initialise a ROS node with the name service_client rospy.wait_for_service( '/static_map') # Wait for the service /static_map to be running get_map_service = rospy.ServiceProxy( '/static_map', GetMap) # Create the connection to the service get_map = GetMapRequest() # Create an object of type GetMapRequest result = get_map_service(get_map) # Call the service print result # print teh result given by the service called
uint32 width uint32 height geometry_msgs/Pose origin geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w int8[] data """ import rospy from nav_msgs.srv import GetMap, GetMapRequest rospy.init_node("get_my_map") rospy.loginfo("Waiting for /static_map service to start...") rospy.wait_for_service("/static_map") try: getMapClient = rospy.ServiceProxy("/static_map", GetMap) except rospy.ServiceException, e: print("Serice call failed {}".format(e)) req = GetMapRequest() results = getMapClient(req) print("Resolution: {}".format(results.map.info.resolution)) print("w: {}, h {}".format(results.map.info.width, results.map.info.height))
#! /usr/bin/env python import rospy from nav_msgs.srv import GetMap, GetMapRequest rospy.init_node('get_map_data') rospy.wait_for_service('/static_map') get_map_request = GetMapRequest() get_map_client = rospy.ServiceProxy('/static_map', GetMap) map_data = get_map_client(get_map_request) rospy.loginfo(map_data)
#! /usr/bin/env python import rospy from nav_msgs.srv import GetMap, GetMapRequest rospy.init_node('get_map_data') rospy.loginfo('Waiting for /static_map service') rospy.wait_for_service('/static_map') rospy.loginfo('Static Map service found') get_static_map = rospy.ServiceProxy('/static_map', GetMap) req_msg = GetMapRequest() result = get_static_map(req_msg) print '\nMap Info:' print 'Dimension[W x H]:', result.map.info.width, 'x', result.map.info.height print 'Resolution:', result.map.info.resolution