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)