ROS仿真中 读取 turtlebot3 扫描雷达传感器数据 求 前方距离和最短距离（对应方向）

获取传感器数据 以 scan为例

启动 waffle 的时候

  <arg name="model" default="waffle"/>

模型上有 激光扫描器 和 深度相机的地方

查看活跃的 topic

有深度相机的 图像 rgb相机的图像 激光
/camera/depth/image_raw
/camera/rgb/image_raw
/scan

tuttlebot3_world.launch 里 启动的是 这个 xacro文件

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

在里面 又包含了 一个 gazebo 的文件

  <xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.xacro"/>
  <xacro:include filename="$(find turtlebot3_description)/urdf/turtlebot3_waffle.gazebo.xacro"/>

里面有差速控制器 gazebo的插件

imu、laser、kinect

本次关注的有两个 laser && kinect
xacro如下

scan 发布的topic ： scan 对应上

<gazebo reference="base_scan">
    <material>Gazebo/FlatBlack</material>
    <sensor type="ray" name="lds_lfcd_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>$(arg laser_visual)</visualize>
      <update_rate>5</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>0.0</min_angle>
            <max_angle>6.28319</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.120</min>
          <max>3.5</max>
          <resolution>0.015</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_lds_lfcd_controller" filename="libgazebo_ros_laser.so">
        <topicName>scan</topicName> 
        <frameName>base_scan</frameName>
      </plugin>
    </sensor>
  </gazebo>

camera 发布的topic ： rgb/image_raw depth/image_raw 对应上了

  <gazebo reference="camera_rgb_frame">
    <sensor type="depth" name="realsense_R200">
      <always_on>true</always_on>
      <visualize>$(arg camera_visual)</visualize>
      <camera>
          <horizontal_fov>1.3439</horizontal_fov>
          <image>
              <width>1920</width>
              <height>1080</height>
              <format>R8G8B8</format>
          </image>
          <depth_camera></depth_camera>
          <clip>
              <near>0.03</near>
              <far>100</far>
          </clip>
      </camera>
      <plugin name="camera_controller" filename="libgazebo_ros_openni_kinect.so">
        <baseline>0.2</baseline>
        <alwaysOn>true</alwaysOn>
        <updateRate>30.0</updateRate>
        <cameraName>camera</cameraName>
        <frameName>camera_rgb_optical_frame</frameName>
        <imageTopicName>rgb/image_raw</imageTopicName>
        <depthImageTopicName>depth/image_raw</depthImageTopicName>
        <pointCloudTopicName>depth/points</pointCloudTopicName>
        <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
        <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
        <pointCloudCutoff>0.4</pointCloudCutoff>
            <hackBaseline>0.07</hackBaseline>
            <distortionK1>0.0</distortionK1>
            <distortionK2>0.0</distortionK2>
            <distortionK3>0.0</distortionK3>
            <distortionT1>0.0</distortionT1>
            <distortionT2>0.0</distortionT2>
        <CxPrime>0.0</CxPrime>
        <Cx>0.0</Cx>
        <Cy>0.0</Cy>
        <focalLength>0</focalLength>
        <hackBaseline>0</hackBaseline>
      </plugin>
    </sensor>
  </gazebo>

下面 就用 scan 数据

rostopic echo /scan

这将显示 一个持续的流 ，这个流 就是 话题上 LaserScan消息，大多是 LaserScan消息 的ranges ，这是要处理的内容

里面有设定的信息

ranges数组包含了 turtlebot 到最近障碍物的距离，从数组元素的序号可以算出这个障碍物的角度。

发布的消息类型是

官网的解释

订阅 /sacn topic

WanderRobot 类 私有变量 声明 订阅句柄

    ros::Subscriber Sub_WanderRobot_LaserScan_ ;

在构造函数里 赋值 订阅句柄

Sub_WanderRobot_LaserScan_ = nh_.subscribe("/scan",1,&WanderRobot::Sub_WanderRobot_LaserScan_Callback,this );

// 回调函数
void WanderRobot::Sub_WanderRobot_LaserScan_Callback(const sensor_msgs::LaserScan::ConstPtr &WanderRobot_LaserScan_msg)
{

 
}

在main()函数里加轮询函数

    ros::spinOnce();

求前方的障碍物距离距离

向量第1个元素 就是 前方的距离 179 是 后方的距离

range_ahead_ = ranges[0];

求最近障碍物的距离 与方向

此问题转化为 求 ros 里面 vector 的 向量里的最小元素 及 位置

首先求 ranges 的向量长度

这里有个插曲

ranges msg里 对于类型是

msg里float32[ ] 变量是 向量 类型 不是数组 ，求长度不能用sizeof（），直接 变量.size（） 就行

std::vector<float> ranges = WanderRobot_LaserScan_msg->ranges;

len_ = ranges.size();

求最短距离

也就是 求 ros 里面 vector 的 向量里的最小元素

这里也有个要注意的
ROS 可以调用 std 库下的 min_element()函数 ，来求向量里最小元素
min_element()函数 需要输入两个参数 起始地址 和 结束地址
在C++ 11 , std 库 中 有 向量变量.begin() 向量变量.end() 来求 向量起始结束地址 。
但是 ROS 1 里 可能使用的 不是 C++ 11 ，或者怎么样的， 反正没有 这两个函数

直接用取址 运算符

代码 ： &ranges[len_-1] 这个是不是还是要 +1，这个要看 min_element 函数 ，先不管了 ，漏一个就漏一个

float* closest_range =   std::min_element(&ranges[0], &ranges[len_-1]) ;
   float  closest_range_ = *closest_range ;

最短距离对于角度

就是求 vector 的 向量里的最小元素 的位置

用地址求
最小元素的地址 - 向量起始地址 再除以 单位向量的所占内存

   int  closest_range_seq = (closest_range - &ranges[0])/sizeof(ranges[0]);

知道 元素 位置了 角度就好算了

float bearing_closest_range_degree = (angle_min + closest_range_seq*(angle_max-angle_min)/len_ )/DEG2RAD ;