Resume Autonomy

Resume autonomy from a previouly paused state.

• Service Name: autonomy/resume

• Service Type: std_srvs/srv/SetBool

Python

import rclpy
from rclpy.node import Node

from std_srvs.srv import SetBool


class ResumeAutonomy(Node):

    def __init__(self):
        super().__init__('resume_autonomy_client')
        self._srv_client = self.create_client(Trigger, 'autonomy/resume')
        while not self._srv_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        self.req = SetBool.Request()

    def send_request(self):
        self.req.data = True
        self.future = self._srv_client.call_async(self.req)
        rclpy.spin_until_future_complete(self, self.future)
        return self.future.result()

if __name__ == '__main__':
    rclpy.init(args=args)
    resume_autonomy_client = ResumeAutonomy()
    response = resume_autonomy_client.send_request()
    if response.success:
      resume_autonomy_client.get_logger().info('Autonomy resumed.'))
    else:
      resume_autonomy_client.get_logger().error('Autonomy failed to resume!')
    
    resume_autonomy_client.destroy_node()
    rclpy.shutdown()

C++

#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/set_bool.hpp"

#include <chrono>
#include <cstdlib>
#include <memory>

using namespace std::chrono_literals;

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("resume_autonomy_client");
  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client =                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            
    node->create_client<std_srvs::srv::SetBool>("autonomy/resume");

  auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
  request.data = true;

  while (!client->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
      return 0;
    }
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
  }

  auto result = client->async_send_request(request);
  // Wait for the result.
  if (rclcpp::spin_until_future_complete(node, result) ==
    rclcpp::FutureReturnCode::SUCCESS)
  {
    if (result.get()->success)
    {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Autonomy resumed", );
    }
    else
    {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Autonomy failed to resume!");
    }
  } else {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service autonomy/resume");
  }

  rclcpp::shutdown();
  return 0;
}