Lecture9. ROS 2 Action Programming

# Terminal 1
ros2 launch src_gazebo wall_world.launch.py

# Terminal 2
cbp py_action_tutorial && source install/local_setup.bash
ros2 run py_action_tutorial parking_action_server

# Terminal 3
ros2 action send_goal /src_parking custom_interfaces/action/Parking "start_flag: true" --feedback

Feedback:
    distance: 0.7399989366531372

Feedback:
    distance: 0.630066454410553

Result:
    message: '[Success!] Oh... Teach me how you did :0'

$ ros2 interface show custom_interfaces/action/Parking
#goal definition
bool start_flag
---
#result definition
string message
---
#feedback definition
float32 distance

import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer

class ParkingActionServer(Node):

    def __init__(self):
        super().__init__('parking_action_server')

        self.laser_sub = self.create_subscription(
            LaserScan, 'scan', self.sub_callback, 10
        )

        self.action_server = ActionServer(
            self, Parking, 'src_parking',
            goal_callback=self.goal_callback,
            cancel_callback=self.cancel_callback,
            execute_callback=self.execute_callback,
        )

from rclpy.action import GoalResponse
...

def goal_callback(self, goal_request):
    self.get_logger().info('Received goal request.')
    self.goal = goal_request
    return GoalResponse.ACCEPT

from rclpy.action import CancelResponse
...
def cancel_callback(self, goal_handle):
    self.is_sub = False
    self.is_done = True
		# print(dir(goal_handle))
    goal_handle.canceled()

    self.get_logger().info('Received cancel request')
    return CancelResponse.ACCEPT

def execute_callback(self, goal_handle):
    ...
    feedback_msg = Parking.Feedback()

    while self.f_obs_distance > 0.5 and self.is_done == False:
        goal_handle.publish_feedback(feedback_msg)
        ...

    ...

    return result

    goal_handle.succeed()

    result = Parking.Result()
    lr_diff = abs(self.r_obs_distance - self.l_obs_distance)
    if lr_diff < 0.15:
        result.message = "[Success!] Oh... Teach me how you did :0"
    else:
        result.message = "[Fail] Be careful, Poor Driver! "

    self.is_done = True

    return result

def main(args=None):

    rclpy.init(args=args)

    # parking_action_server = ParkingActionServer()
    # rclpy.spin(parking_action_server)
    # parking_action_server.destroy_node()
    # rclpy.shutdown()ㅁ
    ...

ros2 action send_goal /src_parking custom_interfaces/action/Parking "start_flag: true" --feedback

ros2 run cpp_action_tutorial parking_status_client

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

from custom_interfaces.action import Parking

class ParkingActionClient(Node):

    def __init__(self):
        super().__init__('parking_action_client')
        self._action_client = ActionClient(self, Parking, 'src_parking')

def send_goal(self):
    ...
    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 main(args=None):
    ...
    action_client.send_goal()

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

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

    self._get_result_future = self.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(f'Result: {result.message}')
    rclpy.shutdown()

def feedback_callback(self, feedback_msg):
    ...
    if feedback.distance > 6.0:
        self.cancel_goal()

def cancel_goal(self):
    self.get_logger().info('Canceling goal')
    future = self.goal_handle.cancel_goal_async()
    future.add_done_callback(self.goal_canceled_callback)

def goal_canceled_callback(self, future):
    cancel_response = future.result()

    if len(cancel_response.goals_canceling) > 0:
        self.get_logger().info('Cancelling of goal complete')
    else:
        self.get_logger().warning('Goal failed to cancel')

def main(args=None):
    rclpy.init(args=args)

    action_client = ParkingActionClient()
    action_client.send_goal()
    rclpy.spin(action_client)

# Terminal1 - gazebo env
ros2 launch rgbd_world depth_world.launch.py

# Terminal2 - Assignment Node
ros2 run py_action_tutorial dataset_generation_server

# Terminal3 - Action Call
ros2 action send_goal /dataset_generation custom_interfaces/action/DataGen "{x: 1, z: 1, model_name: beer}"

#goal definition
string model_name
float32 x
float32 y
float32 z
---
#result definition
string message
---
#feedback definition
string message

self.rate = self.create_rate(0.3)
self.rate.sleep()

