Program Listing for File sonar_lut.hpp

↰ Return to documentation for file (/tmp/ws/src/dbw_ros/dbw_ford_can/include/dbw_ford_can/sonar_lut.hpp)

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015-2019, Dataspeed Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Dataspeed Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#pragma once

#include <dbw_ford_msgs/msg/surround_report.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <cmath> // M_PI

namespace dbw_ford_can {

static constexpr struct {float x; float y; float z; float a;} SONAR_TABLE[] = {
//   x,      y,     z,     angle
 { 4.000,  0.900, 0.100, 0.500 * M_PI}, // Front left side
 { 4.000,  0.500, 0.100, 0.100 * M_PI}, // Front left corner
 { 4.000,  0.200, 0.100, 0.000 * M_PI}, // Front left center
 { 4.000, -0.200, 0.100, 0.000 * M_PI}, // Front right center
 { 4.000, -0.500, 0.100, 1.900 * M_PI}, // Front right corner
 { 4.000, -0.900, 0.100, 1.500 * M_PI}, // Front right side
 {-1.000,  0.900, 0.100, 0.500 * M_PI}, // Rear left side
 {-1.000,  0.500, 0.100, 0.900 * M_PI}, // Rear left corner
 {-1.000,  0.200, 0.100, 1.000 * M_PI}, // Rear left center
 {-1.000, -0.200, 0.100, 1.000 * M_PI}, // Rear right center
 {-1.000, -0.500, 0.100, 1.100 * M_PI}, // Rear right corner
 {-1.000, -0.900, 0.100, 1.500 * M_PI}, // Rear right side
};
static constexpr float sonarMetersFromBits(uint8_t bits) {
  return bits ? ((float)bits * 0.15) + 0.15 : 0.0;
}
static constexpr uint32_t sonarColorFromRange(float range) {
  if (range < 0.7) {
    return 0xC0FF0000; // rgba = RED
  } else if (range < 1.3) {
    return 0xC0FFFF00; // rgba = YELLOW
  } else {
    return 0xC000FF00; // rgba = GREEN
  }
}
static inline void sonarBuildPointCloud2(sensor_msgs::msg::PointCloud2 &cloud, const dbw_ford_msgs::msg::SurroundReport &surround) {
  // Populate message fields
  const uint32_t POINT_STEP = 16;
  cloud.header.frame_id = "base_link";
  cloud.header.stamp = surround.header.stamp;
  cloud.fields.resize(4);
  cloud.fields[0].name = "x";
  cloud.fields[0].offset = 0;
  cloud.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32;
  cloud.fields[0].count = 1;
  cloud.fields[1].name = "y";
  cloud.fields[1].offset = 4;
  cloud.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32;
  cloud.fields[1].count = 1;
  cloud.fields[2].name = "z";
  cloud.fields[2].offset = 8;
  cloud.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32;
  cloud.fields[2].count = 1;
  cloud.fields[3].name = "rgba";
  cloud.fields[3].offset = 12;
  cloud.fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32;
  cloud.fields[3].count = 1;
  cloud.data.resize(12 * POINT_STEP);

  uint8_t *ptr = cloud.data.data();
  for (unsigned int i = 0; i < 12; i++) {
    const float range = surround.sonar[i];
    if (range > 0.0) {
      *((float*)(ptr + 0)) = SONAR_TABLE[i].x + cosf(SONAR_TABLE[i].a) * range; // x
      *((float*)(ptr + 4)) = SONAR_TABLE[i].y + sinf(SONAR_TABLE[i].a) * range; // y
      *((float*)(ptr + 8)) = SONAR_TABLE[i].z; // z
      *((uint32_t*)(ptr + 12)) = sonarColorFromRange(range); // rgba
      ptr += POINT_STEP;
    }
  }
  if (ptr == cloud.data.data()) {
    // Prevent rviz from latching the last message
    *((float*)(ptr + 0)) = NAN; // x
    *((float*)(ptr + 4)) = NAN; // y
    *((float*)(ptr + 8)) = NAN; // z
    *((uint32_t*)(ptr + 12)) = 0x00000000; // rgba
    ptr += POINT_STEP;
  }

  // Populate message with number of valid points
  cloud.point_step = POINT_STEP;
  cloud.row_step = ptr - cloud.data.data();
  cloud.height = 1;
  cloud.width = cloud.row_step / POINT_STEP;
  cloud.is_bigendian = false;
  cloud.is_dense = true;
  cloud.data.resize(cloud.row_step); // Shrink to actual size
}

} // namespace dbw_ford_can