使用机器人操作系统ROS 2和仿真软件Gazebo 9行动进阶实战(九)- mobot区域巡逻

316
0
2020年5月27日 16时43分

行动(action)比服务更为灵活和复杂。在给出行动具体说明之前,先简要复习一下:

主题-服务-行动:

 

使用机器人操作系统ROS 2和仿真软件Gazebo 9行动进阶实战(九)- mobot区域巡逻插图

场合

 

使用机器人操作系统ROS 2和仿真软件Gazebo 9行动进阶实战(九)- mobot区域巡逻插图(1)

具体细节

 

使用机器人操作系统ROS 2和仿真软件Gazebo 9行动进阶实战(九)- mobot区域巡逻插图(2)

服务/行动对比

 

从上面可以非常明显的看出,服务和行动的差异。

 

那么实践任务如下:

 

  • 用行动实现第8讲中,第三种服务的功能,单目标点多参数;

 

使用机器人操作系统ROS 2和仿真软件Gazebo 9行动进阶实战(九)- mobot区域巡逻插图(3)

行动实践1

 

  • 用行动实现mobot在室内环境各房间的巡逻,多目标点多参数;

 

使用机器人操作系统ROS 2和仿真软件Gazebo 9行动进阶实战(九)- mobot区域巡逻插图(4)

行动实践2

 

  • 用行动实现mobot在室外跑道的巡逻,多目标点多参数不确定。

 

使用机器人操作系统ROS 2和仿真软件Gazebo 9行动进阶实战(九)- mobot区域巡逻插图(5)

行动实践3-日光

 

使用机器人操作系统ROS 2和仿真软件Gazebo 9行动进阶实战(九)- mobot区域巡逻插图(6)

行动实践3-灯光

 

由此,需要融合OpenAI,OpenCV和BT。从机器人主题,服务过渡到行为。

 

然而行为的组合,不同状态行为的切换,构成了新的挑战,进一步学习:

 

熟练掌握相应算法进一步可扩展:

  • 物流机器人(点点轨迹,任务调度)
  • 清扫机器人(区域覆盖,协作协同)

行为(action)基础复习:

 

使用机器人操作系统ROS 2和仿真软件Gazebo 9行动进阶实战(九)- mobot区域巡逻插图(7)

斐波那契数列与黄金分割

 

行为服务器端:

 

  • 非推荐,ros1代码风格

 


#include <inttypes.h>

#include <memory>

#include "example_interfaces/action/fibonacci.hpp"

#include "rclcpp/rclcpp.hpp"

// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'

#include "rclcpp_action/rclcpp_action.hpp"

 

using Fibonacci = example_interfaces::action::Fibonacci;

using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

 

rclcpp_action::GoalResponse handle_goal(

  const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const Fibonacci::Goal> goal)

{

  RCLCPP_INFO(rclcpp::get_logger("server"), "Got goal request with order %d", goal->order);

  (void)uuid;

  // Let's reject sequences that are over 9000

  if (goal->order > 9000) {

    return rclcpp_action::GoalResponse::REJECT;

  }

  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;

}

 

rclcpp_action::CancelResponse handle_cancel(

  const std::shared_ptr<GoalHandleFibonacci> goal_handle)

{

  RCLCPP_INFO(rclcpp::get_logger("server"), "Got request to cancel goal");

  (void)goal_handle;

  return rclcpp_action::CancelResponse::ACCEPT;

}

 

void execute(

  const std::shared_ptr<GoalHandleFibonacci> goal_handle)

{

  RCLCPP_INFO(rclcpp::get_logger("server"), "Executing goal");

  rclcpp::Rate loop_rate(1);

  const auto goal = goal_handle->get_goal();

  auto feedback = std::make_shared<Fibonacci::Feedback>();

  auto & sequence = feedback->sequence;

  sequence.push_back(0);

  sequence.push_back(1);

  auto result = std::make_shared<Fibonacci::Result>();

 

  for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {

    // Check if there is a cancel request

    if (goal_handle->is_canceling()) {

      result->sequence = sequence;

      goal_handle->canceled(result);

      RCLCPP_INFO(rclcpp::get_logger("server"), "Goal Canceled");

      return;

    }

    // Update sequence

    sequence.push_back(sequence[i] + sequence[i - 1]);

    // Publish feedback

    goal_handle->publish_feedback(feedback);

    RCLCPP_INFO(rclcpp::get_logger("server"), "Publish Feedback");

 

    loop_rate.sleep();

  }

 

  // Check if goal is done

  if (rclcpp::ok()) {

    result->sequence = sequence;

    goal_handle->succeed(result);

    RCLCPP_INFO(rclcpp::get_logger("server"), "Goal Succeeded");

  }

}

 

void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)

