Execute Mission from Goal

Send the platform on an autonomous mission around the map, starting from a specific goal.

Action Name: autonomy/mission_from_goal

Action Type: clearpath_navigation_msgs/action/ExecuteMissionFromGoal

Python

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

from clearpath_navigation_msgs.action import ExecuteMissionFromGoal


class ExecuteMissionFromGoalActionClient(Node):

    def __init__(self):
        super().__init__('mission_from_goal_action_client')
        self._action_client = ActionClient(self, ExecuteMissionFromGoal, 'autonomy/mission_from_goal')

    def send_goal(self, order):
        goal_msg = ExecuteMissionFronGoal.Goal()

        # TODO: populate goal message

        self._action_client.wait_for_server()

        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)

        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected :(')
            return

        self.get_logger().info('Goal accepted :)')

        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    # def get_result_callback(self, future):
    #     result = future.result().result
    #     self.get_logger().info('Result: {0}'.format(result.sequence))
    #     rclpy.shutdown()

    # def feedback_callback(self, feedback_msg):
    #     feedback = feedback_msg.feedback
    #     self.get_logger().info('Received feedback: {0}'.format(feedback.partial_sequence))


if __name__ == '__main__':
    rclpy.init(args=args)
    action_client = ExecuteMissionFromGoalActionClient()
    # action_client.send_goal()
    rclpy.spin(action_client)

C++

#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>

#include "clearpath_navigation_msgs/action/execute_mission_from_goal.hpp"

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

namespace mission_from_goal_action_client
{
class ExecuteMissionFromGoalActionClient : public rclcpp::Node
{
public:
  using ExecuteMissionFromGoal = action_tutorials_interfaces::action::ExecuteMissionFromGoal;
  using GoalHandleExecuteMissionFromGoal = rclcpp_action::ClientGoalHandle<ExecuteMissionFromGoal>;

  explicit ExecuteMissionFromGoalActionClient(const rclcpp::NodeOptions & options)
  : Node("mission_from_goal_action_client", options)
  {
    this->client_ptr_ = rclcpp_action::create_client<ExecuteMissionFromGoal>(
      this, "autonomy/mission_from_goal");

    this->timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500),
      std::bind(&ExecuteMissionFromGoalActionClient::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(), "Action server not available after waiting");
      rclcpp::shutdown();
    }

    auto goal_msg = ExecuteMissionFromGoal::Goal();
    // TODO: update goal values
    // goal_msg.order = 10;

    RCLCPP_INFO(this->get_logger(), "Sending goal");

    auto send_goal_options = rclcpp_action::Client<ExecuteMissionFromGoal>::SendGoalOptions();
    send_goal_options.goal_response_callback =
      std::bind(&ExecuteMissionFromGoalActionClient::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
      std::bind(&ExecuteMissionFromGoalActionClient::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
      std::bind(&ExecuteMissionFromGoalActionClient::result_callback, this, _1);
    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

private:
  rclcpp_action::Client<ExecuteMissionFromGoal>::SharedPtr client_ptr_;
  rclcpp::TimerBase::SharedPtr timer_;

  void goal_response_callback(std::shared_future<GoalHandleExecuteMissionFromGoal::SharedPtr> future)
  {
    auto goal_handle = future.get();
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    } else {
      RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
    }
  }

// TODO: update with action feedback
//   void feedback_callback(
//     GoalHandleExecuteMissionFromGoal::SharedPtr,
//     const std::shared_ptr<const ExecuteMissionFromGoal::Feedback> feedback)
//   {
//     std::stringstream ss;
//     ss << "Next number in sequence received: ";
//     for (auto number : feedback->partial_sequence) {
//       ss << number << " ";
//     }
//     RCLCPP_INFO(this->get_logger(), ss.str().c_str());
//   }

// TODO: update with action result
//   void result_callback(const GoalHandleExecuteMissionFromGoal::WrappedResult & result)
//   {
//     switch (result.code) {
//       case rclcpp_action::ResultCode::SUCCEEDED:
//         break;
//       case rclcpp_action::ResultCode::ABORTED:
//         RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
//         return;
//       case rclcpp_action::ResultCode::CANCELED:
//         RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
//         return;
//       default:
//         RCLCPP_ERROR(this->get_logger(), "Unknown result code");
//         return;
//     }
//     std::stringstream ss;
//     ss << "Result received: ";
//     for (auto number : result.result->sequence) {
//       ss << number << " ";
//     }
//     RCLCPP_INFO(this->get_logger(), ss.str().c_str());
//     rclcpp::shutdown();
//   }
};  // class ExecuteMissionFromGoalActionClient

}  // namespace mission_from_goal_action_client

RCLCPP_COMPONENTS_REGISTER_NODE(mission_from_goal_action_client::ExecuteMissionFromGoalActionClient)