添加 TreeNodesModel|行为树

使用Groot打开行为树文件，有时会报警一大堆：不识别某Action，这是因为xml文件必须具备 TreeNodesModel才能让Groot识别。显然xml的作者不太会用行为树。

此外它还需要：节点类型，端口的名称和类型（输入/输出）。我们不需要手动去添加，工作了太大了。行为树提供了函数来生成:

1
2
3
4
5
BT::BehaviorTreeFactory factory;  
// register here your user-defined Nodes 

std::string xml_models = BT::writeTreeNodesModelXML(factory); 
cout << xml_models <<endl;

可以将 XML 保存到文件中，然后点击 Groot2 中的导入模型按钮。或者手动将输出的 xml_models直接添加到 .xml文件中。

也就是需要先定义Node，然后注册。才能调用writeTreeNodesModelXML，否则不识别Action的名称。当然也必须在 createTreeFromFile之后。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
#include <iostream>
#include "behaviortree_cpp_v3/action_node.h"
#include "behaviortree_cpp_v3/xml_parsing.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behaviortree_cpp_v3/behavior_tree.h"

using namespace std;
using namespace BT;

class MyClass : public BT::SyncActionNode
{
public:
    MyClass(const std::string& name, [[maybe_unused]] const BT::NodeConfiguration& config):
        BT::SyncActionNode(name, {})
    {
    }
    ~MyClass()
    {
    }
    static BT::PortsList providedPorts()
    {
        return {BT::InputPort<string>("input_code","NULL_ERROR","error code "),
                    BT::InputPort<string>("input_action","NULL_ACTION","input action ")};
    }
    BT::NodeStatus tick()
    {
        return BT::NodeStatus::SUCCESS;
    }
};

int main([[maybe_unused]] int argc, [[maybe_unused]] char ** argv)
{
    BT::BehaviorTreeFactory factory;
    factory.registerNodeType<MyClass>("ClassName");

    auto tree = factory.createTreeFromFile("/home/user/bt.xml");

    std::string xmlmodels = BT::writeTreeNodesModelXML(factory);
    cout << endl << xmlmodels << endl;

    return 0;
}
ROS2中的代价地图|ROSROS2

代价地图是在 planner_server 和 controller_server中初始化的。

清除代价地图

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
<Action ID="ClearEntireCostmap">
	<input_port name="service_name">Service name</input_port>
	<input_port name="server_timeout">Server timeout</input_port>
</Action>

<Action ID="ClearCostmapExceptRegion">
	<input_port name="service_name">Service name</input_port>
	<input_port name="server_timeout">Server timeout</input_port>
	<input_port name="reset_distance">Distance from the robot above which obstacles are cleared</input_port>
</Action>

<Action ID="ClearCostmapAroundRobot">
	<input_port name="service_name">Service name</input_port>
	<input_port name="server_timeout">Server timeout</input_port>
	<input_port name="reset_distance">Distance from the robot under which obstacles are cleared</input_port>
</Action>

行为树节点部分在clear_costmap_service.cpp，三个类都继承了BtServiceNode

服务端在 clear_costmap_service.cpp， 这里创建了service，编译为 libnav2_costmap_2d_core.so. 构造函数部分可以看到三个service的创建及其名称

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
ClearCostmapService::ClearCostmapService(
  const nav2_util::LifecycleNode::WeakPtr & parent,
  Costmap2DROS & costmap)
