自主导航系列4-障碍的探测
1，安装turtlebot3
因为turtlebot3有激光雷达，很简单的操作几条命令
cd ~/catkin_ws/src git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git git clone https://github.com/ROBOTIS-GIT/turtlebot3.git git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git cd ~/catkin_ws rosdep install --from-paths src -i -y catkin_make echo "export TURTLEBOT3_MODEL=burger" >> ~/.bashrc source ~/.bashrc
关于激光雷达的信息，自带的参数为
<link name="base_scan"> <visual> <origin xyz="0 0 0.0" rpy="0 0 0"/> <geometry> <mesh filename="package://turtlebot3_description/meshes/sensors/lds.stl" scale="0.001 0.001 0.001"/> </geometry> <material name="dark"/> </visual> <collision> <origin xyz="0.015 0 -0.0065" rpy="0 0 0"/> <geometry> <cylinder length="0.0315" radius="0.055"/> </geometry> </collision> <inertial> <mass value="0.114" /> <origin xyz="0 0 0" /> <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" /> </inertial> </link>
/scan的参数如下
header: seq: 7 stamp: secs: 1091 nsecs: 646000000 frame_id: "base_scan" angle_min: 0.0 angle_max: 6.28318977356 angle_increment: 0.0175019223243 time_increment: 0.0 scan_time: 0.0 range_min: 0.119999997318 range_max: 3.5 ranges:
我试图修改它的range_max以满足仿真的要求但是修改了.stl文件
2，探测障碍
/*****************************************************
* @author:jianzhuozhu
* E-mail:sz170320221@163.com
* Date:2020/7/19
* Description:提取激光雷达的/scan信息，用于发布关于小车前后左右的墙体距离信息，
* 这里使用的激光雷达是rpridar(range_max: 3.5),若使用range更大
* 激光雷达，则MAX_DISTANCE需要修改
*/
#include "ros/ros.h"
#include <math.h>
#include "sensor_msgs/LaserScan.h"
#include "std_msgs/Int16.h"
#include "std_msgs/Bool.h"
#include "std_msgs/Float64MultiArray.h"
#include <vector>
#define delta_angle 0.1745 //墙壁判断角度,取+-10
#define PI 3.141592654
#define MAX_DISTANCE 3 //最大扫描距离
struct obstacle_judge
{
int flag; //是否有
float distance; //距离
float wall_angle; //墙的夹角
float direction; //相对小车的方向
};
ros::Publisher pub;
int laser_count=0,throttle_scans=1;
int obstacles_num; //障碍物点数
int front_ang = 0;
obstacle_judge front = {0,0,0,0};
obstacle_judge back = {0,0,0,PI};
obstacle_judge left = {0,0,0,PI*0.5};
obstacle_judge right = {0,0,0,PI*1.5};
void scanCallback(const sensor_msgs::LaserScan msg)
{
laser_count++; //雷达数据帧数
if((laser_count%throttle_scans)!=0) //依据采样频率throttle_scans进行采样
{
return;
}
float current_point_angle,current_point_distance;
int origin_scan_len=msg.ranges.size();
int cout_num_f = 0;float diatance_sum_f = 0; //用于统计
int cout_num_b = 0;float diatance_sum_b = 0; //用于统计
int cout_num_l = 0;float diatance_sum_l = 0; //用于统计
int cout_num_r = 0;float diatance_sum_r = 0; //用于统计
for(int i=0;i<origin_scan_len;i++)
{
current_point_angle=msg.angle_min+i*msg.angle_increment; //获取当前点当角度值
current_point_distance=msg.ranges[i]; //获取当前点的距离值
// printf("current_point_distance %f\n",current_point_distance);
if(current_point_distance<3) //剔除探测不到的点
{
if(((current_point_angle>=front.direction)&&(current_point_angle<=(front.direction+delta_angle)))||((current_point_angle>=(front.direction-delta_angle))&&(current_point_angle<=(front.direction+PI*2))))//角度满足一定条件
{
cout_num_f++; diatance_sum_f=diatance_sum_f+current_point_distance;
}
if((current_point_angle>=(back.direction-delta_angle))&&(current_point_angle<=(back.direction+delta_angle)))//角度满足一定条件
{
cout_num_b++; diatance_sum_b=diatance_sum_b+current_point_distance;
}
if((current_point_angle>=(left.direction-delta_angle))&&(current_point_angle<=(left.direction+delta_angle)))//角度满足一定条件
{
cout_num_l++; diatance_sum_l=diatance_sum_l+current_point_distance;
}
if((current_point_angle>=(right.direction-delta_angle))&&(current_point_angle<=(right.direction+delta_angle)))//角度满足一定条件
{
cout_num_r++; diatance_sum_r=diatance_sum_r+current_point_distance;
}
}
}
// printf("diatance_sum_f %f\n",diatance_sum_b);
// printf("cout_num_b=f %d\n",cout_num_b);
//计算障碍的距离
front.distance = diatance_sum_f/cout_num_f;
back.distance = diatance_sum_b/cout_num_b;
left.distance = diatance_sum_l/cout_num_l;
right.distance = diatance_sum_r/cout_num_r;
//初始化
diatance_sum_f = 0;cout_num_f = 0;
diatance_sum_b = 0;cout_num_b = 0;
diatance_sum_l = 0;cout_num_l = 0;
diatance_sum_r = 0;cout_num_r = 0;
printf("\033[40;34mlaser_count:\033[0m");printf(" %d\n",laser_count);
if(front.distance<MAX_DISTANCE){printf("front.distance %f\n",front.distance);}else{printf("front no obstacle\n");}
if(back.distance<MAX_DISTANCE){printf("back.distance %f\n",back.distance);}else{printf("back no obstacle\n");}
if(left.distance<MAX_DISTANCE){printf("left.distance %f\n",left.distance);}else{printf("left no obstacle\n");}
if(right.distance<MAX_DISTANCE){printf("right.distance %f\n",right.distance);}else{printf("right no obstacle\n");}
std::vector<double> array_test(4);
array_test[0]=front.distance;
array_test[1]=back.distance;
array_test[2]=left.distance;
array_test[3]=right.distance;
std_msgs::Float64MultiArray obs;
obs.data = array_test;
pub.publish(obs);
}
int main(int argc,char** argv)
{
ros::init(argc, argv, "scan_control");
ros::NodeHandle n;
ROS_WARN("****angle_min: 0.0 angle_max: 6.28318977356 angle_increment: 0.0175019223243");
ROS_WARN("****range_min: 0.119999997318 range_max: 3.5");
ROS_WARN("****front.direction=%f,back.direction=%f,left.direction=%f,right.direction=%f",front.direction,back.direction,left.direction,right.direction);
pub = n.advertise<std_msgs::Float64MultiArray>("/obs", 1000);
ros::Subscriber origin_scan_data = n.subscribe("/scan", 1, scanCallback);
ros::Rate loop_rate(10); //设置障碍物状态发布频率
ros::spin();
return 0;
}
有趣的冷知识：
roslaunch fira_maze maze_1_world.launch rosrun fira_maze maze_explorer.py
turtlebot3自带了一个导航包，但是用py写的，跑了，效果不太行
扫一扫
805
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
