ROS中的时间

ROS时间信息的来源有两种,系统时间和仿真时间,分别被称为Wall TimeROS Time,可以通过参数use_sim_time来进行选择,当use_sim_time为true时选择使用仿真时间,反之使用系统时间。

系统时间直接通过访问本地时钟来获得,而仿真时间则需要订阅/clock话题获得,每当接收到该话题的消息,时间才会更新,在没有接收到该话题的第一个消息时,当前时间为0,/clock是通过仿真时间服务器发布的,通常为ros_bag节点或仿真器如gazebo

由于Rviz也订阅了clock话题,所以在Rviz的状态栏里能看到这两个时间。clock由秒和纳秒两部分组成:

1
2
3
clock: 
secs: 26
nsecs: 7000000


Sophus配置使用

现如今sophus需要依赖fmt库,因此优先安装 fmt

1
2
3
4
5
6
7
git clone  https://github.com/fmtlib/fmt.git
cd fmt
mkdir build
cd build
cmake ..
make
sudo make install

最后生成一堆头文件和静态库 /usr/local/lib/libfmt.a。fmt装完没有动态库libfmt.so,有静态库和cmake文件,在CMake里这样使用

1
2
find_package(FMT REQUIRED)
target_link_libraries(your_program fmt::fmt)

合并rosbag和rosbag editor

Kaist数据集中的代码kaist_tool/kaist2bag很有用,命令rosrun kaist2bag mergebag.py merged.bag <bag_file_1> ... <bag_file_8>, 即可将多个bag合成一个bag

另外有用的工具是 rosbag_editor


如何精确走拐角
Welcome to my blog, enter password to read.
Read more
actionlib的状态机报错 Your executeCallback did not set the goal to a terminal status

报错 Your executeCallback did not set the goal to a terminal status. This is a bug in your ActionServer implementation. Fix your code !

此时的move_base/status消息是 This goal was aborted by the simple action server. The user should have set a terminal status on this goal and did not

源码在actionlib的simple_action_server_imp.hSimpleActionServer<ActionSpec>::executeLoop()

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
43
// executeLoop函数在不断地轮转状态,以检测新的目标到来
template <class ActionSpec>
void SimpleActionServer<ActionSpec>::executeLoop()
{
ros::Duration loop_duration = ros::Duration().fromSec(.1);

while (n_.ok())
{
{
boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
if (need_to_terminate_) // 只在 shutdown() 函数中为 true
break;
}
boost::recursive_mutex::scoped_lock lock(lock_);
if (isActive())
ROS_ERROR_NAMED("actionlib", "Should never reach this code with an active goal");

// new_goal_ 标志被置为true后,即 goalcallback 函数被执行完毕后
else if (isNewGoalAvailable() )
{
GoalConstPtr goal = acceptNewGoal();

ROS_FATAL_COND(!execute_callback_, "execute_callback_ must exist. This is a bug in SimpleActionServer");

// Make sure we're not locked when we call execute
// 重新以新的goal执行executecallback函数
lock.unlock();
execute_callback_(goal);
lock.lock();

if (isActive())
{
ROS_WARN_NAMED("actionlib", "Your executeCallback did not set the goal to a terminal status.\n"
"This is a bug in your ActionServer implementation. Fix your code!\n"
"For now, the ActionServer will set this goal to aborted");
setAborted(Result(), "This goal was aborted by the simple action server. The user should have set a terminal status on this goal and did not");
}
}
// 如果没有新目标,通过条件变量将线程阻塞
else
execute_condition_.timed_wait(lock, boost::posix_time::milliseconds(loop_duration.toSec() * 1000.0f));
}
}

move_base接收到新目标后,会进入回调函数execute_callback_,执行完之后会判断是否还是Active,如果是那说明服务端没有给最终状态,也就是没有执行setSucceeded或者setAborted函数,说明move_base的流程没有运行到最终状态的逻辑那里。


move_base status话题

move_base/status话题堪称调试利器,有必要专门分析一下。尤其goal_id中的id可以表示move_base的客户端是什么,statustext表示导航状态

同类的move_base/result只能在导航最终结束时,发布一个消息,成功或失败,使用价值不大,不分析了。 另一个move_base/feedbackstatus类似,但text部分不能表现导航的最终状态。

move_base/status话题不在常用的move_base源码里,而是在actionlib的服务端代码action_server_imp.hActionServer<ActionSpec>::initialize()

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
status_pub_ =
node_.advertise<actionlib_msgs::GoalStatusArray>("status",
static_cast<uint32_t>(pub_queue_size), true);

// read the frequency with which to publish status from the parameter server
// if not specified locally explicitly, use search param to find actionlib_status_frequency
double status_frequency, status_list_timeout;
if (!node_.getParam("status_frequency", status_frequency) )
{
std::string status_frequency_param_name;
if (!node_.searchParam("actionlib_status_frequency", status_frequency_param_name) )
{
status_frequency = 5.0;
}
else
{
node_.param(status_frequency_param_name, status_frequency, 5.0);
}
}
else
{
ROS_WARN_NAMED("actionlib",
"You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.");
}
node_.param("status_list_timeout", status_list_timeout, 5.0);

