cp_lidar_data.cpp

Go to the documentation of this file.

// Copyright 2021 RobosoftAI Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#include <smacc2/smacc.hpp>
 
namespace sm_dance_bot_strikes_back
{
namespace cl_lidar
{
using namespace std::chrono_literals;
 
void CpLidarSensorData::onInitialize()
{
  auto client_ =
    dynamic_cast<smacc2::client_bases::SmaccSubscriberClient<sensor_msgs::msg::LaserScan> *>(
      owner_);
  client_->onMessageReceived(&CpLidarSensorData::MessageCallbackStoreDistanceToWall, this);
}
 
void CpLidarSensorData::MessageCallbackStoreDistanceToWall(
  const sensor_msgs::msg::LaserScan & scanmsg)
{
  this->lastMessage_ = scanmsg;
  //auto fwdist = scanmsg.ranges[scanmsg.ranges.size() / 2] /*meter*/;
  auto fwdist = scanmsg.ranges[0] /*meter*/;
 
  /*if the distance is infinity or nan, use max range distance*/
  if (fwdist == std::numeric_limits<float>::infinity() || fwdist != fwdist)
  {
    this->forwardObstacleDistance = scanmsg.range_max - SECURITY_DISTANCE /*meters*/;
    RCLCPP_INFO_THROTTLE(
      getLogger(), *(getNode()->get_clock()), 1000,
      "[CpLidarSensorData] Distance to forward obstacle is not a number, setting default value "
      "to: %lf",
      scanmsg.range_max);
  }
  else
  {
    this->forwardObstacleDistance = std::max(fwdist - SECURITY_DISTANCE, 0.0F);
  }
}
 
}  // namespace cl_lidar
}  // namespace sm_dance_bot_strikes_back