: costmap_(costmap)
{
  auto node = parent.lock();
  logger_ = node->get_logger();
  reset_value_ = costmap_.getCostmap()->getDefaultValue();

  clear_except_service_ = node->create_service<ClearExceptRegion>(
    "clear_except_" + costmap_.getName(),
    std::bind(
      &ClearCostmapService::clearExceptRegionCallback, this,
      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));

  clear_around_service_ = node->create_service<ClearAroundRobot>(
    "clear_around_" + costmap.getName(),
    std::bind(
      &ClearCostmapService::clearAroundRobotCallback, this,
      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));

  clear_entire_service_ = node->create_service<ClearEntirely>(
    "clear_entirely_" + costmap_.getName(),
    std::bind(
      &ClearCostmapService::clearEntireCallback, this,
      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}

比如名称可以是 clear_around_local_costmap

对应3个service

1
2
3
/local_costmap/clear_around_local_costmap
/local_costmap/clear_entirely_local_costmap
/local_costmap/clear_except_local_costmap

问题

编译nav2_costmap_2d后，直接运行ros2 run nav2_costmap_2d nav2_costmap，结果报错

1
[pluginlib.ClassLoader]: Skipped loading plugin with error: XML Document '/home/user/project/install/nav2_costmap_2d/share/nav2_costmap_2d/costmap_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..

如果行为树有节点是Nav2的客户端，在服务端没启动时，服务不可用，行为树运行时会直接报错，不会运行任何节点。

如果没启动服务端，客户端发起请求时会报错

1
2
3
[ERROR] [1754535646.216247848] [nav2_node]: "global_costmap/clear_entirely_global_costmap" service server not available after waiting for 1.00s
[nav2_node-1] terminate called after throwing an instance of 'std::length_error'
[nav2_node-1]   what():  basic_string::_M_create

可以把 BtServiceNode构造函数的throw一行删掉，这样即使没有服务端，也不会报错

.png

Action和service_name不符合，加载行为树会报错

1
what():  could not create client: create_client() called for existing request topic name rq/global_costmap/clear_entirely_global_costmapRequest with incompatible type navit_msgs::srv::dds_::ClearEntireCostmap_Request_, at ./src/rmw_client.cpp:165, at ./src/rcl/client.c:146

Nav2中的map_server|ROSROS2

又是和ROS1的有所不同

先执行：ros2 run nav2_map_server map_server --ros-args --param yaml_filename:=map/fishbot_map.yaml

再打开rviz2，把设置都改好
截图 2025-08-05 14-42-41.png

再手动激活和配置节点

1
2
ros2 lifecycle set /map_server configure  
ros2 lifecycle set /map_server activate

这样才能正常显示地图
在加载map成功后，如果再加载costmap，对应的Rviz里的 Durability Policy 可以不改为 Transient Local

跟ROS1有所不同，不会一直发布/map话题。而且/map不会显示所有地图数据，会用- '...'表示，估计是开发者觉得显示一大堆地图数据没有意义。

也可以在运行时使用load_map服务更改地图

Map Server是一个可组合的 ROS2 节点。默认情况下，有一个map_server可执行文件来实例化这些节点之一，但如果需要，也可以将多个地图服务器节点组合到一个进程中。

Nav2 使用 ROS2 参数机制来获取要使用的 YAML 文件名。这实际上引入了一个间接层来获取地图 yaml 文件名。例如，对于一个名为“map_server”的节点，参数文件看起来是这样的：

1
2
3
map_server:
    ros__parameters:
        yaml_filename: "map.yaml"

也可以在一个进程中运行多个地图服务器节点，此时参数文件会按节点名称分隔参数，例如：

1
2
3
4
5
6
7
8
# combined_params.yaml
map_server1:
    ros__parameters:
        yaml_filename: "some_map.yaml"

map_server2:
    ros__parameters:
        yaml_filename: "another_map.yaml"

然后，使用包含两个节点参数的参数文件来启动该进程：process_with_multiple_map_servers __params:=combined_params.yaml

保存地图

ros2 run nav2_map_server map_saver_cli [arguments] [--ros-args ROS remapping args]

比如：ros2 run nav2_map_server map_saver_cli -f test

两个 service

1
ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "{map_url: /ros/maps/map.yaml}"
1
ros2 service call /map_saver/save_map nav2_msgs/srv/SaveMap "{map_topic: map, map_url: my_map, image_format: pgm, map_mode: trinary, free_thresh: 0.25, occupied_thresh: 0.65}"
ROS2中的参数|ROSROS2

ROS2 中没有一个像ROS 1 那样的集中式参数服务器。ROS 2 的参数是与每个节点绑定的，每个节点都有自己的参数管理系统。也就是说 节点私有: 参数与节点相关联，而不是全局共享。节点可以声明、获取和更新自己的参数，也可以通过客户端库（SyncParametersClient或AsyncParametersClient）访问和修改其他节点的参数，从而实现参数共享。参数可以在节点运行时动态地获取和更新，方便配置和调试.

ROS 2 提供了参数接口，可以用于在节点之间共享参数，实现参数的动态更新和访问。

  • ros2 param list
  • ros2 param describe
1
2
3
4
5
6
7
8
ros2 param describe /turtlesim background_r
Parameter name: background_r
  Type: integer
  Description: Red channel of the background color
  Constraints:
    Min value: 0
    Max value: 255
    Step: 1
  • ros2 param get node_name parameter_name
1
2
ros2 param get /turtlesim background_r
Integer value is: 69

  • ros2 param set (省略)

  • ros2 run + param

  • ros2 run —ros-args —params-file

  • 设置参数
1
2
3
4
5
6
7
8
9
10
11
12
13
14
node->declare_parameter("my_string_param", "default_value");

// nav2_util里实现了先判断参数是否存在，再设置
template<typename NodeT>
void declare_parameter_if_not_declared(
  NodeT node,
  const std::string & parameter_name,
  const rclcpp::ParameterValue & default_value,
  const ParameterDescriptor & parameter_descriptor = ParameterDescriptor())
{
  if (!node->has_parameter(parameter_name)) {
    node->declare_parameter(parameter_name, default_value, parameter_descriptor);
  }
}
  • 获得参数
1
2
3
4
5
6
// Retrieve the parameter
rclcpp::Parameter param_obj = this->get_parameter("my_string_param");
printf("type name: %s\n", param_obj.get_type_name().c_str()  );
// Extract the value
std::string param_value = param_obj.as_string();
std::vector<std::string> param_value = param_obj.as_string_array();

但是有时候参数类型不一定就是string，可能是string的vector，可以用get_type_name查看类型。

如果是vector<std::string>，其size仅为1，实际不如就让类型为string，这应当是ROS2的一个缺陷。

1
2
3
4
template<typename ParameterT >
bool rclcpp::Node::get_parameter_or(const std::string&  name,
   ParameterT&  value,  const ParameterT&  alternative_value 
                                   ) const

获取参数值，如果未设置，则获取alternative_value，并将其赋给value。

  • 删除参数
1
this->remove_parameter("example_param");
ROS2的几个常见基本问题|ROSROS2

ROS2 ConnectionResetError: [Errno 104] Connection reset by peer

执行ros2 node list遇到的错误，有一大堆信息，这是最后一句。按如下步骤解决:

a. 检查守护进程是否确实在运行

ros2 daemon status 或者 systemctl status ros2-daemon.service

b. 重新启动守护进程

如果守护进程没有运行，尝试启动它: ros2 daemon start

然后再次检查状态: ros2 daemon status。现在ros2 node list正常了。

未知节点在运行

1
2
3
4
5
/controller_manager_lift
/diff_chassis_driver
/fault_service_node
/iot
/task_mgr

以上是ros2 node list查看到的结果，但是用ps aux都看不到。由于不存在ros2 node ping，不确定这几个节点到底是不是存在。

这几个节点应该是其他机器上的。需要在.bashrc里加入export ROS_DOMAIN_ID=77

ROS2中的Action和Service|ROSROS2

action

  • ros2 action list -t

  • ros2 action info /compute_path_to_pose

1
2
3
4
Action: /compute_path_to_pose
Action clients: 0
Action servers: 1
    /planner_server

  • ros2 action type /compute_path_to_pose
    结果是 nav2_msgs/action/ComputePathToPose

  • ros2 interface show nav2_msgs/action/ComputePathToPose

结果是 action 文件的详细信息

  • ros2 action send_goal
1
2
3
4
5
6
7
8
9
10
nvidia@ubuntu:~$ ros2 interface show turtlesim/action/RotateAbsolute.action

# The desired heading in radians
float32 theta
---
# The angular displacement in radians to the starting position
float32 delta
---
# The remaining rotation in radians
float32 remaining

这三项依次是目标，结果，反馈

  • 发送操作目标: ros2 action send_goal <action_name> <action_type> <values>

比如 ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: 1.57}"

1
2
3
4
5
6
7
8
9
10
nvidia@ubuntu:~$ ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: 1.57}"
Waiting for an action server to become available...
Sending goal:
theta: 1.57
Goal accepted with ID: 5cd8eb561348477abbb532fc816ce792