{

  // this needs to return quickly to avoid blocking the executor, so spin up a new thread

  std::thread{execute, goal_handle}.detach();

}

 

int main(int argc, char ** argv)

{

  rclcpp::init(argc, argv);

  auto node = rclcpp::Node::make_shared("minimal_action_server");

 

  // Create an action server with three callbacks

  //   'handle_goal' and 'handle_cancel' are called by the Executor (rclcpp::spin)

  //   'execute' is called whenever 'handle_goal' returns by accepting a goal

  //    Calls to 'execute' are made in an available thread from a pool of four.

  auto action_server = rclcpp_action::create_server<Fibonacci>(

    node,

    "fibonacci",

    handle_goal,

    handle_cancel,

    handle_accepted);

 

  rclcpp::spin(node);

 

  rclcpp::shutdown();

  return 0;

}

 

  • 推荐,ros2新风格

 


#include <inttypes.h>

#include <memory>

#include "example_interfaces/action/fibonacci.hpp"

#include "rclcpp/rclcpp.hpp"

// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'

#include "rclcpp_action/rclcpp_action.hpp"

 

class MinimalActionServer : public rclcpp::Node

{

public:

  using Fibonacci = example_interfaces::action::Fibonacci;

  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

 

  explicit MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())

  : Node("minimal_action_server", options)

  {

    using namespace std::placeholders;

 

    this->action_server_ = rclcpp_action::create_server<Fibonacci>(

      this->get_node_base_interface(),

      this->get_node_clock_interface(),

      this->get_node_logging_interface(),

      this->get_node_waitables_interface(),

      "fibonacci",

      std::bind(&MinimalActionServer::handle_goal, this, _1, _2),

      std::bind(&MinimalActionServer::handle_cancel, this, _1),

      std::bind(&MinimalActionServer::handle_accepted, this, _1));

  }

 

private:

  rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;

 

  rclcpp_action::GoalResponse handle_goal(

    const rclcpp_action::GoalUUID & uuid,

    std::shared_ptr<const Fibonacci::Goal> goal)

  {

    RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);

    (void)uuid;

    // Let's reject sequences that are over 9000

    if (goal->order > 9000) {

      return rclcpp_action::GoalResponse::REJECT;

    }

    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;

  }

 

  rclcpp_action::CancelResponse handle_cancel(

    const std::shared_ptr<GoalHandleFibonacci> goal_handle)

  {

    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");

    (void)goal_handle;

    return rclcpp_action::CancelResponse::ACCEPT;

  }

 

  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)

  {

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

    rclcpp::Rate loop_rate(1);

    const auto goal = goal_handle->get_goal();

    auto feedback = std::make_shared<Fibonacci::Feedback>();

    auto & sequence = feedback->sequence;

    sequence.push_back(0);

    sequence.push_back(1);

    auto result = std::make_shared<Fibonacci::Result>();

 

    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {

      // Check if there is a cancel request

      if (goal_handle->is_canceling()) {

        result->sequence = sequence;

        goal_handle->canceled(result);

        RCLCPP_INFO(this->get_logger(), "Goal Canceled");

        return;

      }

      // Update sequence

      sequence.push_back(sequence[i] + sequence[i - 1]);

      // Publish feedback

      goal_handle->publish_feedback(feedback);

      RCLCPP_INFO(this->get_logger(), "Publish Feedback");

 

      loop_rate.sleep();

    }

 

    // Check if goal is done

    if (rclcpp::ok()) {

      result->sequence = sequence;

      goal_handle->succeed(result);

      RCLCPP_INFO(this->get_logger(), "Goal Suceeded");

    }

  }

 

  void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)

  {

    using namespace std::placeholders;

    // this needs to return quickly to avoid blocking the executor, so spin up a new thread

    std::thread{std::bind(&MinimalActionServer::execute, this, _1), goal_handle}.detach();

  }

};  // class MinimalActionServer

 

int main(int argc, char ** argv)

{

  rclcpp::init(argc, argv);

 

  auto action_server = std::make_shared<MinimalActionServer>();

 

  rclcpp::spin(action_server);

 

  rclcpp::shutdown();

  return 0;

}

 

行为客户端:

 

  • 非推荐,ros1风格

 


#include <inttypes.h>

#include <memory>

#include "example_interfaces/action/fibonacci.hpp"

#include "rclcpp/rclcpp.hpp"

// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'

#include "rclcpp_action/rclcpp_action.hpp"

 

using Fibonacci = example_interfaces::action::Fibonacci;

 

 

int main(int argc, char ** argv)

