Create New Dock

Create a new dock. Dock information is stored in persistent memory.

• Service Name: docking/dock_manager/add_dock

• Service Type: clearpath_dock_msgs/srv/AddDock

Python

from clearpath_config.common.utils.yaml import read_yaml
from clearpath_dock_msgs.msg import DockInfo
from clearpath_dock_msgs.srv import AddDock
from geometry_msgs.msg import Quaternion
import rclpy
from rclpy.node import Node

ROBOT_CONFIG_PATH = '/etc/clearpath/robot.yaml'
LOGGING_NAME = 'AddDock'

# TODO: Enter your dock info
DOCK_INFO = DockInfo(
            name='',
            dock_template='a300_side_dock',
            latitude=,
            longitude=,
            orientation=Quaternion()
)


class AddDockCLient(Node):

    def __init__(self, namespace: str):
        super().__init__('add_dock_client')
        self._add_dock_client = self.create_client(AddDock, f'/{namespace}/docking/dock_manager/add_dock')

        while not self._add_dock_client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info(f'[{LOGGING_NAME}] /{namespace}/docking/dock_manager/add_dock service not available, waiting again...')

    def add_dock(self, dock_info):
        req = AddDock.Request()
        req.dock = dock_info
        self.future = self._add_dock_client.call_async(req)
        self.future.add_done_callback(self.add_dock_srv_response_cb)

    def add_dock_srv_response_cb(self, future):
        try:
            response = future.result()
            if response.success:
                self.get_logger().info(f'[{LOGGING_NAME}] Added dock successfully!')
            else:
                self.get_logger().error(f'[{LOGGING_NAME}] Failed to add dock')
        except Exception as e:
            self.get_logger().error(f'[{LOGGING_NAME}] Service call failed: {e}')

        self.destroy_node()
        rclpy.shutdown()

def main(args=None):
    rclpy.init(args=args)
    robot_config = read_yaml(ROBOT_CONFIG_PATH)
    namespace = robot_config['system']['ros2']['namespace']

    add_dock_client = AddDockCLient(namespace)
    add_dock_client.add_dock(DOCK_INFO)

    rclpy.spin(add_dock_client)


if __name__ == '__main__':
    main()

C++

#include "rclcpp/rclcpp.hpp"
#include "clearpath_dock_msgs/srv/add_dock.hpp"

#include <chrono>
#include <cstdlib>
#include <memory>
#include <yaml-cpp/yaml.h>

using namespace std::chrono_literals;

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  
  YAML::Node robot_config = YAML::LoadFile("/etc/clearpath/robot.yaml");
  std::string namespace_ = robot_config["system"]["ros2"]["namespace"].as<std::string>();

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_dock_client");
  rclcpp::Client<clearpath_dock_msgs::srv::AddDock>::SharedPtr client =
    node->create_client<clearpath_dock_msgs::srv::AddDock>("/" + namespace_ + "/" + std::string("docking/dock_manager/add_dock"));

  auto request = std::make_shared<clearpath_dock_msgs::srv::AddDock::Request>();
  auto dock_info = clearpath_dock_msgs::msg::DockInfo();
  dock_info.name = "";
  dock_info.dock_template = "a300_side_dock";
  dock_info.latitude = ;
  dock_info.longitude = ;
  dock_info.orientation = ;

  request->dock = dock_info;

  while (!client->wait_for_service(2s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(rclcpp::get_logger("add_dock_client"), "Interrupted while waiting for the service. Exiting.");
      return 0;
    }
    RCLCPP_INFO(rclcpp::get_logger("add_dock_client"), "/%s/docking/dock_manager/add_dock service not available, waiting again...", namespace_.c_str());
  }

  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("add_dock_client"), "Added dock succesfully!");
    }
    else
    {
      RCLCPP_ERROR(rclcpp::get_logger("add_dock_client"), "Failed to add dock!");
    }
  } else {
    RCLCPP_ERROR(rclcpp::get_logger("add_dock_client"), "Failed to call service /%s/docking/dock_manager/add_dock", namespace_.c_str());
  }

  rclcpp::shutdown();
  return 0;
}