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
