简述本章项目，参考如下：

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

流程图绘制，参考如下：

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

然后，在3.3和3.4分别用C++和Python编程实现

（br2_fsm_bumpgo_cpp/ + br2_fsm_bumpgo_py/）。

br2 fsm bumpgo cpp功能包组成如下：

.
├── CMakeLists.txt
├── include
│   └── br2_fsm_bumpgo_cpp
│       └── BumpGoNode.hpp
├── launch
│   └── bump_and_go.launch.py
├── package.xml
└── src
    ├── br2_fsm_bumpgo_cpp
    │   └── BumpGoNode.cpp
    └── bumpgo_main.cpp

5 directories, 6 files

头文件：BumpGoNode.hpp
功能实现：BumpGoNode.cpp
主文件：bumpgo_main.cpp
启动文件：bump_and_go.launch.py

输入Subscription和输出Publisher：

  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;

回调支持：

void scan_callback(const sensor_msgs::msg::LaserScan & msg);

void scan_callback(sensor_msgs::msg::LaserScan::UniquePtr msg);

void scan_callback(sensor_msgs::msg::LaserScan::SharedConstPtr msg);

void scan_callback(const sensor_msgs::msg::LaserScan::SharedConstPtr & msg);

void scan_callback(sensor_msgs::msg::LaserScan::SharedPtr msg);

依据需要选择，如：

  void scan_callback(sensor_msgs::msg::LaserScan::UniquePtr msg);
  void control_cycle();

更具体一些：

void
BumpGoNode::scan_callback(sensor_msgs::msg::LaserScan::UniquePtr msg)
{
  last_scan_ = std::move(msg);
}

void
BumpGoNode::control_cycle()
{
  // Do nothing until the first sensor read
  if (last_scan_ == nullptr) {
    return;
  }

  geometry_msgs::msg::Twist out_vel;

  switch (state_) {
    case FORWARD:
      out_vel.linear.x = SPEED_LINEAR;

      if (check_forward_2_stop()) {
        go_state(STOP);
      }

      if (check_forward_2_back()) {
        go_state(BACK);
      }
      break;
    case BACK:
      out_vel.linear.x = -SPEED_LINEAR;

      if (check_back_2_turn()) {
        go_state(TURN);
      }
      break;
    case TURN:
      out_vel.angular.z = SPEED_ANGULAR;

      if (check_turn_2_forward()) {
        go_state(FORWARD);
      }

      break;
    case STOP:
      if (check_stop_2_forward()) {
        go_state(FORWARD);
      }
      break;
  }

  vel_pub_->publish(out_vel);
}

里面可以看到四种状态切换：

  static const int FORWARD = 0;
  static const int BACK = 1;
  static const int TURN = 2;
  static const int STOP = 3;

状态对应和切换：

void
BumpGoNode::go_state(int new_state)
{
  state_ = new_state;
  state_ts_ = now();
}

bool
BumpGoNode::check_forward_2_back()
{
  // going forward when deteting an obstacle
  // at 0.5 meters with the front laser read
  size_t pos = last_scan_->ranges.size() / 2;
  return last_scan_->ranges[pos] < OBSTACLE_DISTANCE;
}

bool
BumpGoNode::check_forward_2_stop()
{
  // Stop if no sensor readings for 1 second
  auto elapsed = now() - rclcpp::Time(last_scan_->header.stamp);
  return elapsed > SCAN_TIMEOUT;
}

bool
BumpGoNode::check_stop_2_forward()
{
  // Going forward if sensor readings are available
  // again
  auto elapsed = now() - rclcpp::Time(last_scan_->header.stamp);
  return elapsed < SCAN_TIMEOUT;
}

bool
BumpGoNode::check_back_2_turn()
{
  // Going back for 2 seconds
  return (now() - state_ts_) > BACKING_TIME;
}

bool
BumpGoNode::check_turn_2_forward()
{
  // Turning for 2 seconds
  return (now() - state_ts_) > TURNING_TIME;
}

例如：

bool
BumpGoNode::check_forward_2_back()
{
  // going forward when deteting an obstacle
  // at 0.5 meters with the front laser read
  size_t pos = last_scan_->ranges.size() / 2;
  return last_scan_->ranges[pos] < OBSTACLE_DISTANCE;
}

检查机器人前方是否有障碍物。

主程序main：

#include <memory>

#include "br2_fsm_bumpgo_cpp/BumpGoNode.hpp"
#include "rclcpp/rclcpp.hpp"

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

  auto bumpgo_node = std::make_shared<br2_fsm_bumpgo_cpp::BumpGoNode>();
  rclcpp::spin(bumpgo_node);

  rclcpp::shutdown();

  return 0;
}

执行

启动仿真，参考：

ROS2机器人编程简述humble-第二章-SIMULATED ROBOT SETUP .4

ros2 launch br2_tiago sim.launch.py

加载避障程序：

ros2 run br2_fsm_bumpgo_cpp bumpgo --ros-args -r output_vel:=/nav_vel -r input_scan:=/scan_raw -p use_sim_time:=true

或者使用启动文件：

ros2 launch br2_fsm_bumpgo_cpp bump_and_go.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():

    bumpgo_cmd = Node(package='br2_fsm_bumpgo_cpp',
                      executable='bumpgo',
                      output='screen',
                      parameters=[{
                        'use_sim_time': True
                      }],
                      remappings=[
                        ('input_scan', '/scan_raw'),
                        ('output_vel', '/nav_vel')
                      ])

    ld = LaunchDescription()
    ld.add_action(bumpgo_cmd)

    return ld