存在两种常见的死锁情况,第一种是对同一把锁连续加锁,结果第二次加锁时因为自己已经加了锁,会导致线程阻塞无法运行。
第二种是两个线程各自加了一把锁之后,还未解锁就去获取对方的锁。
线程t1加了锁A,t2加了锁B,两个线程都未解锁,然后t1去获取锁B,此时t1会阻塞;同样t2去获取锁A,也会阻塞。两个线程都阻塞也就无法解锁,程序无法再运行。此时可以将两个线程都杀死。为防止这种情况可以用trylock函数。
boost::mutex::scoped_lock
顾名思义就是在作用域内有效,当离开作用域自动释放锁,传递参数是锁。构造函数会进行上锁操作,析构函数会对互斥量进行解锁操作。这样当锁离开作用域时,析构函数会自动释放锁。 即使运行时抛出异常,由于析构函数仍然会运行,所以锁仍然能释放。
scoped_lock
特别之处是,可以指定多个互斥量。
之前的mutex
的那篇文章,把两个线程可以修改如下1
2
3
4
5
6
7
8
9
10
11
12
13
14
15void 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;
}
不用mutex
的lock
和unlock
函数。
scoped_lock
使用std::lock
函数,其会调用一个特殊的算法对所提供的互斥量调用try_lock
函数,这是为了避免死锁。
C++17引入的std::scoped_lock
允许一次性锁住多个互斥量。可以传递多个互斥量给scoped_lock
的构造函数,它会自动锁住所有传递的互斥量,并且在scoped_lock
的生命周期结束时自动解锁,这样可以避免出现死锁,因为它会在一次性锁住所有互斥量时,自动避免死锁情况。
unique_lock
在构造时只能锁住一个互斥量。但与scoped_lock
不同的是,可以在后续的代码中手动解锁、重新锁住或者在不同的地方重新锁住另一个互斥量。这种灵活性有时可以用于更复杂的场景。
报错 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.h
的 SimpleActionServer<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
话题堪称调试利器,有必要专门分析一下。尤其goal_id
中的id
可以表示move_base
的客户端是什么,status
和text
表示导航状态
同类的move_base/result
只能在导航最终结束时,发布一个消息,成功或失败,使用价值不大,不分析了。 另一个move_base/feedback
和status
类似,但text
部分不能表现导航的最终状态。
move_base/status
话题不在常用的move_base
源码里,而是在actionlib
的服务端代码action_server_imp.h
的ActionServer<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
33status_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
导航时观察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
23header:
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
重启了,显然是出错了。
查看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
32header:
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: 88
时 task_dispatcher
节点发了goal,进入导航状态,当seq: 89
时,同一个节点取消了goal,也就是调用了cancelGoal
函数,因为status: 2
dmesg
可以在应用程序crash掉时,显示内核中保存的相关信息。通过dmesg命令可以查看发生段错误的程序名称、引起段错误发生的内存地址、指令指针地址、堆栈指针地址、错误代码、错误原因等. 就是在运行报了段错误之后,马上执行。
出现段错误时,首先应该想到段错误的定义,从它出发考虑引发错误的原因。
在使用指针时,定义了指针后记得初始化指针,在使用的时候记得判断是否为NULL。
在使用数组时,注意数组是否被初始化,数组下标是否越界,数组元素是否存在等。
在访问变量时,注意变量所占地址空间是否已经被程序释放掉。
在处理变量时,注意变量的格式控制是否合理等
程序中创建的线程(也称为轻量级进程,LWP)会具有和程序的PID相同的“线程组ID”。各个线程会获得其自身的线程ID
列出由进程号为1
ps -T -p <pid>
SID
栏表示线程ID,而CMD
栏则显示了线程名称
还可以通过htop查看单个进程的线程,它是一个基于ncurses的交互进程查看器。该程序允许你在树状视图中监控单个独立线程。
开启htop,然后按<F2>
进入htop的设置菜单。选择“设置”栏下面的“显示选项”,然后开启“树状视图”和“显示自定义线程名”选项。按<F10>
退出设置。
1 | $ ls /usr/bin/python* |
查看是否配置了Python替代方案。1
2$ sudo update-alternatives --list python
update-alternatives: error: no alternatives for python
对于本例,我们将设置两个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
无论传感器是雷达还是相机,都只能看到障碍物的一部分。如果障碍物比较大,而目标点在障碍物内(我们知道,机器人不知道),机器人会绕过它看到的障碍部分,前往目标点,然后它又看到障碍,于是接着绕行,这样会绕很久。目前只能设置参数planner_patience
,绕的时间太长时,进行Abort
写一个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
# 每隔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 ---------- "
donesleep 1 && kill -2 $!
是关键,sleep 1秒就杀死进程。 如果没有这句,机器人执行第一次导航时,必须等待Ctrl+C才能进入下一次导航
经过测试,向move_base发布目标点的频率如果太高,会报错,最快为0.6秒一次。 但是关掉rviz后,可达到0.5秒一次,可见优化程序会有效。
init.py的2140行
rospy client.py 251行
这里就涉及ROS源码了,想彻底优化就得着手ROS源码了,但是感觉没有必要。
旋转式的雷达寿命短,难过车规。 Livox雷达寿命更长,符合车规
按照说明书获得雷达和电脑应设置的静态IP。说明书指出可以只连接电源线,红正黑负。直流12V
可以去Livox的官网下载LivoxViewer2_Ubuntu_v2.3.0
,配置很简单。运行LivoxViewer2.sh
即可
先安装Livox-SDK2,然后安装livox_ros_driver2
修改MID360_config.json
把cmd_data_ip
改为电脑的静态IP,把lidar_configs
里的ip
改为雷达IP。 然后运行roslaunch livox_ros_driver2 rviz_MID360.launch
就可以看到结果了。
参考: 配置过程