在ROS2下启动建图或者导航的时候,Rviz收不到数据。

先说解决办法,先打开Rviz,把需要显示的插件和话题名准备好,这里建议大家用Rviz的配置文件,最后再启动数据节点。

为什么会有这个问题呢?原因是因为ROS2比ROS1多了一个东西——LifecycleNode!

ROS2中的LifecycleNode是一个特殊的ROS节点,它实现了一组标准的生命周期状态转换。这些状态包括未配置、已配置、未激活、激活、未激活和已清理。LifecycleNode提供了一个标准化的接口,使得节点可以在这些状态之间转换,并在转换过程中执行必要的操作。

LifecycleNode的实现基于ROS2的rclcpp库,它提供了一组标准的回调函数,可以在节点状态转换时执行。LifecycleNode还提供了一组标准的服务,用于执行节点状态转换操作。这些服务包括配置节点、激活节点、暂停节点、重启节点和清理节点。

LifecycleNode可以用于管理ROS2中的各种节点,例如传感器节点、控制节点、通信节点等。它可以确保节点在启动和关闭时执行必要的操作,并在节点状态发生变化时自动执行相应的回调函数。这使得节点的开发和维护更加简单和可靠。

下面是一个ROS2中LifecycleNode的一个使用模板。

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/rclcpp_lifecycle.hpp>

class MyNode : public rclcpp_lifecycle::LifecycleNode
{
public:
  explicit MyNode(const rclcpp::NodeOptions& options)
  : rclcpp_lifecycle::LifecycleNode("my_node", options)
  {
    // 节点初始化代码
  }

protected:
  virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_configure(const rclcpp_lifecycle::State& state)
  {
    //节点配置代码
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }

  virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_activate(const rclcpp_lifecycle::State& state)
  {
    //节点激活代码
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }

  virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_deactivate(const rclcpp_lifecycle::State& state)
  {
    //节点暂停代码
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }

  virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_cleanup(const rclcpp_lifecycle::State& state)
  {
    //节点清理代码
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }

  virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_shutdown(const rclcpp_lifecycle::State& state)
  {
    //节点关闭
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto options = rclcpp::NodeOptions().use_intra_process_comms(true);
  auto node = std::make_shared<MyNode>(options);
  rclcpp::spin(node->get_node_base_interface());
  rclcpp::shutdown();
  return 0;
}

在这个模板中,我们创建了一个名为"MyNode"的派生自LifecycleNode的类。在类的构造函数中,我们初始化了节点,并在类的保护部分中实现了LifecycleNode的回调函数。这些回调函数分别在节点的不同状态下被调用,我们可以在这些回调函数中编写节点的配置、激活、暂停、重启、清理和关闭代码。

在main函数中,我们初始化了ROS2节点,并使用NodeOptions配置了节点的通信方式。然后,我们创建了一个MyNode对象,并通过调用spin函数运行节点。最后,我们在main函数结束时关闭ROS2节点。

这是LifecycleNode的模型图,请结合这个图理解上面的模板和下面的代码!

  • 配置 - 执行 onConfigure() - 加载配置、冗长的设置步骤、获取节点生命周期内使用的资源,如常量发布者/侦听器、内存缓冲区分配等…
  • 清理- 执行 onCleanup() - 放弃资源,擦除内存。 新的开始,清理状态。
  • 激活 - 执行 onActivate() - 获取传感器等短期资源,激活所有资源。 设置时间短。 启动主节点任务。
  • 停用 - 执行 onDeactivate() - 反向Activatingsteps(反激活)。 暂停执行,释放短期资源。
  • 关闭 - 执行 onShutdown() - 最后步骤。 删除剩下的资源等。 没有信息从此处回来。
  • 错误处理- 执行 onError() - 错误处理状态。 如果错误可以处理,则恢复到 Unconfigured,否则,转到 Finalized 销毁节点。
#include "nav2_map_server/map_server.hpp"

#include <fstream>
#include <memory>
#include <stdexcept>
#include <string>

#include "nav2_map_server/occ_grid_loader.hpp"
#include "nav2_util/node_utils.hpp"
#include "yaml-cpp/yaml.h"

using namespace std::chrono_literals;

namespace nav2_map_server
{

