Execute GoTo

Send the platform to a location on the map.

• Action Name: autonomy/goto

• Action Type: clearpath_navigation_msgs/action/ExecuteGoTo

Python

from action_msgs.msg import GoalStatus  # Import for reference to status constants
from clearpath_config.common.utils.yaml import read_yaml
from clearpath_navigation_msgs.action import ExecuteGoTo
from clearpath_navigation_msgs.msg import Waypoint
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

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

# TODO: Enter your map uuid and waypoint info
MAP_ID: str = ''
WAYPOINT = Waypoint(
            uuid='',
            name='',
            latitude=,
            longitude=,
            heading=,
            position_tolerance=-1.0,    # -1.0 means off
            yaw_tolerance=-1.0,         # -1.0 means off
            tasks=[]
)


class ExecuteGoToActionClient(Node):

    def __init__(self, namespace: str):
        super().__init__('execute_goto')
        self._namespace = namespace
        self._action_client = ActionClient(self, ExecuteGoTo, f'/{self._namespace}/autonomy/goto')
        self._goto_goal_handle = None
        self._goto_goal_cancelled = False
        self._goto_running = False
        self._goto_completed = False

    ###################################
    # GoTo Action Client Functions #
    ###################################
    def send_goto_goal(self, map_id, waypoint):
        goal_msg = ExecuteGoTo.Goal()

        # populate goal message
        goal_msg.map_uuid = map_id
        goal_msg.waypoint = waypoint

        if not self._action_client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error(f'[{LOGGING_NAME}] /{self._namespace}/autonomy/goto action server not available!')
            self.destroy_node()
            rclpy.shutdown()
            return

        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.goto_feedback_cb)
        self._send_goal_future.add_done_callback(self.goto_goal_response_cb)

    def goto_goal_response_cb(self, future):
        self._goto_goal_handle = future.result()
        if not self._goto_goal_handle.accepted:
            self.get_logger().info(f'[{LOGGING_NAME}] GoTo Goal rejected')
            self._goto_goal_handle = None
            self.destroy_node()
            rclpy.shutdown()
            return

        self._goto_running = True
        self._goto_completed = False
        self._goto_goal_cancelled = False
        self.get_logger().info(f'[{LOGGING_NAME}] GoTo Goal accepted')

        self._get_result_future = self._goto_goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.goto_get_result_cb)

    def goto_feedback_cb(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info(f'[{LOGGING_NAME}] GoTo has been running for: {feedback.elapsed_time:.2f}s', throttle_duration_sec=60)

    def goto_cancel_response_cb(self, future):
        cancel_response = future.result()
        self.get_logger().info(f'[{LOGGING_NAME}] Cancel response received: {cancel_response.return_code}')
        if len(cancel_response.goals_canceling) > 0:
            self.get_logger().info(f'[{LOGGING_NAME}] Canceling of goto goal complete')
        else:
            self.get_logger().warning(f'[{LOGGING_NAME}] GoTo Goal failed to cancel')

    def goto_get_result_cb(self, future):
        result = future.result().result
        status = future.result().status

        # Compare the status integer against the constants defined in the message
        if status == GoalStatus.STATUS_SUCCEEDED:
            if result.success:
                self.get_logger().info(f'[{LOGGING_NAME}] [GoTo Goal Result] Succeeded! Result: Robot completed goto!')
                self._goto_completed = True
            else:
                self.get_logger().info(f'[{LOGGING_NAME}] [GoTo Goal Result] Succeeded! Result: Robot failed to complete goto')

        elif status == GoalStatus.STATUS_CANCELED:
            self.get_logger().info(f'[{LOGGING_NAME}] [GoTo Goal Result] Cancelled!')
            self._goto_goal_cancelled = True

        elif status == GoalStatus.STATUS_ABORTED:
            self.get_logger().info(f'[{LOGGING_NAME}] [GoTo Goal Result] Aborted!')
        else:
            self.get_logger().info(f'[{LOGGING_NAME}] [GoTo Goal Result] Finished with unknown status: {status}')

        self._goto_goal_handle = None
        self._goto_running = False

        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']

    goto = ExecuteGoToActionClient(namespace)
    goto.send_goto_goal(MAP_ID, WAYPOINT)
    rclpy.spin(goto)


if __name__ == '__main__':
    main()

C++

#include <functional>
#include <future>
#include <memory>
#include <string>
#include <yaml-cpp/yaml.h>

#include "clearpath_navigation_msgs/action/execute_go_to.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

// TODO: Enter your map id and waypoint information
const std::string MAP_ID = "";
const std::string WAYPOINT_ID = "";
const std::string WAYPOINT_NAME = "";
const double WAYPOINT_LAT = ;
const double WAYPOINT_LON = ;
const double WAYPOINT_HEADING = ;
const double WAYPOINT_POSITION_TOLERANCE = -1.0;  // -1.0 means off
const double WAYPOINT_YAW_TOLERANCE = -1.0;       // -1.0 means off


namespace goto_action_client
{
class ExecuteGoToActionClient : public rclcpp::Node
{
public:
  using ExecuteGoTo = clearpath_navigation_msgs::action::ExecuteGoTo;
  using GoalHandleExecuteGoTo = rclcpp_action::ClientGoalHandle<ExecuteGoTo>;

  explicit ExecuteGoToActionClient(const rclcpp::NodeOptions & options)
  : Node("execute_goto", options)
  {
    YAML::Node robot_config = YAML::LoadFile("/etc/clearpath/robot.yaml");
    namespace_ = robot_config["system"]["ros2"]["namespace"].as<std::string>();

    this->client_ptr_ = rclcpp_action::create_client<ExecuteGoTo>(
      this, "/" + namespace_ + "/" + std::string("autonomy/goto"));

    this->timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500),
      std::bind(&ExecuteGoToActionClient::send_goal, this));
  }

  void send_goal()
  {
    using namespace std::placeholders;

    this->timer_->cancel();

    if (!this->client_ptr_->wait_for_action_server()) {
      RCLCPP_ERROR(this->get_logger(), "%s/autonomy/goto action server not available after waiting", namespace_.c_str());
      rclcpp::shutdown();
    }

    auto goal_msg = ExecuteGoTo::Goal();
    goal_msg.map_uuid = MAP_ID;
    
    auto waypoint = clearpath_navigation_msgs::msg::Waypoint();
    waypoint.uuid = WAYPOINT_ID;
    waypoint.name = WAYPOINT_NAME;
    waypoint.uuid = WAYPOINT_ID;
    waypoint.latitude = WAYPOINT_LAT;
    waypoint.longitude = WAYPOINT_LON;
    waypoint.heading = WAYPOINT_HEADING;
    waypoint.position_tolerance = WAYPOINT_POSITION_TOLERANCE;
    waypoint.yaw_tolerance = WAYPOINT_YAW_TOLERANCE;
    goal_msg.waypoint = waypoint;

    auto send_goal_options = rclcpp_action::Client<ExecuteGoTo>::SendGoalOptions();

    send_goal_options.goal_response_callback = [this](const GoalHandleExecuteGoTo::SharedPtr & goal_handle)
    {
      if (!goal_handle) {
        RCLCPP_ERROR(this->get_logger(), "GoTo Goal was rejected by server");
      } else {
        RCLCPP_INFO(this->get_logger(), "GoTo Goal accepted by server, waiting for result");
      }
    };

    send_goal_options.feedback_callback = [this](
      GoalHandleExecuteGoTo::SharedPtr,
      const std::shared_ptr<const ExecuteGoTo::Feedback> feedback)
    {
      RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 60000, "GoTo has been running for: %.3f", feedback->elapsed_time);
    };

    send_goal_options.result_callback = [this](const GoalHandleExecuteGoTo::WrappedResult & result)
    {
      switch (result.code) {
        case rclcpp_action::ResultCode::SUCCEEDED:
          if (result.result->success)
          {
            RCLCPP_INFO(this->get_logger(), "[GoTo Goal Result] Succeeded! Result: Robot arrived at goto position successfully!");
          }
          else
          {
            RCLCPP_ERROR(this->get_logger(), "[GoTo Goal Result] Succeeded! Result: Robot failed to arrive to goto position");
          }
          break;
        case rclcpp_action::ResultCode::ABORTED:
          RCLCPP_ERROR(this->get_logger(), "[GoTo Goal Result] Aborted!");
          return;
        case rclcpp_action::ResultCode::CANCELED:
          RCLCPP_ERROR(this->get_logger(), "[GoTo Goal Result] Cancelled!");
          return;
        default:
          RCLCPP_ERROR(this->get_logger(), "[GoTo Goal Result] Finished with unknown code");
          return;
      }
      rclcpp::shutdown();
    };

    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

private:
  rclcpp_action::Client<ExecuteGoTo>::SharedPtr client_ptr_;
  rclcpp::TimerBase::SharedPtr timer_;
  std::string namespace_;

};  // class ExecuteGoToActionClient

}  // namespace goto_action_client

int main()
{
}

RCLCPP_COMPONENTS_REGISTER_NODE(goto_action_client::ExecuteGoToActionClient)