{

  rclcpp::init(argc, argv);

  auto node = rclcpp::Node::make_shared("minimal_action_client");

  auto action_client = rclcpp_action::create_client<Fibonacci>(node, "fibonacci");

 

  if (!action_client->wait_for_action_server(std::chrono::seconds(20))) {

    RCLCPP_ERROR(node->get_logger(), "Action server not available after waiting");

    return 1;

  }

 

  // Populate a goal

  auto goal_msg = Fibonacci::Goal();

  goal_msg.order = 10;

 

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

  // Ask server to achieve some goal and wait until it's accepted

  auto goal_handle_future = action_client->async_send_goal(goal_msg);

  if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=

    rclcpp::executor::FutureReturnCode::SUCCESS)

  {

    RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");

    return 1;

  }

 

  rclcpp_action::ClientGoalHandle<Fibonacci>::SharedPtr goal_handle = goal_handle_future.get();

  if (!goal_handle) {

    RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");

    return 1;

  }

 

  // Wait for the server to be done with the goal

  auto result_future = goal_handle->async_result();

 

  RCLCPP_INFO(node->get_logger(), "Waiting for result");

  if (rclcpp::spin_until_future_complete(node, result_future) !=

    rclcpp::executor::FutureReturnCode::SUCCESS)

  {

    RCLCPP_ERROR(node->get_logger(), "get result call failed :(");

    return 1;

  }

 

  rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult wrapped_result = result_future.get();

 

  switch (wrapped_result.code) {

    case rclcpp_action::ResultCode::SUCCEEDED:

      break;

    case rclcpp_action::ResultCode::ABORTED:

      RCLCPP_ERROR(node->get_logger(), "Goal was aborted");

      return 1;

    case rclcpp_action::ResultCode::CANCELED:

      RCLCPP_ERROR(node->get_logger(), "Goal was canceled");

      return 1;

    default:

      RCLCPP_ERROR(node->get_logger(), "Unknown result code");

      return 1;

  }

 

  RCLCPP_INFO(node->get_logger(), "result received");

  for (auto number : wrapped_result.result->sequence) {

    RCLCPP_INFO(node->get_logger(), "%" PRId64, number);

  }

 

  • 推荐:ros2风格

 


#include <inttypes.h>

#include <memory>

#include <string>

#include <iostream>

#include "example_interfaces/action/fibonacci.hpp"

#include "rclcpp/rclcpp.hpp"

// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'

#include "rclcpp_action/rclcpp_action.hpp"

 

class MinimalActionClient : public rclcpp::Node

{

public:

  using Fibonacci = example_interfaces::action::Fibonacci;

  using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;

 

  explicit MinimalActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())

  : Node("minimal_action_client", node_options), goal_done_(false)

  {

    this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(

      this->get_node_base_interface(),

      this->get_node_graph_interface(),

      this->get_node_logging_interface(),

      this->get_node_waitables_interface(),

      "fibonacci");

 

    this->timer_ = this->create_wall_timer(

      std::chrono::milliseconds(500),

      std::bind(&MinimalActionClient::send_goal, this));

  }

 

  bool is_goal_done() const

  {

    return this->goal_done_;

  }

 

  void send_goal()

  {

    using namespace std::placeholders;

 

    this->timer_->cancel();

 

    this->goal_done_ = false;

 

    if (!this->client_ptr_) {

      RCLCPP_ERROR(this->get_logger(), "Action client not initialized");

    }

 

    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {

      RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");

      this->goal_done_ = true;

      return;

    }

 

    auto goal_msg = Fibonacci::Goal();

    goal_msg.order = 10;

 

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

 

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

    send_goal_options.goal_response_callback =

      std::bind(&MinimalActionClient::goal_response_callback, this, _1);

    send_goal_options.feedback_callback =

      std::bind(&MinimalActionClient::feedback_callback, this, _1, _2);

    send_goal_options.result_callback =

      std::bind(&MinimalActionClient::result_callback, this, _1);

    auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options);

  }

 

private:

  rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;

  rclcpp::TimerBase::SharedPtr timer_;

  bool goal_done_;

 

  void goal_response_callback(std::shared_future<GoalHandleFibonacci::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(

    GoalHandleFibonacci::SharedPtr,

    const std::shared_ptr<const Fibonacci::Feedback> feedback)

  {

    RCLCPP_INFO(

      this->get_logger(),

      "Next number in sequence received: %" PRId64,

      feedback->sequence.back());

  }

 

  void result_callback(const GoalHandleFibonacci::WrappedResult & result)

  {

    this->goal_done_ = true;

    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");

    for (auto number : result.result->sequence) {

      RCLCPP_INFO(this->get_logger(), "%" PRId64, number);

    }

  }

};  // class MinimalActionClient

 

int main(int argc, char ** argv)

{

  rclcpp::init(argc, argv);

  auto action_client = std::make_shared<MinimalActionClient>();

 

  while (!action_client->is_goal_done()) {

    rclcpp::spin_some(action_client);

  }

 

  rclcpp::shutdown();

  return 0;

}

 

发表评论

后才能评论