简述本章项目,参考如下:
流程图绘制,参考如下:
然后,在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 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