2.使用STDR進行ROS小車超聲波避障策略模擬實驗

阿新 • • 發佈：2018-12-14

轉自連結：http://www.corvin.cn/573.html

0x00 什麼是模擬實驗

在沒有真實物理實體實驗裝置的情況下，通過在計算機上用軟體模擬真實裝置執行的效果，這就叫做軟體模擬實驗，用軟體模擬是一條可行性高、測試成本低、實驗效率高的一條路。就拿ROS小車的避障策略設計來說，如果我們設計了一個避障策略想要實驗一下效果如何的話，一般都需要先做一個真實的ROS小車，這樣就需要花費不少的錢。當在真實的小車上實驗時還會遇到小車電池電量不夠、執行時間短、測試環境不夠完善等各種問題，有時為了測試一次避障策略程式就需要準備好長時間使小車處於正常工作狀態才能開始實驗，費時費力。

如果我們使用軟體模擬出真實小車的各種引數、各種運動狀態的話，我們將自己的程式在模擬中跑起來就方便多了，STDR就是這麼一款可以模擬ROS小車在二維平面移動的軟體，模擬軟體中的小車上感測器釋出的資料跟真實的小車釋出的話題資料基本上是一樣的，那麼我們的避障策略程式就可以很好的在模擬軟體上進行實驗修改完善，當在模擬環境中沒有問題的話，最後再在真實的小車上執行，這樣就會有事半功倍的效果。

0x01 模擬小車介紹

當使用如下命令來啟動STDR模擬軟體時會自動的載入一個ROS模擬差速驅動小車，我們可以把它當作跟turtlebot一樣的小車即可，只有線速度x和角速度z：

roslaunch stdr_launchers server_with_map_and_gui_plus_robot.launch

Screenshot from 2018-03-29 13:38:33.png

使用如下命令即可在rviz中開啟該模擬軟體，稍微配置一下即可得到如下圖所示的小車超聲波感測器探測範圍：

roslaunch stdr_launchers rviz.launch

Screenshot from 2018-03-29 14:15:20.png

Screenshot from 2018-03-29 14:11:14.png

0x02 設計超聲波避障策略

由於該ROS機器人上面裝有５個超聲波感測器，為了在本次實驗中減輕設計策略的複雜度我們只使用機器人前面的三個感測器，分別是正前方、左側、右側感測器，下面的流程圖是設計的使用三個超聲波感測器來避障的程式設計流程圖（該避障策略僅供參考，只是為了實驗需要而設計的，在真實的環境中我們可能需要設計更為複雜完善的避障策略），如果下圖看不清楚的話可以檢視以下附件：

超聲波避障策略.jpeg

0x03 熟悉超聲波話題及其訊息格式

首先來認識一下超聲波話題和訊息格式，在首先啟動了模擬器後接下來通過如下圖所示命令來操作：

Screenshot from 2018-03-29 16:44:52.png

對於sensor_msgs/Range.msg的具體詳細描述可以從roswiki的官網獲得，網址如下：

http://mirror.umd.edu/roswiki/doc/diamondback/api/sensor_msgs/html/msg/Range.html

Screenshot from 2018-03-29 17:28:54.png

輸出當前超聲波話題內容,可以看到與定義的格式內容是匹配的:

Screenshot from 2018-03-29 17:23:13.png

0x04 獲取一個超聲波的測距輸出

我們先以正前方的超聲波sonar_0來演示如何編寫程式碼獲取其測距輸出值,我們首先建立自己的超聲波避障軟體包ultrasonic_obstacle_avoidance,在工作空間的src目錄下使用如下命令來建立:

catkin_create_pkg ultrasonic_obstacle_avoidance roscpp sensor_msgs

Screenshot from 2018-03-29 18:08:09.png

接下來進入剛才建立的軟體包下的src目錄下建立main.cpp,輸入如下程式碼,該程式碼可以讀取robot0上的sonar_0的測距值並列印輸出:

