代价地图是在 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));
}

对应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

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

又是和ROS1的有所不同

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

再打开rviz2，把设置都改好

再手动激活和配置节点

1
2

ros2 lifecycle set /map_server configure  
ros2 lifecycle set /map_server activate

跟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 中没有一个像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 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

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

查看当前用户名和邮箱

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"

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

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

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的形式

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

异步动作有以下要求：

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

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

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

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

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

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

行为树中的共有数据是存放在Blackboard中的。Blackboard是以键值对的形式存储数据的。各个行为树的叶子节点均可以通过key访问到需要的数据。节点的输入端口可以从黑板读取数据，节点的输出端口可以写入黑板。

Ports是用于在节点间交换数据而存在的。一个行为树叶子节点可以定义输入的Ports和输出的Ports。当不同叶子节点的Port有相同的key名称时，可以认为它们是相通的。当在行为树叶子节点中声明了这些Ports时，也需要同时在xml文件中描述这些Ports。

注意设置子树的__shared_blackboard属性为true，否则C++程序可能报错

程序中如果加载了多个行为树，黑板变量可以共享。