ROS2中的代价地图

代价地图是在 planner_servercontroller_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

问题

编译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

又是和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中的参数

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

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的几个常见基本问题

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

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等等,也可以用TRACEINFO或者 LOG_T的形式


异步动作 StatefulActionNode

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++程序可能报错

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