/**************************************************************

*Copyright(C): ROS小課堂 www.corvin.cn

*FileName: main.cpp

*Author: corvin

*Date: 20180330

*Description:在STDR上進行超聲波避障策略模擬,實驗程式碼.

*

**************************************************************/

#include "ros/ros.h"

#include "sensor_msgs/Range.h"

void sonar0_callback(const sensor_msgs::Range::ConstPtr& msg)

{

ROS_INFO("Sonar0 range:[%f]", msg->range);

}

int main(int argc, char **argv)

{

ros::init(argc, argv, "ultrasonic_obstacle_avoidance_node");

ros::NodeHandle handle;

ros::Subscriber sub_sonar0 = handle.subscribe("/robot0/sonar_0", 100, sonar0_callback);

ros::spin();

return 0;

}

然後修改CMakeLists.txt配置檔案,按照如下圖所示修改:

Screenshot from 2018-03-30 08:51:22.png

最後就可以來編譯該程式碼了,在catkin_ws目錄下執行catkin_make編譯,然後source devel/setup.bash配置好環境變數,如下操作:

[email protected]:~/catkin_ws$ catkin_make

[email protected]:~/catkin_ws$ source devel/setup.bash

編譯完成後就可以來執行檢視效果了,首先啟動STDR模擬軟體,然後在啟動列印超聲波資料的節點,分別在兩個終端下執行如下兩條命令:

[email protected]:~/catkin_ws$ roslaunch stdr_launchers server_with_map_and_gui_plus_robot.launch

[email protected]:~/catkin_ws$ rosrun ultrasonic_obstacle_avoidance ultrasonic_obstacle_avoidance_node

當啟動節點後可以發現已經有列印資料,當我們使用鍵盤來遙控小車四處走動時可以發現該資料也跟著變動,啟動鍵盤遙控節點命令如下:

[email protected]:~$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

0x05 實現完整避障策略進行模擬

現在已經可以獲取單個超聲波感測器的輸出了,接下來我們就按照前面的程式設計流程圖來獲取其他感測器輸出實現完整的避障程式碼,完整程式碼如下所示:

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

/**************************************************************

*Copyright(C): ROS小課堂 www.corvin.cn

*FileName: main.cpp

*Author: corvin

*Date: 20180330

*Description:在STDR上進行超聲波避障策略模擬,實驗程式碼.

*

**************************************************************/

#include "ros/ros.h"

#include "sensor_msgs/Range.h"

#include "geometry_msgs/Twist.h"

#define setbit(x, y) x|=(1<<y)

#define clrbit(x, y) x&=~(1<<y)

//low three bit as sonar warn flag

// left font right

// x x x x x 0 0 0

#define STATUS_A 0x04 // v x x

#define STATUS_B 0x02 // x v x

#define STATUS_C 0x01 // x x v

#define STATUS_D 0x07 // v v v

#define STATUS_E 0x06 // v v x

#define STATUS_F 0x03 // x v v

#define STATUS_G 0x05 // v x v

//global variable

geometry_msgs::Twist twist_cmd;

ros::Publisher twist_pub;

const double warn_range = 0.5; //warn check distance

double default_period_hz = 10; //hz

double default_linear_x = 0.5; // (m/s)

double default_yaw_rate = 0.5; // rad/s

double range_array[3]; //save three sonar value

void sonar0_callback(const sensor_msgs::Range::ConstPtr& msg)

{

ROS_INFO("front Sonar0 range:[%f]", msg->range);

range_array[1] = msg->range;

}

void sonar1_callback(const sensor_msgs::Range::ConstPtr& msg)

{

ROS_INFO("left Sonar1 range:[%f]", msg->range);

range_array[0] = msg->range;

}

void sonar2_callback(const sensor_msgs::Range::ConstPtr& msg)

{

ROS_INFO("right Sonar2 range:[%f]", msg->range);

range_array[2] = msg->range;

}

void publishTwistCmd(double linear_x, double angular_z)

{

twist_cmd.linear.x = linear_x;

twist_cmd.linear.y = 0.0;

twist_cmd.linear.z = 0.0;

twist_cmd.angular.x = 0.0;

twist_cmd.angular.y = 0.0;

twist_cmd.angular.z = angular_z;

twist_pub.publish(twist_cmd);

}

