def clicked_point(msg):
# rospy.loginfo(msg)
goal_pub = rospy.Publisher("/goal", MoveBaseActionGoal, queue_size=1)
plan_client = ServiceProxy("/move_base/make_plan", GetPlan)
goalPos = PoseStamped()
goalPos.header.frame_id = "map"
goalPos.header.stamp = rospy.Time.now()
goalPos.pose.position.x = msg.point.x
goalPos.pose.position.y = msg.point.y
goalPos.pose.orientation.x = rot[0]
goalPos.pose.orientation.y = rot[1]
goalPos.pose.orientation.z = rot[2]
goalPos.pose.orientation.w = rot[3]
getplan = GetPlanRequest()
getplan.start.header.frame_id = "map"
getplan.start.header.stamp = rospy.Time.now()
getplan.start.pose.position.x = x
getplan.start.pose.position.y = y
getplan.start.pose.orientation.x = 0
getplan.start.pose.orientation.y = 0
getplan.start.pose.orientation.z = 0
getplan.start.pose.orientation.w = 1
getplan.goal = goalPos
getplan.tolerance = 0.5
goal = MoveBaseActionGoal()
goal.goal.target_pose = goalPos
goal.header.frame_id = "map"
goal.header.stamp = rospy.Time.now()
goal_pub.publish(goal)
res = plan_client.call(getplan)
rospy.loginfo(res.plan.header.frame_id)
path_pub = rospy.Publisher("trajectory", Path, queue_size=1)
if isinstance(res, GetPlanResponse):
res.plan.header.frame_id = "map"
# res.plan.header.stamp = rospy.Time.now()
path_pub.publish(res.plan)
while True:
plan_pub = rospy.Publisher("/move_base/GlobalPlanner/plan",
Path,
queue_size=1)
plan_pub.publish(res.plan)
def call_service(service, args=None):
# Given the service name, fetch the type and class of the service,
# and a request instance
service_type = get_service_type(str(service))
if service_type is None:
raise InvalidServiceException(service)
service_class = get_service_class(service_type)
inst = get_service_request_instance(service_type)
# Populate the instance with the provided args
args_to_service_request_instance(service, inst, args)
# Call the service
proxy = ServiceProxy(service, service_class)
response = proxy.call(inst)
# Turn the response into JSON and pass to the callback
json_response = extract_values(response)
return json_response
def call_service(service, args=None):
# Given the service name, fetch the type and class of the service,
# and a request instance
service_type = get_service_type(str(service))
if service_type is None:
raise InvalidServiceException(service)
service_class = get_service_class(service_type)
inst = get_service_request_instance(service_type)
# Populate the instance with the provided args
args_to_service_request_instance(service, inst, args)
# Call the service
proxy = ServiceProxy(service, service_class)
response = proxy.call(inst)
# Turn the response into JSON and pass to the callback
json_response = extract_values(response)
return json_response
class AsynchonousRospyServiceProxy(object):
"""
An specialization of the rospy.ServiceProxy class which allows to call ros services
asynchronously. While the invokation of the service happens in a blocking manner, the call
does not wait for the service to terminate but passes the results to a callback method.
"""
def __init__(self, name, service_class, persistent=True, headers=None):
"""
Creates a new asynchronous service proxy.
@param name: name of service to call
@param service_class: auto-generated service class
@param persistent: (optional) if True, proxy maintains a persistent
connection to service. For the asynchronous retrieval of the result the connection has to
be persistent.
@param headers: (optional) arbitrary headers
"""
self.__proxy = ServiceProxy(name, service_class, persistent, headers)
self.__await_finish_thread = Thread(
target=self.__retrieve_results_async)
self.__await_finish_thread.daemon = True
self.__retrieve_result = Event()
self.__call_future = Future()
self.__shutdown_result_thread = False
def __call__(self, *args, **kwargs):
"""
Callable-style version of the service API compatible to ROS Service Proxy. This accepts
either a request message instance, or you can call directly with arguments to create a
new request instance. e.g.::
add_two_ints(AddTwoIntsRequest(1, 2))
add_two_ints(1, 2)
add_two_ints(a=1, b=2)
@param args: arguments to remote service
@param kwargs: message keyword arguments
@raise ROSSerializationException: If unable to serialize
message. This is usually a type error with one of the fields.
"""
return self.call(*args, **kwargs)
def call(self, *args, **kwds):
"""
Call the service. This accepts either a request message instance,
or you can call directly with arguments to create a new request instance. e.g.::
add_two_ints(AddTwoIntsRequest(1, 2))
add_two_ints(1, 2)
add_two_ints(a=1, b=2)
The first service invokation is executed in a blocking manner until the transport
connection is established. If the connection is set to be persistent (default) subsequent
service calls are executed asynchronously. In either case the result is passed to the
specified callback method.
@raise TypeError: if request is not of the valid type (Message)
@raise ServiceException: if communication with remote service fails
@raise ROSInterruptException: if node shutdown (e.g. ctrl-C) interrupts service call
@raise ROSSerializationException: If unable to serialize
message. This is usually a type error with one of the fields.
"""
if self.__retrieve_result.is_set():
raise ServiceException("Previous call did not return yet")
self.__call_future = Future()
self.__call_future.set_running_or_notify_cancel()
self.__call_future.start = time.time()
# initialize transport
if self.__proxy.transport is None:
result = self.__proxy.call(*args, **kwds)
self.__call_future.end = time.time()
self.__call_future.set_result(result)
else:
if not self.__await_finish_thread.is_alive():
self.__await_finish_thread.start()
# convert args/kwds to request message class
request = args_kwds_to_message(self.__proxy.request_class, args,
kwds)
# send the actual request message
self.__proxy.seq += 1
self.__proxy.transport.send_message(request, self.__proxy.seq)
self.__retrieve_result.set()
return self.__call_future
def close(self):
"""
Closes the connection to the rospy service
"""
self.__shutdown_result_thread = True
self.__retrieve_result.set()
self.__proxy.close()
def __retrieve_results_async(self):
"""
The method running in a separate thread which is responsible for retrieving the result of
the service call and invoking the callback methods.
"""
while not self.__shutdown_result_thread:
try:
self.__retrieve_result.wait()
if self.__shutdown_result_thread:
return
## The code is copied and adapted from rospy.ServiceProxy from here
try:
responses = self.__proxy.transport.receive_once()
if len(responses) == 0:
raise ServiceException(
"service [%s] returned no response" %
self.__proxy.resolved_name)
elif len(responses) > 1:
raise ServiceException(
"service [%s] returned multiple responses: %s" %
(self.__proxy.resolved_name, len(responses)))
except TransportException as e:
# convert lower-level exception to exposed type
if is_shutdown():
raise ROSInterruptException(
"node shutdown interrupted service call")
else:
raise ServiceException(
"transport error completing service call: %s" %
(str(e)))
## The code is copied and adapted from rospy.ServiceProxy until here
self.__retrieve_result.clear()
self.__call_future.end = time.time()
self.__call_future.set_result(*responses)
# pylint: disable=broad-except
except Exception:
logger.error("Error retrieving service result")
exept_info = exc_info()[1:3]
old_future = self.__call_future
self.__retrieve_result.clear()
old_future.end = time.time()
old_future.set_exception_info(*exept_info)
