rikirobot小车使用激光雷达RPLIDAR-A2进行避障

硬件平台

rikirobot小车（包含stm32电机驱动板，工控机，电源和RPLIDAR-A2）

实现任务

使用激光雷达RPLIDAR-A2测量障碍物到rikirobot小车的距离，根据左右距离信息，确定rikirobot小车的转向，通过ROS发布/cmd_vel消息控制小车的差速转向运动

实现思路

1）激光雷达测距角度设置（将采集数据由0-360°设置为-60°-60°）
2) 控制rikirobot小车向前运动（通过ROS不断给/cmd_vel发送x方向线速度）
3）将前两步结果合并为一个文件，联合调试，使得rikirobot小车实现避障功能

实现步骤

1）激光雷达测距角度设置：在RPLIDAR-A2驱动里面直接修改，添加测距内容

#include “ros/ros.h”
#include “sensor_msgs/LaserScan.h”

#define RAD2DEG(x) ((x)*180./M_PI)

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
int count = scan->scan_time / scan->time_increment;
ROS_INFO(“I heard a laser scan %s[%d]:”, scan->header.frame_id.c_str(), count);
ROS_INFO(“angle_range, %f, %f”, RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));

for(int i = 0; i < count; i++) {
    float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
    if(degree >=-60 && degree <=60) //避光雷达角度设置：-60°-60°，并输出显示scan->angle_increment和count
	ROS_INFO(": [%f, %f, %f, %d]", degree, scan->ranges[i], scan->angle_increment, count);
    else 
	continue;
}

}

int main(int argc, char **argv)
{
ros::init(argc, argv, “rplidar_node_client”);
ros::NodeHandle n;

ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback);

ros::spin();

return 0;

}
2）控制rikirobot小车向前运动：
（1）rostopic pub -1 /cmd_vel geometry_msgs/Twist – ‘[2.0,0.0,0.0]’ ‘[0.0,0.0,0.0]’ 控制小车以2.0m/s速度向前运动，仅发送一次指令；
（2）rostopic pub -r 10 /cmd_vel geometry_msgs/Twist – ‘[2.0,0.0,0.0]’ ‘[0.0,0.0,0.0]’ 控制小车以2.0m/s速度向前运动，以10HZ速率发送指令；
（3）编写节点，向小车以恒定速率发送指令。
#include “ros/ros.h”
#include “geometry_msgs/Twist.h”
#include <stdlib.h>
int main(int argc,char **argv)
{
ros::init(argc,argv,“cmd_node”);
ros::NodeHandle n;
ros::Publisher cmd_pub=n.advertise<geometry_msgs::Twist>("/cmd_vel",1000);
ros::Rate loop_rate(10);
while(ros::ok())
{
geometry_msgs::Twist msg;
//msg.linear.x=10;
msg.linear.x=(double)(rand()/(double(RAND_MAX)));
msg.angular.z=(double)(rand()/(double(RAND_MAX)));
cmd_pub.publish(msg);
ROS_INFO(“msg.linear.x:%f , msg.angular.z:%f”,msg.linear.x,msg.angular.z);
loop_rate.sleep();
}
return 0;
}
3）将（1）和（2）的节点合并，并增加控制策略，采用最简单的c代码实现
#include “ros/ros.h”
#include “sensor_msgs/LaserScan.h”
#include “geometry_msgs/Twist.h”
#include <stdlib.h>
#include <stdio.h>
#define RAD2DEG(x) ((x)*180./M_PI)

int stopFlag=0;
int turnFlag=0;
float distMinLeft=0.0;
float distMinRight=0.0;
void miniAngleCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
int size = scan->scan_time / scan->time_increment;
int minIndexLeft = 0;
int minIndexRight = size/2;
for(int i=479; i<size/2; i++)
{
if (scan->ranges[i] < scan->ranges[minIndexRight] && scan->ranges[i] > 0.15){
minIndexRight = i;
}
}
for (int i = size/2; i <959 ; i++)
{
if (scan->ranges[i] < scan->ranges[minIndexLeft] && scan->ranges[i] > 0.15){
minIndexLeft = i;
}
}
distMinLeft = scan->ranges[minIndexLeft];
distMinRight = scan->ranges[minIndexRight];
}
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
int count = scan->scan_time / scan->time_increment;

ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count);
ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));

for(int i = 0; i < count; i++) {
    float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
    if(degree >=-30 && degree <=30 ){
    ROS_INFO(": [%f, %f]", degree, scan->ranges[i]);
    if (scan->ranges[i] <= 0.8)
        {
	    stopFlag = 1;
        }
        else    stopFlag = 0;
}
    else 
    {
    continue;
    }
    miniAngleCallback(scan);
if(stopFlag == 1 && distMinLeft > distMinRight)
    {
                turnFlag = 1;//turn left;
    }
    else 
        {       
	    turnFlag = 0;//turn right;
    }

}

}

int main(int argc, char **argv)
{
ros::init(argc, argv, “rplidar_node_client”);
ros::NodeHandle n;

ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 1000, scanCallback);

ros::Publisher pub=n.advertise<geometry_msgs::Twist>("/cmd_vel",1000);
ros::Rate loop_rate(15);

while(ros::ok())
{
ros::spinOnce();
geometry_msgs::Twist msg;
msg.linear.x=0.3;
//msg.linear.x=(double)(rand()/(double(RAND_MAX)));
//msg.angular.z=(double)(rand()/(double(RAND_MAX)));
if(stopFlag == 1 && turnFlag==1)  // ture left
{
    msg.linear.x= 0;
    msg.angular.z=2;
}
else if(stopFlag == 1 && turnFlag ==0)  // ture right
{
msg.linear.x= 0;
    msg.angular.z=-2;
}
else if (stopFlag == 0)
{
msg.linear.x=0.3;
}
pub.publish(msg);
ROS_INFO("msg.linear.x:%f ,  msg.angular.z:%f",msg.linear.x,msg.angular.z);
loop_rate.sleep();
} 

return 0;
}

参考链接

链接: https://blog.csdn.net/w_tom/article/details/78571746.