思岚激光雷达rplidar从ROS 1到ROS 2的移植
ROS 2下navigation 2 stack的构建
订阅rviz2的导航目标位置消息“/goal_pose”
打断behavior tree的异步动作节点，并执行其他节点动作
ROS 1下苇航智能的NaviBOT A0通过底盘驱动代码，利用actionLib的cancelGoal和setGoal实现导航目标在完成旋转运动后的内部流转。迁移到ROS 2的初期，NaviBOT A0依然沿用了ROS 1下的实现机制，但碰到了很多实际问题，运行效果相比ROS 1下要差，且需要底盘驱动代码自行维护旋转和导航之间的状态机，造成了代码的可维护性较差。ROS 2相比ROS 1其中的一个较大变化就是behavior tree状态控制，通过bt能够实现状态逻辑的便利扩展和修改，而不需要修改原有代码。因此，转而考虑利用bt机制实现对导航目标的原地旋转逻辑
ROS 2的behavior tree通过库BehaviorTree.CPP实现，其机制和概念是本文后续实现的基础，请参考官网链接：
https://www.behaviortree.dev/
navigation 2导航任务的逻辑是通过package “nav2_bt_navigator”控制和维护的，包括路径计算，脱困恢复等，该package基于behavior tree实现对导航任务的逻辑状态维护，详细内容参看官网链接：
https://navigation.ros.org/configuration/packages/configuring-bt-navigator.html
https://index.ros.org/p/nav2_bt_navigator/
通过查看其默认调用的bt，“navigate_w_replanning_and_recovery.xml”，可以发现在顺序执行的control node “NavigateWithReplanning”节点下的两个action nodes即为接收导航目标，计算路径，并开始遵循路径运行的导航过程，如下图：
由于该过程由顺序执行的control node控制，即PipelineSequence，所以完全可以在路径计算和遵循路径两个节点中间加入原地旋转调整导航目标航向角的过程，如下图：
TIP：利用behavior tree的可视化工具Groot可以实现对bt的预览和编辑。Groot的使用超出本内容范围，不在此累述，详细内容可以参看官网链接：
https://github.com/BehaviorTree/Groot
在nav2_bt_navigator的behavior tree中，所有的action node即上图中蓝色节点，为该package的plugin，因此，希望能实现面向导航目标的原地旋转功能，即是创建一个能够被nav2_bt_navigator加载的自定义plugin
bt plugin本质是一个action server的client，运行navigation stack后，可以在终端输入命令“ros2 action list”罗列所有action server提供的action服务，再通过命令“ros2 action info /”查看该服务的服务端，如下图中服务“/spin”的服务端由“/recoveries_server”提供：
上图中服务“/spinGO”是基于plugin “spin”构建的原地旋转的action plugin，该服务只是简单的复制了spin的代码，只修改了plugin的ID为“spinGO”，运行后能看到该服务，但却没有服务端，即“Action servers: 0”，这是因为服务端“recoveries_server”只有三个注册服务，即“backup”，“wait”和“spin”，因此，自定义的bt plugin首先需要注册到服务端
recoveries_server是一个现成的服务端，自定义的plugin可以选择注册到该服务端，即参照“backup”，“wait”和“spin”三个plugins的实现方式，为recoveries_server创建一个自定义的plugin “spinGO”，这是最为便利和简单的方式。但是，从模块逻辑结构上看，原地旋转到导航目标点航向角的动作，不应该属于脱困恢复的逻辑。从更利于后续维护和拓展的角度出发，因此考虑创建一个类似于recoveries_server的服务端，同时具备动态加载plugin的能力，通过加载自定义的plugin模块来实现控制原地旋转的功能
参考nav2_recoveries server，该server用于机器人的脱困恢复，有三个plugins：Backup，Spin，Wait组成。Nav2_recoveries server采用了managed node的方式构建，该方式能够很好的适应launch的nodes组合运行，自动解决nodes间的运行依赖及顺序，manage nodes的详细说明，可以参考官网链接：
https://design.ros2.org/articles/node_lifecycle.html
如果采用简单省力的方式，是可以基于nav2_recoveries server构建自己的spin to orientate the goal插件，然后集成到nav2_recoveries server。苇航智能从功能逻辑和后期维护角度触发，选择构建和nav2_recoveries类似的action server，这样功能逻辑更加清晰，同时还能支持后期扩展的action plugin
由于过程中涉及到的内容相对较多，先将所有步骤完整罗列，再对关键环节做进一步的说明
在终端输入命令：
cd ~/dev_ws/src
ros2 pkg create --build-type ament_cmake motion_ctrl_btnav_action --dependencies rclcpp
创建完毕后，修改package.xml中版本、维护、版权等信息
本说明的目标是构建使机器人在规划路径完成后，能够依据所规划的路径和当前航向角，进行原地旋转以调整到和规划路径一致的航向角上，因此，所构建的plugin的动作即是旋转。可以看到在nav2_msgs中有一个spin的action，该action也被nav2_recoveries作为脱困时的旋转动作。查看“Spin.action”，可以看到goal为目标yaw，结果反馈为旋转过程的耗时，过程反馈为已经旋转的角度，因此，该action完全可以满足即将构建的旋转动作，自定义的旋转plugin可以基于该action，如下图：
首先，需要创建plugin的基类
在“include/<package>/”目录下，建立文件“btnavAction.hpp”，添加如下代码：
/******************************************************************
base class of action plugin for navigation bt actions
Features:
- base class without ROS action
- base class with ROS action
- xxx
Written by Xinjue Zou, xinjue.zou.whi@gmail.com
GNU General Public License, check LICENSE for more information.
All text above must be included in any redistribution.
Changelog:
2020-10-30: Initial version
2020-xx-xx: xxx
******************************************************************/
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/create_timer_ros.h"
#include "geometry_msgs/msg/twist.hpp"
#include "nav2_util/simple_action_server.hpp"
#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
namespace btnav_actions
{
/**
* @class action base
* @brief Abstract interface for recoveries to adhere to with pluginlib
*/
class btnavActionBase
{
public:
using Ptr = std::shared_ptr<btnavActionBase>;
/**
* @brief Virtual destructor
*/
virtual ~btnavActionBase() {}
/**
* @param parent pointer to user's node
* @param name The name of this planner
* @param tf A pointer to a TF buffer
* @param costmap_ros A pointer to the costmap
*/
virtual void configure(
const rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker) = 0;
/**
* @brief Method to cleanup resources used on shutdown.
*/
virtual void cleanup() = 0;
/**
* @brief Method to active action and any threads involved in execution.
*/
virtual void activate() = 0;
/**
* @brief Method to deactive action and any threads involved in execution.
*/
virtual void deactivate() = 0;
/**
* @brief Method Execute action behavior
* @param name The name of this planner
* @return true if successful, false is failed to execute fully
*/
// TODO(stevemacenski) evaluate design for recoveries to not host
// their own servers and utilize a action server exposed action.
// virtual bool executeAction() = 0;
};
enum class Status : int8_t
{
SUCCEEDED = 1,
FAILED = 2,
RUNNING = 3,
};
using namespace std::chrono_literals; //NOLINT
template<typename ActionT>
class btnavAction : public btnavActionBase
{
public:
using ActionServer = nav2_util::SimpleActionServer<ActionT, rclcpp_lifecycle::LifecycleNode>;
btnavAction()
: action_server_(nullptr),
cycle_frequency_(10.0),
enabled_(false)
{
}
virtual ~btnavAction()
{
}
// Derived classes can override this method to catch the command and perform some checks
// before getting into the main loop. The method will only be called
// once and should return SUCCEEDED otherwise behavior will return FAILED.
virtual Status onRun(const std::shared_ptr<const typename ActionT::Goal> command) = 0;
// This is the method derived classes should mainly implement
// and will be called cyclically while it returns RUNNING.
// Implement the behavior such that it runs some unit of work on each call
// and provides a status. The action will finish once SUCCEEDED is returned
// It's up to the derived class to define the final commanded velocity.
virtual Status onCycleUpdate() = 0;
// an opportunity for derived classes to do something on configuration
// if they chose
virtual void onConfigure()
{
}
// an opportunity for derived classes to do something on cleanup
// if they chose
virtual void onCleanup()
{
}
void configure(
const rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker) override
{
RCLCPP_INFO(parent->get_logger(), "Configuring %s", name.c_str());
node_ = parent;
action_name_ = name;
tf_ = tf;
node_->get_parameter("cycle_frequency", cycle_frequency_);
node_->get_parameter("global_frame", global_frame_);
node_->get_parameter("robot_base_frame", robot_base_frame_);
node_->get_parameter("transform_tolerance", transform_tolerance_);
action_server_ = std::make_shared<ActionServer>(
node_, action_name_,
std::bind(&btnavAction::execute, this));
collision_checker_ = collision_checker;
vel_pub_ = node_->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
onConfigure();
}
void cleanup() override
{
action_server_.reset();
vel_pub_.reset();
onCleanup();
}
void activate() override
{
RCLCPP_INFO(node_->get_logger(), "Activating %s", action_name_.c_str());
vel_pub_->on_activate();
enabled_ = true;
}
void deactivate() override
{
vel_pub_->on_deactivate();
enabled_ = false;
}
protected:
rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
std::string action_name_;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
std::shared_ptr<ActionServer> action_server_;
std::shared_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
double cycle_frequency_;
double enabled_;
std::string global_frame_;
std::string robot_base_frame_;
double transform_tolerance_;
// Clock
rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
void execute()
{
RCLCPP_INFO(node_->get_logger(), "Attempting %s", action_name_.c_str());
if (!enabled_)
{
RCLCPP_WARN(node_->get_logger(), "Called while inactive, ignoring request.");
return;
}
if (onRun(action_server_->get_current_goal()) != Status::SUCCEEDED)
{
RCLCPP_INFO(node_->get_logger(), "Initial checks failed for %s", action_name_.c_str());
action_server_->terminate_current();
return;
}
// Log a message every second
auto timer = node_->create_wall_timer(
1s,
[&]() {RCLCPP_INFO(node_->get_logger(), "%s running...", action_name_.c_str());});
auto start_time = steady_clock_.now();
// Initialize the ActionT result
auto result = std::make_shared<typename ActionT::Result>();
rclcpp::Rate loop_rate(cycle_frequency_);
while (rclcpp::ok())
{
if (action_server_->is_cancel_requested())
{
RCLCPP_INFO(node_->get_logger(), "Canceling %s", action_name_.c_str());
stopRobot();
result->total_elapsed_time = steady_clock_.now() - start_time;
action_server_->terminate_all(result);
return;
}
if (action_server_->is_preempt_requested())
{
RCLCPP_ERROR(
node_->get_logger(), "Received a preemption request for %s,"
" however feature is currently not implemented. Aborting and stopping.",
action_name_.c_str());
stopRobot();
result->total_elapsed_time = steady_clock_.now() - start_time;
action_server_->terminate_current(result);
return;
}
switch (onCycleUpdate())
{
case Status::SUCCEEDED:
RCLCPP_INFO(node_->get_logger(), "%s completed successfully", action_name_.c_str());
result->total_elapsed_time = steady_clock_.now() - start_time;
action_server_->succeeded_current(result);
return;
case Status::FAILED:
RCLCPP_WARN(node_->get_logger(), "%s failed", action_name_.c_str());
result->total_elapsed_time = steady_clock_.now() - start_time;
action_server_->terminate_current(result);
return;
case Status::RUNNING:
default:
loop_rate.sleep();
break;
}
}
}
void stopRobot()
{
auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
cmd_vel->linear.x = 0.0;
cmd_vel->linear.y = 0.0;
cmd_vel->angular.z = 0.0;
vel_pub_->publish(std::move(cmd_vel));
}
};
} // namespace btnav_actions
其次，创建具体的action plugin
在plugins中创建文件“spinGoalOrientation.hpp”，“spinGoalOrientation.cpp”，添加如下代码：
HPP
/******************************************************************
spin to target yaw action plugin for navigation bt actions
Features:
- spin to target yaw with collision check
- publish cmd_vel message
- xxx
Written by Xinjue Zou, xinjue.zou.whi@gmail.com
GNU General Public License, check LICENSE for more information.
All text above must be included in any redistribution.
Changelog:
2020-10-30: Initial version
2020-xx-xx: xxx
******************************************************************/
#pragma once
#include "motion_ctrl_btnav_action/btnavAction.hpp"
#include "nav2_msgs/action/spin.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
namespace btnav_action_server
{
using SpinGOAction = nav2_msgs::action::Spin;
class SpinGO : public btnav_actions::btnavAction<SpinGOAction>
{
public:
SpinGO();
~SpinGO();
btnav_actions::Status onRun(const std::shared_ptr<const SpinGOAction::Goal> command) override;
void onConfigure() override;
btnav_actions::Status onCycleUpdate() override;
protected:
bool isCollisionFree(
const double& distance,
geometry_msgs::msg::Twist* cmd_vel,
geometry_msgs::msg::Pose2D& pose2d);
SpinGOAction::Feedback::SharedPtr feedback_;
double rotate_in_place_vel_;
double cmd_yaw_;
double prev_yaw_;
double relative_yaw_;
double simulate_ahead_time_;
};
} // namespace btnav_action_server
CPP
/******************************************************************
spin to target yaw action plugin for navigation bt actions
Features:
- spin to target yaw with collision check
- publish cmd_vel message
- xxx
Written by Xinjue Zou, xinjue.zou.whi@gmail.com
GNU General Public License, check LICENSE for more information.
All text above must be included in any redistribution.
******************************************************************/
#include "nav2_util/node_utils.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2/utils.h"
#include "spinGoalOrientation.hpp"
using namespace std::chrono_literals;
namespace btnav_action_server
{
SpinGO::SpinGO()
: btnavAction<SpinGOAction>(),
feedback_(std::make_shared<SpinGOAction::Feedback>()),
prev_yaw_(0.0)
{
}
SpinGO::~SpinGO()
{
}
void SpinGO::onConfigure()
{
nav2_util::declare_parameter_if_not_declared(
node_,
"simulate_ahead_time", rclcpp::ParameterValue(2.0));
node_->get_parameter("simulate_ahead_time", simulate_ahead_time_);
nav2_util::declare_parameter_if_not_declared(
node_,
"rotate_in_place_vel", rclcpp::ParameterValue(2.0));
node_->get_parameter("rotate_in_place_vel", rotate_in_place_vel_);
}
btnav_actions::Status SpinGO::onRun(const std::shared_ptr<const SpinGOAction::Goal> command)
{
geometry_msgs::msg::PoseStamped current_pose;
if (!nav2_util::getCurrentPose(
current_pose, *tf_, global_frame_, robot_base_frame_,
transform_tolerance_))
{
RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
return btnav_actions::Status::FAILED;
}
prev_yaw_ = tf2::getYaw(current_pose.pose.orientation);
relative_yaw_ = 0.0;
cmd_yaw_ = command->target_yaw;
RCLCPP_INFO(
node_->get_logger(), "Turning %0.2f for spin recovery.",
cmd_yaw_);
return btnav_actions::Status::SUCCEEDED;
}
btnav_actions::Status SpinGO::onCycleUpdate()
{
geometry_msgs::msg::PoseStamped current_pose;
if (!nav2_util::getCurrentPose(
current_pose, *tf_, global_frame_, robot_base_frame_,
transform_tolerance_))
{
RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
return btnav_actions::Status::FAILED;
}
const double current_yaw = tf2::getYaw(current_pose.pose.orientation);
double delta_yaw = current_yaw - prev_yaw_;
if (abs(delta_yaw) > M_PI) {
delta_yaw = copysign(2 * M_PI - abs(delta_yaw), prev_yaw_);
}
relative_yaw_ += delta_yaw;
prev_yaw_ = current_yaw;
feedback_->angular_distance_traveled = relative_yaw_;
action_server_->publish_feedback(feedback_);
double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_);
if (remaining_yaw <= 0)
{
stopRobot();
return btnav_actions::Status::SUCCEEDED;
}
auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
cmd_vel->angular.z = cmd_yaw_;
geometry_msgs::msg::Pose2D pose2d;
pose2d.x = current_pose.pose.position.x;
pose2d.y = current_pose.pose.position.y;
pose2d.theta = tf2::getYaw(current_pose.pose.orientation);
if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d))
{
stopRobot();
RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting Spin");
return btnav_actions::Status::SUCCEEDED;
}
vel_pub_->publish(std::move(cmd_vel));
return btnav_actions::Status::RUNNING;
}
bool SpinGO::isCollisionFree(
const double& relative_yaw,
geometry_msgs::msg::Twist* cmd_vel,
geometry_msgs::msg::Pose2D& pose2d)
{
// Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments
int cycle_count = 0;
double sim_position_change;
const int max_cycle_count = static_cast<int>(cycle_frequency_ * simulate_ahead_time_);
while (cycle_count < max_cycle_count)
{
sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_);
pose2d.theta += sim_position_change;
cycle_count++;
if (abs(relative_yaw) - abs(sim_position_change) <= 0.)
{
break;
}
if (!collision_checker_->isCollisionFree(pose2d))
{
return false;
}
}
return true;
}
} // namespace btnav_action_server
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(btnav_action_server::SpinGO, btnav_actions::btnavActionBase)
分别在package的“src/”和“include/<package>/”目录下创建文件“action_server.cpp”，“action_server.hpp”，添加如下代码：
HPP
/******************************************************************
lifecycleNode action server for navigation bt actions
Features:
- lifecycleNode action server
- dynamic and configurable custom action plugins
- xxx
Written by Xinjue Zou, xinjue.zou.whi@gmail.com
GNU General Public License, check LICENSE for more information.
All text above must be included in any redistribution.
Changelog:
2020-10-30: Initial version
2020-xx-xx: xxx
******************************************************************/
#pragma once
#include "nav2_util/lifecycle_node.hpp"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "btnavAction.hpp"
namespace btnav_action_server
{
class actionServer : public nav2_util::LifecycleNode
{
public:
actionServer();
~actionServer();
void loadPlugins();
protected:
// Implement the lifecycle interface
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State& state) override;
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override;
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& state) override;
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& state) override;
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& state) override;
std::shared_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
// Plugins
std::vector<pluginlib::UniquePtr<btnav_actions::btnavActionBase>> actions_;
pluginlib::ClassLoader<btnav_actions::btnavActionBase> plugin_loader_;
std::vector<std::string> default_ids_;
std::vector<std::string> default_types_;
std::vector<std::string> action_ids_;
std::vector<std::string> action_types_;
// Utilities
std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
double transform_tolerance_;
};
} // namespace btnav_action_server
CPP
/******************************************************************
lifecycleNode action server for navigation bt actions
Features:
- lifecycleNode action server
- dynamic and configurable custom action plugins
- xxx
Written by Xinjue Zou, xinjue.zou.whi@gmail.com
GNU General Public License, check LICENSE for more information.
All text above must be included in any redistribution.
******************************************************************/
#include "nav2_util/node_utils.hpp"
#include "motion_ctrl_btnav_action/action_server.hpp"
namespace btnav_action_server
{
actionServer::actionServer()
: LifecycleNode("btnav_action_server", "", true),
plugin_loader_("motion_ctrl_btnav_action", "btnav_actions::btnavActionBase"),
default_ids_{"SpinGO"},
default_types_{"btnav_action_server/SpinGO"}
{
declare_parameter(
"costmap_topic",
rclcpp::ParameterValue(std::string("local_costmap/costmap_raw")));
declare_parameter(
"footprint_topic",
rclcpp::ParameterValue(std::string("local_costmap/published_footprint")));
declare_parameter("cycle_frequency", rclcpp::ParameterValue(10.0));
declare_parameter("btnav_action_plugins", default_ids_);
declare_parameter(
"global_frame",
rclcpp::ParameterValue(std::string("odom")));
declare_parameter(
"robot_base_frame",
rclcpp::ParameterValue(std::string("base_link")));
declare_parameter(
"transform_tolerance",
rclcpp::ParameterValue(0.1));
}
actionServer::~actionServer()
{
}
nav2_util::CallbackReturn actionServer::on_configure(const rclcpp_lifecycle::State& /*state*/)
{
RCLCPP_INFO(get_logger(), "Configuring");
tf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
get_node_base_interface(),
get_node_timers_interface());
tf_->setCreateTimerInterface(timer_interface);
transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
std::string costmap_topic, footprint_topic;
get_parameter("costmap_topic", costmap_topic);
get_parameter("footprint_topic", footprint_topic);
get_parameter("transform_tolerance", transform_tolerance_);
costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(
shared_from_this(), costmap_topic);
footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
shared_from_this(), footprint_topic, 1.0);
std::string global_frame, robot_base_frame;
get_parameter("global_frame", global_frame);
get_parameter("robot_base_frame", robot_base_frame);
collision_checker_ = std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
*costmap_sub_, *footprint_sub_, *tf_, this->get_name(),
global_frame, robot_base_frame, transform_tolerance_);
get_parameter("btnav_action_plugins", action_ids_);
if (action_ids_ == default_ids_)
{
for (size_t i = 0; i < default_ids_.size(); ++i)
{
declare_parameter(default_ids_[i] + ".plugin", default_types_[i]);
}
}
action_types_.resize(action_ids_.size());
loadPlugins();
return nav2_util::CallbackReturn::SUCCESS;
}
void actionServer::loadPlugins()
{
auto node = shared_from_this();
for (size_t i = 0; i != action_ids_.size(); i++)
{
action_types_[i] = nav2_util::get_plugin_type_param(node, action_ids_[i]);
try
{
RCLCPP_INFO(
get_logger(), "Creating btnav action plugin %s of type %s",
action_ids_[i].c_str(), action_types_[i].c_str());
actions_.push_back(plugin_loader_.createUniqueInstance(action_types_[i]));
actions_.back()->configure(node, action_ids_[i], tf_, collision_checker_);
}
catch (const pluginlib::PluginlibException & ex)
{
RCLCPP_FATAL(
get_logger(), "Failed to create btnav action %s of type %s."
" Exception: %s", action_ids_[i].c_str(), action_types_[i].c_str(),
ex.what());
exit(-1);
}
}
}
nav2_util::CallbackReturn actionServer::on_activate(const rclcpp_lifecycle::State& /*state*/)
{
RCLCPP_INFO(get_logger(), "Activating");
std::vector<pluginlib::UniquePtr<btnav_actions::btnavActionBase>>::iterator iter;
for (iter = actions_.begin(); iter != actions_.end(); ++iter)
{
(*iter)->activate();
}
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn actionServer::on_deactivate(const rclcpp_lifecycle::State& /*state*/)
{
RCLCPP_INFO(get_logger(), "Deactivating");
std::vector<pluginlib::UniquePtr<btnav_actions::btnavActionBase>>::iterator iter;
for (iter = actions_.begin(); iter != actions_.end(); ++iter)
{
(*iter)->deactivate();
}
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn actionServer::on_cleanup(const rclcpp_lifecycle::State& /*state*/)
{
RCLCPP_INFO(get_logger(), "Cleaning up");
std::vector<pluginlib::UniquePtr<btnav_actions::btnavActionBase>>::iterator iter;
for (iter = actions_.begin(); iter != actions_.end(); ++iter)
{
(*iter)->cleanup();
}
actions_.clear();
transform_listener_.reset();
tf_.reset();
footprint_sub_.reset();
costmap_sub_.reset();
collision_checker_.reset();
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn actionServer::on_shutdown(const rclcpp_lifecycle::State&)
{
RCLCPP_INFO(get_logger(), "Shutting down");
return nav2_util::CallbackReturn::SUCCESS;
}
} // end namespace btnav_action_server
自定义的action server以独立进程的形式存在，因此需要建立其node，在package的“src/”目录下创建文件“main.cpp”，添加如下代码：
/******************************************************************
lifecycleNode action server node
Features:
- lifecycleNode action server node
- xxx
Written by Xinjue Zou, xinjue.zou.whi@gmail.com
GNU General Public License, check LICENSE for more information.
All text above must be included in any redistribution.
Changelog:
2020-10-30: Initial version
2020-xx-xx: xxx
******************************************************************/
#include "rclcpp/rclcpp.hpp"
#include "motion_ctrl_btnav_action/action_server.hpp"
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto actions_node = std::make_shared<btnav_action_server::actionServer>();
rclcpp::spin(actions_node->get_node_base_interface());
rclcpp::shutdown();
return 0;
}
在package的根目录创建文件“btnav_action_plugin.xml”，添加如下内容：
<class_libraries>
<library path="btnav_action_spinGO">
<class name="btnav_action_server/SpinGO" type="btnav_action_server::SpinGO" base_class_type="btnav_actions::btnavActionBase">
<description></description>
</class>
</library>
</class_libraries>
在原来基础上增加lib的依赖，以及plugin.xml文件的导出，添加如下代码：
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>motion_ctrl_btnav_action</name>
<version>0.0.1</version>
<description>plugins for bt_navigator</description>
<maintainer email="xinjue.zou.whi@gmail.com">Xinjue</maintainer>
<license>BSD</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>rclcpp_lifecycle</depend>
<depend>nav2_util</depend>
<depend>nav2_costmap_2d</depend>
<depend>nav2_msgs</depend>
<depend>pluginlib</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
<motion_ctrl_btnav_action plugin="${prefix}/btnav_action_plugin.xml" />
</export>
</package>
也是在原来的基础上添加lib的依赖，自定义plugin的编译目标，以及自定义plugin的导出，添加如下代码：
cmake_minimum_required(VERSION 3.5)
project(motion_ctrl_btnav_action)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(nav2_util REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(pluginlib REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
include_directories(
include
)
set(library_name btnav_action_server_core)
set(executable_name btnav_action_server)
set(dependencies
rclcpp
rclcpp_action
rclcpp_lifecycle
nav2_util
nav2_costmap_2d
nav2_msgs
pluginlib)
# plugins
add_library(btnav_action_spinGO SHARED
plugins/spinGoalOrientation.cpp
)
ament_target_dependencies(btnav_action_spinGO
${dependencies}
)
pluginlib_export_plugin_description_file(motion_ctrl_btnav_actionbtnav_action_plugin.xml)
# library
add_library(${library_name}
src/action_server.cpp)
ament_target_dependencies(${library_name}
${dependencies})
# executable
add_executable(${executable_name}
src/main.cpp)
target_link_libraries(${executable_name} ${library_name})
ament_target_dependencies(${executable_name}
${dependencies})
install(TARGETS ${library_name}
btnav_action_spinGO
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(TARGETS ${executable_name}
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY include/
DESTINATION include/
)
install(FILES btnav_action_plugin.xml
DESTINATION share/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
在终端输入命令：
cd ~/dev_ws
colcon build --packages-select motion_ctrl_btnav_action
ros2 run motion_ctrl_btnav_action btnav_action_server
成功运行后如下图：
ros2 lifecycle command line interface的详细说明可以参考官网链接：
https://index.ros.org/p/lifecycle/
在终端中先运行btnav_action_server，然后打开一个新终端，输入lifecycle command line interface验证action server
查看btnav_action_server当前可用状态：
在终端输入命令：
ros2 lifecycle list btnav_action_server
将显示自定义action server当前可用的状态，比如当前可以transit的两个状态“configure”和“shutdown”，如下图：
查看btnav_action_server当前状态：
在终端输入命令：
ros2 lifecycle get /btnav_action_server
将显示自定义action server当前的状态，比如当前状态“unconfigured”，如下图：
查看状态迁移的ID：
在终端输入命令：
ros2 interface show lifecycle_msgs/msg/Transition
将显示自定义action server可迁移状态的ID，如下图：
执行configure：
在终端输入命令：
ros2 lifecycle set /btnav_action_server configure
将显示状态迁移成功，同时，运行action server终端显示创建并正在配置自定义的插件“spinGO”，如下图：
激活插件：
通过上述查看到的可迁移状态ID，比如“activate=3”，尝试切换状态到active，在终端输入命令：
ros2 service call /btnav_action_server/change_state lifecycle_msgs/srv/ChangeState "{transition: {id: 3}}"
将显示状态迁移成功，同时，运行action server终端显示正在激活自定义的插件“spinGO”，如下图：
navigation 2官网，有自定义plugin的说明，涉及到了costmap2D，planner，controller，behavior tree，recoveries，详细内容及其对应概念可以参考官网链接：
https://navigation.ros.org/plugin_tutorials/index.html
但涉及到具体实施环节，有些关键点的说明不是特别明确，尤其是自定义plugin在各个环节的识别和衔接，苇航智能将重点对该部分做说明
action server能够动态的识别plugins并对其进行调用，依赖于plugin的描述文件<xxx_plugin>.xml文件，如下图：
该文件是action server所需要动态装载plugins的描述，包含了plugins的名称，类名称，以及基类名称，如下图：
该文件被读取依赖于package.xml中对该文件路径的申明以及CMakeLists中对该文件的导出，通常将原文件存储于package根目下，并导出到“install<package>/share/<package>”目录下，如下图：
该文件中所声明plugins，在action server动态装载时的识别由自定义的action server中plugin load定义，如下图：
上述动态加载的识别如果出现不匹配的情况，则可能会出现下图中不能正确加载插件的错误，如下图：
如果插件描述文件plugin.xml没有正确导出，或者路径有误则会出现下图中不能识别插件声明类型的错误：
NOTE：注意观察最后语句“Declared types are ”，在未正确找到plugin.xml和找到情况下的区别
需要注意区别的是：这里的action nodes不是通常ROS概念中的node，而是behavior tree里的action node 概念，关于behavior tree的基本概念，请参考官网链接：
https://www.behaviortree.dev/
本说明基于苇航智能的导航package，即“motion_ctrl_nav”，创建action nodes，也可以单独创建package。在“motion_ctrl_nav”的根目录下，创建文件夹“plugins”，用于存储behavior tree的plugins，如下图：
分别在目录“include/<package>”和“plugins”中创建文件“spin_goal_orientation_action.hpp”和“spin_goal_orientation_action.cpp”，添加如下代码：
HPP
/******************************************************************
spin to the goal orientation, an action plugin for bt_navigator
Features:
- get current position from blackboard
- calculate spin dist toward the navigating goal
- interact with ROS 2 action interfaces
- xxx
Written by Xinjue Zou, xinjue.zou@outlook.com
GNU General Public License, check LICENSE for more information.
All text above must be included in any redistribution.
Changelog:
2020-11-01: Initial version
2020-xx-xx: xxx
******************************************************************/
#pragma once
#include <string>
#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/spin.hpp"
#include "behaviortree_cpp_v3/action_node.h"
namespace nav2_behavior_tree
{
class SpinGOAction : public BtActionNode<nav2_msgs::action::Spin>
{
public:
SpinGOAction(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf);
void on_tick() override;
static BT::PortsList providedPorts()
{
return providedBasicPorts(
{
BT::InputPort<double>("spin_dist", 1.57, "Spin distance")
});
}
};
} // namespace nav2_behavior_tree
CPP
/******************************************************************
spin to the goal orientation, an action plugin for bt_navigator
Features:
- get current position from blackboard
- calculate spin dist toward the navigating goal
- interact with ROS 2 action interfaces
- xxx
Written by Xinjue Zou, xinjue.zou@outlook.com
GNU General Public License, check LICENSE for more information.
All text above must be included in any redistribution.
******************************************************************/
#include "motion_ctrl_nav/plugins/action/spin_goal_orientation_action.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
namespace nav2_behavior_tree
{
SpinGOAction::SpinGOAction(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::Spin>(xml_tag_name, action_name, conf)
{
}
void SpinGOAction::on_tick()
{
RCLCPP_INFO(node_->get_logger(), "spinnnnnnnnnnnnnnnnnnnnnnnnnnn");
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
BT::NodeBuilder builder =
[](const std::string & name, const BT::NodeConfiguration & config)
{
return std::make_unique<nav2_behavior_tree::SpinGOAction>(name, "SpinGO", config);
};
factory.registerBuilder<nav2_behavior_tree::SpinGOAction>("SpinGO", builder);
}
在package的根目录创建文件夹“behavior_trees”，在该目录下继续创建文件“navigate_w_replanning_and_recovery_spin_goal.xml”，添加如下代码：
<!--
This Behavior Tree replans the global path periodically at 1 Hz and it also has
recovery actions.
-->
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</RateController>
<SpinGO/>
<!--<Spin spin_dist="1.57"/>-->
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="FollowPath"/>
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
</RecoveryNode>
</PipelineSequence>
<ReactiveFallback name="RecoveryFallback">
<GoalUpdated/>
<SequenceStar name="RecoveryActions">
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
<Spin spin_dist="1.57"/>
<Wait wait_duration="5"/>
</SequenceStar>
</ReactiveFallback>
</RecoveryNode>
</BehaviorTree>
</root>
该behavior tree修改于navigation 2默认的behavior tree文件“navigate_w_replanning_and_recovery.xml”，区别在于在“PipelineSequence”的两个子节点中插入了自定义action node的节点“SpinGO”
在原来基础上增加lib的依赖，添加如下代码：
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>motion_ctrl_nav</name>
<version>0.0.1</version>
<description>navigation on motion_ctrl</description>
<maintainer email="xinjue.zou.whi@gmail.com">Xinjue</maintainer>
<license>BSD</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>rclcpp_lifecycle</depend>
<depend>behaviortree_cpp_v3</depend>
<depend>builtin_interfaces</depend>
<depend>geometry_msgs</depend>
<depend>nav2_msgs</depend>
<depend>nav_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>nav2_util</depend>
<depend>lifecycle_msgs</depend>
<depend>nav2_common</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
也是在原来的基础上添加lib的依赖，自定义plugin的编译目标，以及自定义plugin的导出，添加如下代码：
cmake_minimum_required(VERSION 3.5)
project(motion_ctrl_nav)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(nav2_common REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(std_msgs REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(behaviortree_cpp_v3 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(nav2_util REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
include_directories(
include
)
set(dependencies
rclcpp
rclcpp_action
rclcpp_lifecycle
geometry_msgs
nav2_msgs
nav_msgs
behaviortree_cpp_v3
tf2
tf2_ros
tf2_geometry_msgs
std_msgs
std_srvs
nav2_util
)
add_library(nav2_spinGO_action_bt_node SHARED plugins/action/spin_goal_orientation_action.cpp)
list(APPEND plugin_libs nav2_spinGO_action_bt_node)
foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies})
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
endforeach()
install(TARGETS ${plugin_libs}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(DIRECTORY include/
DESTINATION include/
)
# install others
install(DIRECTORY
launch
param
rviz
behavior_trees
DESTINATION share/${PROJECT_NAME}
)
ament_export_libraries(
${plugin_libs}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
navigation_launch负责启动各类lifecycle nodes，因此，上述自定义的btnav_action_server也应由该launch文件负责
首先，在“lifecycle_nodes”中添加自定义的“btnav_action_server”，如下图：
其次，添加运行自定义lifecycle node的代码，如下图：
如果navigation stack之前使用的是navigation 2默认的bringup_launch.py，则需要对其进行重载，以调用重载后的navigation_launch.py
首先，添加重载后navigation_launch.py的路径，如下图：
其次，修改include navigation_launch.py的参数，如下图：
本文依据苇航智能的navigation stack文件为例，请根据实际情况做适应性修改
navigation stack的launch文件需要为上述重载的两个launch文件，提供behavior tree的路径，同时，需要调用重载后的bringup_launch
首先，指定bringup_launch的路径，如下图：
其次，修改include bringup_launch.py的参数，以及为bringup_launch提供behavior tree路径的参数。如果使用的是turtlebot3的navigation stack launch文件，默认是不提供behavior tree路径参数的，需要自行添加，如下图：
完成上述步骤后，编译，在终端输入命令：
cd ~/dev_ws
colcon build --packages-select motion_ctrl_nav
现在可以验证自定义的bt plugin是否生效。本文目的为构建bt plugin的路径说明，因此上述action node SpinGO的on_tick函数没有具体的旋转执行代码，而是通过输出文本“spinnnnnnnnnnnnnnnnnnnnnnnnnnn”用于验证
首先，运行机器人，这里以苇航智能NaviBOT A0为例，在终端输入命令：
cd ~/dev_ws
sudo su
. install/setup.bash
ros2 launch motion_ctrl NaviBOT_A0.launch.py
其次，运行remote导航stack，在新终端输入命令：
cd ~/dev_ws
. install/setup.bash
ros2 launch motion_ctrl_nav navigation_remote_NaviBOT_A0.launch.py map:=/home/whi/dev_ws/whiMap_h.yaml
导航stack运行后，查看终端输出，可以看到自定义的lifecycle node “btnav_action_server”运行，如下图：
在bt_navigator中，可以看到添加的自定义plugin “SpinGO”被初始化，如下图：
最后，在rviz中为机器人设置初始位置以激活导航，并设置导航目标点，查看运行导航的终端，将看到自定义plugin “SpinGO”输出的字符串“spinnnnnnnnnnnnnnnnnnnnnnnnnnn”，表明自定义behavior tree节点SpinGO成功运行，如下图：
至此，自定义behavior tree的plugin已经构建完毕，当修改了behavior tree的描述文件，插入了自定义plugin后，bt_navigator将能够动态的加载自定义的action plugin，能够使得机器人完成自定义动作和逻辑，并且后续动作逻辑的变更和扩展不会影响到原有代码，大大提高了机器人代码的扩展性
本文所述内容以整个过程为主线，意在介绍创建自定义bt plugin的宏观过程，整个过程涉及到了ROS 2和Navigation 2新构架下的很多关键知识点，限于篇幅文中没有具体展开，只给出了链接，读者可以通过文中连接详细了解
本文第三部分的自定义action server，没有介绍在param.yaml中对server的参数配置关联的内容，其方式可以参考nav2_recoveries实现，留给读者练习实践
为Navigation Stack加入自定义的逻辑插件不限于本文所述方式，在ROS 2的架构下提供了很多灵活的方式，比如构建自定义的controller，planner等等，但基本过程和思想与本文描述类似，留给读者去探索
如果文中有错误和不明确的地方，欢迎指正，共同交流，xinjue.zou.whi@gmail.com