Result:
delta: 1.5680029392242432

Goal finished with status: SUCCEEDED

当然结果与目标有一定误差

要查看此目标的反馈，请在上次运行的命令中添加 --feedback，会看到连续的打印输出，将继续收到反馈，直到目标完成。

interface

  • ros2 interface list

分类显示消息、动作、服务的所有类型

  • ros2 interface package pkg_name
1
2
3
4
5
user@robot:~$ ros2 interface package action_msgs
action_msgs/srv/CancelGoal
action_msgs/msg/GoalInfo
action_msgs/msg/GoalStatus
action_msgs/msg/GoalStatusArray

获知某个消息类型是谁在用，可以这样查

1
2
3
4
5
6
7
8
user@robot:~$ ros2 interface packages | grep pendulum_msgs
pendulum_msgs
user@robot:~$ ros2 interface package pendulum_msgs
pendulum_msgs/msg/RttestResults
pendulum_msgs/msg/JointState
pendulum_msgs/msg/JointCommand

user@robot:~$ ros2 topic find pendulum_msgs/msg/RttestResults

  • 显示消息的成员
1
2
user@robot:~$ ros2 interface proto std_msgs/msg/String
"data: ''"

service

  • ros2 service type

可以看到服务的定义。

  • ros2 service find

找出指定类型的所有服务

  • ros2 service find std_srvs/srv/Empty

  • ros2 interface show