void checkSonarRange(double sonar_l, double sonar_f, double sonar_r)

{

unsigned char flag = 0;

if(sonar_l < warn_range)

{

setbit(flag, 2);

}

else

{

clrbit(flag, 2);

}

if(sonar_f < warn_range)

{

setbit(flag, 1);

}

else

{

clrbit(flag, 1);

}

if(sonar_r < warn_range)

{

setbit(flag, 0);

}

else

{

clrbit(flag, 0);

}

ROS_INFO("CheckSonarRange get status:0x%x", flag);

switch(flag)

{

case STATUS_A: //turn right 0x04

ROS_WARN("left warn,turn right");

publishTwistCmd(0, -default_yaw_rate);

break;

case STATUS_B: // 0x02

ROS_WARN("front warn, left and right ok, compare left and right value to turn");

if(sonar_l > sonar_r)

{

ROS_WARN("turn left");

publishTwistCmd(0, default_yaw_rate);

}

else

{

ROS_WARN("turn right");

publishTwistCmd(0, -default_yaw_rate);

}

break;

case STATUS_C: //turn left

ROS_WARN("left ok, front ok, right warn, turn left");

publishTwistCmd(0, default_yaw_rate);

break;

case STATUS_D:

ROS_WARN("left,front,right all warn, turn back");

publishTwistCmd(0, 10*default_yaw_rate);

break;

case STATUS_E:

ROS_WARN("left warn, front warn, right ok, turn right");

publishTwistCmd(0, (-default_yaw_rate*2));

break;

case STATUS_F:

ROS_WARN("left ok, front warn, right warn, turn left");

publishTwistCmd(0, (default_yaw_rate*2));

break;

case STATUS_G:

ROS_WARN("left and right warn, front ok, speed up");

publishTwistCmd(2*default_linear_x, 0);

break;

default: //go forward straight line

publishTwistCmd(default_linear_x, 0);

break;

}

int main(int argc, char **argv)

{

ros::init(argc, argv, "ultrasonic_obstacle_avoidance_node");

ros::NodeHandle handle;

ros::Rate loop_rate = default_period_hz;

ros::Subscriber sub_sonar0 = handle.subscribe("/robot0/sonar_0", 100, sonar0_callback);

ros::Subscriber sub_sonar1 = handle.subscribe("/robot0/sonar_1", 100, sonar1_callback);

ros::Subscriber sub_sonar2 = handle.subscribe("/robot0/sonar_2", 100, sonar2_callback);

twist_pub = handle.advertise<geometry_msgs::Twist>("/robot0/cmd_vel", 10);

while(ros::ok())

{

checkSonarRange(range_array[0], range_array[1], range_array[2]);

ros::spinOnce();

loop_rate.sleep();

}

return 0;

}

在以上原始碼中需要注意以下幾點,方便大家更容易理解程式碼:

(1)使用unsigned char的變數的低3bit來記錄三個超聲波感測器測距值是否到達警戒值,高5bit不使用,如果我們有更多的超聲波的話就可以用上了.對於低3bit來說,為了方便大家更容易的與三個位置的超聲波感測器相對應,因此就規定3bit中最高位存放左邊超聲波感測器的報警標誌,中間bit放置正前方超聲波感測器報警標誌,最低bit放置右邊超聲波感測器報警標誌,如下所示:

(高5bit不儲存任何標誌) (左邊) (正前方) (右邊)

X X X X X 0 0 0

(2)使用一個double陣列range_array來儲存三個超聲波感測器每次回調回來的測距值,同樣的為了方便與三個超聲波位置匹配,在range_array[0]儲存的是左邊sonar的測距值,range_array[1]儲存的是正前方sonar的測距值,range_array[2]儲存的是右邊sonar的測距值.

下面準備編譯該程式碼,由於我們新增了控制小車移動的訊息geometry_msgs/Twist.msg格式,因此需要相應的修改CMakeLists.txt和package.xml檔案,首先是修改CMakeLists.txt檔案,按照如下圖修改即可:

Screenshot from 2018-03-30 14:32:08.png

修改package.xml檔案,按照如下圖所示修改:

Screenshot from 2018-03-30 14:35:35.png

接下來就可以來重新編譯程式碼了,編譯完成後重新啟動STDR模擬器,然後在啟動我們的避障節點程式來檢視效果:

通過上面的模擬效果來看,我們的機器人在三個超聲波測距模組下基本上可以進行自由的避障行走,但是模擬中難免也會遇到其他問題,像下圖這種振盪判斷,會導致機器人一直在一個地方左右搖擺:

其實在上面出現的這個問題是由於我們的程式碼設計有問題,我們通過模擬就可以模擬出該效果,省了我們在真實的小車上來測試效果了,那下面來說出現該現象的原因:

(1)當小車正前方的超聲波測距顯示距離太近時,會來判斷左右兩邊的超聲波測距值哪個更大一點,哪邊大一點就說明那邊肯定更空一點.

(2)例如我們判斷左側超聲波資料大點,那就往左邊轉動,但是剛轉過去一點點,在ROS的主迴圈中又開始下一輪判斷超聲波資料了.

(3)由於剛才是往左邊轉了一點,這樣就導致右邊的資料肯定就比左邊的又大了,那在本輪的避障中又控制小車往右邊轉動了.

(4)這樣就會出現上面動圖出現的現象了,小車在不停的左右擺動.

對於出現這種現象既然我們已經知道了產生的原因,那麼就需要來找到解決辦法來消滅掉這個bug,大家可以在上面提供的原始碼基礎上來修改完善,算是作為大家的課後習題了,而且第一位小夥伴解決了該問題並在公眾號中給我發訊息留言的話,就會有獎勵啊,大家來開動腦子想想更好的避障策略來遍歷整個房間且不會撞到牆上吧,有了這個STDR就可以很容易的驗證大家的避障策略是否可行了.

0x06 參考資料

[1].STDR在ROS WiKi上主頁地址[OL]. http://wiki.ros.org/stdr_simulator/Tutorials

[2].sensor_msgs/Range.msg在ROS WiKi網站上的定義[OL]. http://mirror.umd.edu/roswiki/doc/diamondback/api/sensor_msgs/html/msg/Range.html

[3].evarobot_sonar讀取超聲波資料的例項教程[OL]. http://wiki.ros.org/evarobot_sonar/Tutorials/indigo/Writing%20a%20Simple%20Subscriber%20for%20Sonar%20Sensor

2.使用STDR進行ROS小車超聲波避障策略模擬實驗

2.使用STDR進行ROS小車超聲波避障策略模擬實驗

Arduino小程式——超聲波避障遙控小車

嵌入式實訓之智慧小車超聲波壁障

ROS（九）- moveit使用C++介面進行運動路徑規劃和避障.cpp

STM32循跡避障小車製作程式碼詳解（簡單實現版）

實現紅外避障功能小車（軟體：mixly）

ros小車除錯經驗2

ASP.Net Core項目在Mac上使用Entity Framework Core 2.0進行遷移可能會遇到的一個問題.

使用JTAG調試器和Freemaster 2.0 進行powerpc架構的mpc5XXX系列的調試

Dynamic Window Approach_機器人區域性避障的動態視窗法

機器人避障研究現狀

HRG(2)——IntelRealSense for ROS

體驗Unity2017.2.0f3進行Vuforia開發

2.建立一個ROS功能包

讓無人環衛車自主移動避障的祕密——高仙SLA

JUC之CyclicBarrier迴圈避障

在linux上使用c++實現http/2協議進行通訊

Sqoop1.4.5+hadoop2.2.0進行Mysql到HDFS的資料轉換

使用jave1.0.2.jar進行音視訊轉碼

使用FFMPEG3.4.2版本進行視訊的解碼為YUV格式

2.使用STDR進行ROS小車超聲波避障策略模擬實驗

相關推薦