ROS2机器人编程简述humble-第三章-COMPUTATION GRAPH .2

news2024/11/14 0:35:41

ROS2机器人编程简述humble-第三章-PERCEPTION AND ACTUATION MODELS .1

避开障碍物计算图如何呢？

该应用程序的计算图非常简单：订阅激光主题的节点向机器人发布速度命令。

控制逻辑解释：输入的感知信息并产生控制命令（输出）。这个逻辑就是要用FSM实现的。逻辑控制将以20 Hz反复运行。执行频率取决于发布控制命令。

通常，接收信息的频率与发布信息的频率不同（差异）。必须处理这个问题。不要抱怨问题，要解决问题。如果希望软件在不同的机器人上运行，不能为机器人指定特定的主题。在例子中，它订阅的主题是/input scan，并在/output vel中发布。这些主题不存在或与模拟机器人的主题相对应。当执行它时（在部署时），将重新映射端口以将它们连接到特定机器人的真实主题。

在这里讨论一点。为什么使用重映射而不是传递主题名称作为参数？嗯，这是许多ROS2开发人员提倡的一种替代方案。当一个节点不总是具有相同的订阅者/发布者时，这个替代方案可能更方便，并且只能在配置参数的YAML文件中指定。

一个好的方法是，如果节点中的发布者和订阅者的数量是已知的，则使用通用主题名称（如本示例中使用的名称），并执行重新映射。使用通用主题名称可能更好（/cmd_vel是许多机器人的通用控制速度主题）。经验丰富的ROS2程序员将在文档中阅读它使用的主题，了解ROS2节点信息，并快速使用remap，而不是寻找要在配置中设置的正确参数文件夹。

尽管本书主要使用C++，但在本章中，将提供两种类似的实现，一种是C++实现，另一种是Python实现，每种都包含在不同的包中：br2-fsm-bumpgo-cpp和br2-fsm bumpgo-py。

案例cpp：

#include <string>
#include <memory>

#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behaviortree_cpp_v3/utils/shared_library.h"
#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"

#include "ament_index_cpp/get_package_share_directory.hpp"

#include "rclcpp/rclcpp.hpp"


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

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

  BT::BehaviorTreeFactory factory;
  BT::SharedLibrary loader;

  factory.registerFromPlugin(loader.getOSName("br2_forward_bt_node"));
  factory.registerFromPlugin(loader.getOSName("br2_back_bt_node"));
  factory.registerFromPlugin(loader.getOSName("br2_turn_bt_node"));
  factory.registerFromPlugin(loader.getOSName("br2_is_obstacle_bt_node"));

  std::string pkgpath = ament_index_cpp::get_package_share_directory("br2_bt_bumpgo");
  std::string xml_file = pkgpath + "/behavior_tree_xml/bumpgo.xml";

  auto blackboard = BT::Blackboard::create();
  blackboard->set("node", node);
  BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard);

  auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 1666, 1667);

  rclcpp::Rate rate(10);

  bool finish = false;
  while (!finish && rclcpp::ok()) {
    finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;

    rclcpp::spin_some(node);
    rate.sleep();
  }

  rclcpp::shutdown();
  return 0;
}

其中，如br2_is_obstacle_bt_node：

#include <string>
#include <utility>

#include "br2_bt_bumpgo/IsObstacle.hpp"

#include "behaviortree_cpp_v3/behavior_tree.h"

#include "sensor_msgs/msg/laser_scan.hpp"
#include "rclcpp/rclcpp.hpp"

namespace br2_bt_bumpgo
{

using namespace std::chrono_literals;
using namespace std::placeholders;

IsObstacle::IsObstacle(
  const std::string & xml_tag_name,
  const BT::NodeConfiguration & conf)
: BT::ConditionNode(xml_tag_name, conf)
{
  config().blackboard->get("node", node_);

  laser_sub_ = node_->create_subscription<sensor_msgs::msg::LaserScan>(
    "/input_scan", 100, std::bind(&IsObstacle::laser_callback, this, _1));

  last_reading_time_ = node_->now();
}

void
IsObstacle::laser_callback(sensor_msgs::msg::LaserScan::UniquePtr msg)
{
  last_scan_ = std::move(msg);
}

BT::NodeStatus
IsObstacle::tick()
{
  if (last_scan_ == nullptr) {
    return BT::NodeStatus::FAILURE;
  }

  double distance = 1.0;
  getInput("distance", distance);

  if (last_scan_->ranges[last_scan_->ranges.size() / 2] < distance) {
    return BT::NodeStatus::SUCCESS;
  } else {
    return BT::NodeStatus::FAILURE;
  }
}

}  // namespace br2_bt_bumpgo

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  factory.registerNodeType<br2_bt_bumpgo::IsObstacle>("IsObstacle");
}

后退：