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：
代码语言：javascript复制#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：
代码语言：javascript复制#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");
}后退：
代码语言：javascript复制vel_msgs.linear.x = -0.3;前进：
代码语言：javascript复制vel_msgs.linear.x = 0.3;转向：
代码语言：javascript复制vel_msgs.angular.z = 0.5;