this->status_list_timeout_ = ros::Duration(status_list_timeout);

if (status_frequency > 0)
{
status_timer_ = node_.createTimer(ros::Duration(1.0 / status_frequency),
boost::bind(&ActionServer::publishStatus, this, boost::placeholders::_1));
}

看来是通过ros的Timer轮转地发布信息。

话题的默认频率是5hz,如果要修改,可以添加move_base参数actionlib_status_frequency,因为代码里没有默认设置这个参数 比如actionlib_status_frequency: 10

问题 1

导航时观察move_base/status,发现如下结果

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
header: 
seq: 82
stamp:
secs: 379
nsecs: 572000000
frame_id: ''
status_list:
-
goal_id:
stamp:
secs: 376
nsecs: 346000000
id: "Rviz383"
status: 1
text: "This goal has been accepted by the simple action server"
---
header:
seq: 0
stamp:
secs: 381
nsecs: 731000000
frame_id: ''
status_list: []

从Rviz发目标点,突然消息全重置了(除了时间戳),连seq也为0了,原因只有一个: move_base重启了,显然是出错了。

问题 2

查看move_base/status话题,发现这样一段

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
header:
seq: 88
stamp:
secs: 1709519925
nsecs: 385386549
frame_id: " "
status_list:
goal_id:
stamp:
secs: 1709519919
nsecs: 87035653
id: "/task_dispatcher-1-1709519919.87035653"
status: 6
text: "This goal has been accepted by the simple action server"


header:
seq: 89
stamp:
secs: 1709519925
nsecs: 408925409
frame_id:
status_list:
goal_id:
stamp:
secs: 1709519919
nsecs: 87035653
id: "/task_dispatcher-1-1709519919.87035653"
status: 2
text:

...... // 重复 status: 2 的消息

这段表示,seq: 88task_dispatcher节点发了goal,进入导航状态,当seq: 89时,同一个节点取消了goal,也就是调用了cancelGoal函数,因为status: 2


dmesg命令

dmesg可以在应用程序crash掉时,显示内核中保存的相关信息。通过dmesg命令可以查看发生段错误的程序名称、引起段错误发生的内存地址、指令指针地址、堆栈指针地址、错误代码、错误原因等. 就是在运行报了段错误之后,马上执行。

  1. 出现段错误时,首先应该想到段错误的定义,从它出发考虑引发错误的原因。

  2. 在使用指针时,定义了指针后记得初始化指针,在使用的时候记得判断是否为NULL。

  3. 在使用数组时,注意数组是否被初始化,数组下标是否越界,数组元素是否存在等。

  4. 在访问变量时,注意变量所占地址空间是否已经被程序释放掉。

  5. 在处理变量时,注意变量的格式控制是否合理等


导航时,暂停再继续后,接近终点时没有规划下一段全局路径
abstract Welcome to my blog, enter password to read.
Read more
如何用top查看每个进程的线程

程序中创建的线程(也称为轻量级进程,LWP)会具有和程序的PID相同的“线程组ID”。各个线程会获得其自身的线程ID

列出由进程号为的进程创建的所有线程

1
ps -T -p <pid>

SID栏表示线程ID,而CMD栏则显示了线程名称

还可以通过htop查看单个进程的线程,它是一个基于ncurses的交互进程查看器。该程序允许你在树状视图中监控单个独立线程。

开启htop,然后按<F2>进入htop的设置菜单。选择“设置”栏下面的“显示选项”,然后开启“树状视图”和“显示自定义线程名”选项。按<F10>退出设置。


设置python使用的版本
  1. 检查系统上可用的Python版本
1
2
$ ls /usr/bin/python*
/usr/bin/python2 /usr/bin/python2.7 /usr/bin/python3 /usr/bin/python3.8

查看是否配置了Python替代方案。

1
2
$ sudo update-alternatives --list python
update-alternatives: error: no alternatives for python

  1. 设置替代版本

对于本例,我们将设置两个Python替代:Python2和Python3。

1
2
$ sudo update-alternatives --install /usr/bin/python python /usr/bin/python2 1
$ sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 2

确认两个备选方案都可以使用:

1
2
3
$ sudo update-alternatives --list python
/usr/bin/python2
/usr/bin/python3

选择可选的Python版本

1
2
3
4
5
6
7
8
$ sudo update-alternatives --config python
There are 2 choices for the alternative python (providing /usr/bin/python).
Selection Path Priority Status
------------------------------------------------------------
* 0 /usr/bin/python3 2 auto mode
1 /usr/bin/python2 1 manual mode
2 /usr/bin/python3 2 manual mode
Press to keep the current choice[*], or type selection number: 1

输入您的选择。在本例中,选择1来选择python2

检查你的python版本:

1
2
$ python -V
Python 2.7.18rc1