基于激光雷达的行人检测与跟踪

duanzhuohua

前提：实现了激光雷达标定。（源码详见前一贴子），补充标定场景图片见附件。
扩展：（1）follow_me01对行人的检测并不是十分准确，修改其逻辑或设计新的行人检测算法（或更好的特征）来进行更精确的行人检测及跟踪。

（2）开发性扩展。查阅基于激光雷达的行人检测与跟踪的相关文献，对现有研究进行综述总结，实现并改进。

/*
* 节点followme01
   根据LaserScan消息判断行人，并跟随行人。主要步骤：
   (1)从scan话题订阅sensor_msgs/LaserScan消息
   (2)识别2m以内最近的行人（双脚在LaserScan中的特征，假设行人不与其它
              物体（如座椅、墙壁等）相连且有一定距离，且只考虑一个
              人在机器人周围的情况，则属于人的一只脚的扫描点应该
              为相邻的射线，且这些射线测得的距离应该较为接近）
    (3)识别出在2m内存在单个行人后，计算该行人与机器人的距离以及相对于
       机器人的方位。设根据第2步，得到行人在第r1射线-r2射线，
       取r=(r1+r2)/2，设根据laserscan_calib01标定出机器人正前方为第h射线
       则，如果r<h,则行人在机器人右边；如果r==h,则行人在机器人前方，
       如果r>h，则行人在机器人左边。设两条相邻射线的角度为b,则行人在机器人的
       (r-h)b方向。另外，设第r条射线测量的距离为d(为行人相对于机器人的距离)
    （4）控制机器人跟随行人：转向(r-h)b弧度，然后前进(d-0.3)米（d>=0.3为前提
         0.3为设定的安全距离阈值），设计指令序列，向/cmd_vel话题发布
* 开发者：段琢华,duanzhuohua@163.com 2022-10-25
*/

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "sensor_msgs/LaserScan.h"
//申明sensor_msgs:aserScan消息类型
#include <cmath>
#include <vector>
const double PI = 3.1415926535898;

const int ahead_beam_index=151;//机器人前方的射线编号（根据laserscan_calib01标定获得)

int is_in_judge=0;//1: person judge processing

int scan_available=0;
double linear_speed=0.2,angular_speed=PI/6;

double safe_dist;//安全距离阈值，机器人(激光雷达)与人的最短距离>=0.2m
double follow_dist;//跟踪行人的最远距离0.5m~5m之间,该距离之外的行人不做跟踪

ros::Rate *loop_rate;
ros:ublisher vel_pub;
geometry_msgs::Twist cmd_vel;
sensor_msgs:aserScan cur_scan;//最新的LaserScan消息

void scanCallback(const sensor_msgs:aserScan::ConstPtr& scan_msg)
{
  //回调函数，接收sensor_msgs:aserScan消息
  scan_available=1;
  if (is_in_judge==0) cur_scan=*scan_msg;  
}

void goahead(double distance){
//前进distance米
  double dist_togo=distance;//还需要走的距离
  cmd_vel.angular.z=0;
  while(ros:k() && dist_togo>linear_speed)
  {
    cmd_vel.linear.x = linear_speed;
    vel_pub.publish(cmd_vel);
    dist_togo-=linear_speed;
    loop_rate->sleep();
  }
  cmd_vel.linear.x = dist_togo;
  vel_pub.publish(cmd_vel);
  loop_rate->sleep();
}

void turn(double angle)
{
  //旋转angle弧度  
  cmd_vel.linear.x=0;
  double angle_toturn=angle;
  if (angle>0) {//向左转
    while(ros:k() && angle_toturn>angular_speed)
    {
      cmd_vel.angular.z = angular_speed;
      vel_pub.publish(cmd_vel);
      angle_toturn=angle_toturn-angular_speed;
      loop_rate->sleep();
    }
    cmd_vel.angular.z = angle_toturn;
    vel_pub.publish(cmd_vel);
    loop_rate->sleep();
  } else
  { //向右转
    while(ros:k() && angle_toturn<-angular_speed)
    {
      cmd_vel.angular.z = -angular_speed;
      vel_pub.publish(cmd_vel);
      angle_toturn=angle_toturn+angular_speed;
      loop_rate->sleep();
    }
    cmd_vel.angular.z = angle_toturn;
    vel_pub.publish(cmd_vel);
    loop_rate->sleep();
  }

}

int person_judge(double& ang, double& dist)
{//返回1,2：有行人，行人相对于机器人的方位和距离写入形式参数ang,dist中
//注意：形式参数ang,dist的传递方式为按引用传递，因此可以改变实际参数的值

NXROS小助理

沙发！！

航智向晶磊

厉害呀

杨晨佳

学习新知识！