# Terminal 1
ros2 launch src_gazebo wall_world.launch.py

# Terminal 2
ros2 run cpp_action_tutorial parking_status_server

# Terminal 3
ros2 action send_goal /src_parking custom_interfaces/action/Parking "start_flag: true" --feedback

#include "rclcpp/rclcpp.hpp"

#include "sensor_msgs/msg/laser_scan.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "custom_interfaces/action/parking.hpp"

using LaserScan = sensor_msgs::msg::LaserScan;
using Parking = custom_interfaces::action::Parking;
using GoalHandleParking = rclcpp_action::ServerGoalHandle<Parking>;

public:
  ParkingActionServer() : Node("parking_action_server") {
    using namespace std::placeholders;
    m_action_server = rclcpp_action::create_server<Parking>(
      this, "src_parking",
      std::bind(&ParkingActionServer::handle_goal, this, _1, _2),
      std::bind(&ParkingActionServer::handle_cancel, this, _1),
      std::bind(&ParkingActionServer::handle_accepted, this, _1)
    );

    m_laser_sub = this->create_subscription<LaserScan>(
      "scan", 10, std::bind(&ParkingActionServer::laser_callback, this, _1)
    );

    RCLCPP_INFO(get_logger(), "Action Ready...");
  }

rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid,
            std::shared_ptr<const Parking::Goal> goal) {
  RCLCPP_INFO(get_logger(), "Got goal request with order %s", goal->start_flag ? "true" : "false");
  (void)uuid;
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleParking> goal_handle) {
  RCLCPP_WARN(get_logger(), "Got request to cancel goal");
  (void)goal_handle;
  return rclcpp_action::CancelResponse::ACCEPT;
}

void handle_accepted(const std::shared_ptr<GoalHandleParking> goal_handle) {
  // this needs to return quickly to avoid blocking the executor, so spin up a
  // new thread
  using namespace std::placeholders;
  std::thread{std::bind(&ParkingActionServer::execute, this, _1), goal_handle}.detach();
}

class ParkingActionServer(Node):

  def __init__(self):
    super().__init__('parking_action_server')

    self.laser_sub = self.create_subscription(
      LaserScan, 'scan', self.sub_callback, 10
    )

    self.action_server = ActionServer(
      self, Parking, 'src_parking',
      goal_callback=self.goal_callback,
      cancel_callback=self.cancel_callback,
      execute_callback=self.execute_callback,
    )

while (f_obs_distance > 0.5 && is_done == false) {
  if (goal_handle->is_canceling()) {
    result->message = "Canceled";
    goal_handle->canceled(result);
    is_done = true;
    RCLCPP_WARN(get_logger(), "Goal Canceled");
    return;
  }

  feedback->distance = f_obs_distance;
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(get_logger(), "Distance from forward obstacle %f", f_obs_distance);

  loop_rate.sleep();
}

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

  rclcpp::executors::MultiThreadedExecutor executor;
  auto server_node = std::make_shared<ParkingActionServer>();

  executor.add_node(server_node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

ros2 run cpp_action_tutorial parking_status_client

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "custom_interfaces/action/parking.hpp"

using namespace std::placeholders;
using Parking = custom_interfaces::action::Parking;
using GoalHandleParking = rclcpp_action::ClientGoalHandle<Parking>;

class ParkingActionClient : public rclcpp::Node {
private:
  rclcpp_action::Client<Parking>::SharedPtr m_action_client;

public:
  ParkingActionClient() : Node("parking_action_client"){
    m_action_client = rclcpp_action::create_client<Parking>(this, "src_parking");
  }

void goal_response_callback(std::shared_future<GoalHandleParking::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");
  }
}

void feedback_callback(GoalHandleParking::SharedPtr,
  const std::shared_ptr<const Parking::Feedback> feedback){
  RCLCPP_INFO(this->get_logger(), "Received feedback: %f", feedback->distance);

  if(feedback->distance > 6.0)
    m_action_client->async_cancel_all_goals();
}

void result_callback(const GoalHandleParking::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;
  }

  RCLCPP_INFO(this->get_logger(), "Result received: %s", result.result->message.c_str());
  rclcpp::shutdown();
}