显示服务的定义

  • call命令比ROS1复杂了，需要指明service类型: ros2 service call service_name service_type
Git不常用的命令|常用工具

查看当前用户名和邮箱 

1
2
git config user.name
git config user.email

修改用户名和地址，不加global只能生效一次

1
2
git config --global user.name "your name"
git config --global user.email "your email"

打标签

Git 中的tag指向一次commit的id，通常用来给开发分支做一个标记，如标记一个版本号。

1
git tag -a v1.01 -m "Relase version 1.01"

注解：git tag 是打标签的命令，-a 是添加标签，其后要跟新标签号，-m 及后面的字符串是对该标签的注释。

Git 遇到的问题|常用工具

rm 问题

对git本地仓库，使用rm而不是git rm删除一个文件A后，直接commit 和 push，远程仓库仍然有A。此时已经不能再执行git rm了，所以将删除动作添加到暂存区 git add .，然后再commit 和 push
操作过程

branch diverged

1
2
3
4
5
6
7
# 执行 git status 的结果
On branch master
Your branch and 'origin/master' have diverged,
and have 5 and 21 different commits each, respectively.
  (use "git pull" to merge the remote branch into yours)

nothing to commit, working tree clean

这发生在多人可能操作 master 分支的时候，我自己的分支提交完了，切换到master分支，要执行合并时，出现这个问题，并不是文件冲突的问题。按如下方法解决：

如果不需要保留本地的修改，只要执行下面两步：

1
2
git fetch origin
git reset --hard origin/master

当我们在本地提交到远程仓库的时候，如果遇到上述问题，我们可以首先使用如下命令：
1
2
3
4
git rebase origin/master
git pull --rebase
# 提交到远程仓库
git push origin master

一次合并冲突的解决

我在自己的分支me开发，主分支比我领先很多。有一次，我从git上下载了主分支的压缩包，然后手动覆盖了一部分文件以更新，这其实是不正确的操作。

第二天我先切换到主分支，更新到最新，然后切换到me分支，提交到远程后，合并主分支，结果出现的冲突比我自己修改的多很多，因为把我昨天手动覆盖的那些也包括了。这种情况下，只能手动解决冲突，但是大部分文件一定是用主分支，如果一个个打开会很麻烦。

用VSCode打开工程，分支管理里会显示文件冲突，对使用主分支版本的文件，右键选择就可以，而且能同时选多个文件。解决完之后，选择暂存文件，之后可以push了。

fatal: 无法访问 The requested URL returned error: 502

需要关闭代理软件

remote: error: cannot lock ref ‘refs/heads/feat/user’: ‘refs/heads/feat/user/interface’ exists; cannot create ‘refs/heads/feat/user’

push到了不存在的分支

使用VS Code git push报错：Missing or invalid credentials.Error: connect ECONNREFUSED /run/user/1000/vscode-git-ec01

image.png

解决：文件 —> 首选项 —> 搜索 git.auth，取消勾选后重新勾选，然后重启终端。

spdlog的使用|第三方库

spdlog定义的日志级别最低为TRACE，最高为FATAL。当要发布程序时，把SPDLOG_ACTIVE_LEVEL宏的级别提高即可。

只有用SPDLOG_LOGGER_宏写日志，才能正常输出消息所在的文件、函数、行数。SPDLOG_LOGGER_宏本身支持format格式，比如 SPDLOG_LOGGER_DEBUG(console, "debug arg1={1}, arg2={0}", arg2, arg1)

代码中对spdlog进行了一次封装，即quick_log.h的类QUICK_LOG，使用时用LOG_TRACE, LOG_INFO等等，也可以用TRACE， INFO或者 LOG_T的形式

异步动作 StatefulActionNode|行为树

Asynchronous Action 即一个需要很长时间才能完成的动作，在完成标准未满足时会返回 RUNNING。

异步动作有以下要求：

  • 该方法 tick() 不应阻塞太久，执行流程应尽快返回。
  • 如果调用 halt() 方法，应尽快中止。

StatefulActionNode 是实现异步动作的首选方式。当你的代码包含请求-回复模式时，它特别有用，即动作向另一个进程发送异步请求，并定期检查回复是否已收到。根据那则回复，它可能会返回 SUCCESS 或 FAILURE。

StatefulActionNode 的派生类必须重写以下虚拟方法，而不是 tick() ：

  • NodeStatus onStart() ：当节点处于 IDLE 状态时被调用。它可能会立即成功或失败，或者返回 RUNNING。在后一种情况下，下次收到 tick 时，方法 onRunning 将被执行。

  • NodeStatus onRunning() : 当节点处于 RUNNING 状态时调用。返回新状态。

  • void onHalted() : 当此节点被树中的另一个节点中止时调用。