机器人编程趣味实践04-逻辑判断(if)

举报
zhangrelay 发表于 2021/07/14 22:42:41 2021/07/14
【摘要】 在上一节中,介绍了简单的运算规则。核心代码如下: 服务端: RCLCPP_INFO( g_node->get_logger(), "分别获取两个整数 %" PRId64 " + %" PRId64, request->a, request->b); response->sum = request->a + request->b; ...

在上一节中,介绍了简单的运算规则。核心代码如下:

服务端:


        RCLCPP_INFO( g_node->get_logger(),
      "分别获取两个整数 %" PRId64 " + %" PRId64, request->a, request->b);
        response->sum = request->a + request->b;
  
 

当然,+可以更换为其他运算。

客户端:


        RCLCPP_INFO(
       node->get_logger(), "结果 %" PRId64 " + %" PRId64 " = %" PRId64,
       request->a, request->b, result->sum);
  
 

这里的结果是由服务端发送给客户端的,这是一个简单的运算调用案例。

在实际运行中,由于使用中文字符,出现了乱码的情况,后续将以英文字符为主,毕竟国外开源代码几乎都是全英文的嘛。


最初,介绍了基本的消息传递,然后是运算,本节将重点关注逻辑判断,如if。

黄金分割与斐波那契数列

先上程序

服务器端:


      #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 Succeeded");
       }
        }
       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;
      }
  
 

客户端:


      #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: %" PRId32,
       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(), "%" PRId32, 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;
      }
  
 

那么这时候,就需要引入逻辑判断了,if


       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;
  
 

当需要计算量大于9000时,需要拒绝,这个数值可以修改。同样也可以发现这个程序运行时间比上一个案例中简单加减要长,并且过程也复杂。

那么中途如果不需要,就可以取消等。


       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;
        }
  
 

先看看效果^_^  分别计算6,66……

计算6阶,还是一切正常的,但是66阶,明显就不对劲啦,这时候溢出,需要终止程序,无需继续下去:

上图中的有明显问题,-1323752223

虽然运行到最后,也会出现如下:

当然,可以查看更多详细信息,使用如下命令:

  1. ros2 action list
  2. ros2 action info /fibonacci
  3. ros2 action send_goal /fibonacci example_interfaces/action/Fibonacci order:\ 20\

示例程序中有大量的判断,下一节将融合图形化界面进行扩展。


 

文章来源: zhangrelay.blog.csdn.net,作者:zhangrelay,版权归原作者所有,如需转载,请联系作者。

原文链接:zhangrelay.blog.csdn.net/article/details/116668434

【版权声明】本文为华为云社区用户转载文章,如果您发现本社区中有涉嫌抄袭的内容,欢迎发送邮件进行举报,并提供相关证据,一经查实,本社区将立刻删除涉嫌侵权内容,举报邮箱: cloudbbs@huaweicloud.com
  • 点赞
  • 收藏
  • 关注作者

评论(0

0/1000
抱歉,系统识别当前为高风险访问,暂不支持该操作

全部回复

上滑加载中

设置昵称

在此一键设置昵称,即可参与社区互动!

*长度不超过10个汉字或20个英文字符,设置后3个月内不可修改。

*长度不超过10个汉字或20个英文字符,设置后3个月内不可修改。