	MapServer::MapServer():nav2_util::LifecycleNode("map_server")
	{
		//构造函数,节点创建
		RCLCPP_INFO(get_logger(), "Creating");
		declare_parameter("yaml_filename", rclcpp::ParameterValue(std::string("map.yaml")));
	}

	MapServer::~MapServer()
	{
		//析构函数
		RCLCPP_INFO(get_logger(), "Destroying");
	}

	nav2_util::CallbackReturn
	MapServer::on_configure(const rclcpp_lifecycle::State & state)
	{
		//节点配置代码,
		RCLCPP_INFO(get_logger(), "Configuring");
		
		std::string yaml_filename;
		get_parameter("yaml_filename", yaml_filename);
		
		std::ifstream fin(yaml_filename.c_str());
		if (fin.fail()) 
		{
			throw std::runtime_error("Could not open '" + yaml_filename + "': file not found");
		}
		
		YAML::Node doc = YAML::LoadFile(yaml_filename);
		
		std::string map_type;
		try
		{
			map_type = doc["map_type"].as<std::string>();
		}
		catch (YAML::Exception &) 
		{
			map_type = "occupancy";
		}
		
		if (map_type == "occupancy")
		{
			map_loader_ = std::make_unique<OccGridLoader>(shared_from_this(), yaml_filename);
		}
		else
		{
			std::string msg = "Cannot load unknown map type: '" + map_type + "'";
			throw std::runtime_error(msg);
		}
		
		map_loader_->on_configure(state);
		
		return nav2_util::CallbackReturn::SUCCESS;
	}

	nav2_util::CallbackReturn
	MapServer::on_activate(const rclcpp_lifecycle::State & state)
	{
		//节点激活代码
		RCLCPP_INFO(get_logger(), "Activating");
		map_loader_->on_activate(state);
		
		return nav2_util::CallbackReturn::SUCCESS;
	}

	nav2_util::CallbackReturn
	MapServer::on_deactivate(const rclcpp_lifecycle::State & state)
	{
		//节点释放代码
		RCLCPP_INFO(get_logger(), "Deactivating");
		map_loader_->on_deactivate(state);
		
		return nav2_util::CallbackReturn::SUCCESS;
	}

	nav2_util::CallbackReturn
	MapServer::on_cleanup(const rclcpp_lifecycle::State & state)
	{
		//节点清理代码
		RCLCPP_INFO(get_logger(), "Cleaning up");
		
		map_loader_->on_cleanup(state);
		map_loader_.reset();
		
		return nav2_util::CallbackReturn::SUCCESS;
	}

	nav2_util::CallbackReturn
	MapServer::on_error(const rclcpp_lifecycle::State &)
	{
		//节点错误代码
		RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state");
		
		return nav2_util::CallbackReturn::SUCCESS;
	}

	nav2_util::CallbackReturn
	MapServer::on_shutdown(const rclcpp_lifecycle::State &)
	{
		//节点关闭代码
		RCLCPP_INFO(get_logger(), "Shutting down");
		
		return nav2_util::CallbackReturn::SUCCESS;
	}

}

这里ROS2中Navigation2里面的地图发布节点源代码,在ROS2中启动导航后再打开Rviz,70%的概率是看不到这个节点发布的栅格地图的。从代码来看,地图数据的发布只在激活调用了map_loader_的on_activate,而这里正是栅格地图数据发布的代码。

所以ROS2在启动导航的时候,map数据只会发布一次!如果你没在它发布前打开Rviz,那么Rviz就订阅不到这个数据。

ROS2下面很多节点都有这个特性,虽然方便的节点的管理和维护,但是用久了ROS1的话,这个问题确实很操蛋~