保存的bag在断电重启后大小变为0
abstract Welcome to my blog, enter password to read.
Read more
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
死锁和scoped_lock

死锁

存在两种常见的死锁情况,第一种是对同一把锁连续加锁,结果第二次加锁时因为自己已经加了锁,会导致线程阻塞无法运行。

第二种是两个线程各自加了一把锁之后,还未解锁就去获取对方的锁。

线程t1加了锁A,t2加了锁B,两个线程都未解锁,然后t1去获取锁B,此时t1会阻塞;同样t2去获取锁A,也会阻塞。两个线程都阻塞也就无法解锁,程序无法再运行。此时可以将两个线程都杀死。为防止这种情况可以用trylock函数。

scoped_lock

boost::mutex::scoped_lock顾名思义就是在作用域内有效,当离开作用域自动释放锁,传递参数是锁。构造函数会进行上锁操作,析构函数会对互斥量进行解锁操作。这样当锁离开作用域时,析构函数会自动释放锁。 即使运行时抛出异常,由于析构函数仍然会运行,所以锁仍然能释放。

scoped_lock特别之处是,可以指定多个互斥量。

之前的mutex的那篇文章,把两个线程可以修改如下

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
void thread_1(int n)
{
boost::mutex::scoped_lock lock_1(mut);
cout << "start thread 1" << endl;
sum = sum * n;
cout << "thread 1, sum: " << sum << endl;
}

void thread_2(int n)
{
boost::mutex::scoped_lock lock_2(mut);
cout << "start thread 2" << endl;
sum = sum * 7 * n;
cout << "thread 2, sum: " << sum << endl;
}

不用mutexlockunlock函数。

scoped_lock使用std::lock函数,其会调用一个特殊的算法对所提供的互斥量调用try_lock函数,这是为了避免死锁。

C++17引入的std::scoped_lock允许一次性锁住多个互斥量。可以传递多个互斥量给scoped_lock的构造函数,它会自动锁住所有传递的互斥量,并且在scoped_lock的生命周期结束时自动解锁,这样可以避免出现死锁,因为它会在一次性锁住所有互斥量时,自动避免死锁情况。

unique_lock在构造时只能锁住一个互斥量。但与scoped_lock不同的是,可以在后续的代码中手动解锁、重新锁住或者在不同的地方重新锁住另一个互斥量。这种灵活性有时可以用于更复杂的场景。


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