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

后退：

vel_msgs.linear.x = -0.3;

前进：

vel_msgs.linear.x = 0.3;

转向：

vel_msgs.angular.z = 0.5;