-BUMP AND GO IN C++ .3

Posted 2023-02-19 zhangrelay

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

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