如何精确走拐角
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
如何用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


move_base的缺陷

TEB避障的实际情况
无论传感器是雷达还是相机,都只能看到障碍物的一部分。如果障碍物比较大,而目标点在障碍物内(我们知道,机器人不知道),机器人会绕过它看到的障碍部分,前往目标点,然后它又看到障碍,于是接着绕行,这样会绕很久。目前只能设置参数planner_patience,绕的时间太长时,进行Abort

向move_base发布目标的频率太高,会出错

写一个loop程序,高频率循环发两个目标点,连C++程序都不用,写bash脚本即可

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
44
#!/bin/bash

# 每隔1秒就发布下一个目标点
while true; do

rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped "header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: 'map'
pose:
position:
x: 20
y: 7
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 1
w: 0" & sleep 1 && kill -2 $!

# -2 是用信号量表示的 $!是子进程号 相当于执行ctrl+C

rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped "header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: 'map'
pose:
position:
x: 24
y: 7
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0
w: 1" & sleep 1 && kill -2 $!

sleep 1
echo " ---------- start next loop ---------- "
done

sleep 1 && kill -2 $!是关键,sleep 1秒就杀死进程。 如果没有这句,机器人执行第一次导航时,必须等待Ctrl+C才能进入下一次导航

经过测试,向move_base发布目标点的频率如果太高,会报错,最快为0.6秒一次。 但是关掉rviz后,可达到0.5秒一次,可见优化程序会有效。

init.py的2140行
rospy client.py 251行
这里就涉及ROS源码了,想彻底优化就得着手ROS源码了,但是感觉没有必要。


Livox-MID360

旋转式的雷达寿命短,难过车规。 Livox雷达寿命更长,符合车规

按照说明书获得雷达和电脑应设置的静态IP。说明书指出可以只连接电源线,红正黑负。直流12V

可以去Livox的官网下载LivoxViewer2_Ubuntu_v2.3.0,配置很简单。运行LivoxViewer2.sh即可

先安装Livox-SDK2,然后安装livox_ros_driver2

修改MID360_config.jsoncmd_data_ip改为电脑的静态IP,把lidar_configs里的ip改为雷达IP。 然后运行roslaunch livox_ros_driver2 rviz_MID360.launch就可以看到结果了。

参考: 配置过程