.. _program_listing_file__tmp_ws_src_SMACC2_smacc2_sm_reference_library_sm_dance_bot_warehouse_3_include_sm_dance_bot_warehouse_3_clients_cl_lidar_components_cp_forward_obstacle_detector.hpp:

Program Listing for File cp_forward_obstacle_detector.hpp
=========================================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_SMACC2_smacc2_sm_reference_library_sm_dance_bot_warehouse_3_include_sm_dance_bot_warehouse_3_clients_cl_lidar_components_cp_forward_obstacle_detector.hpp>` (``/tmp/ws/src/SMACC2/smacc2_sm_reference_library/sm_dance_bot_warehouse_3/include/sm_dance_bot_warehouse_3/clients/cl_lidar/components/cp_forward_obstacle_detector.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // 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.
   
   /*****************************************************************************************************************
    *
    *   Authors: Pablo Inigo Blasco, Brett Aldrich
    *
    ******************************************************************************************************************/
   #pragma once
   
   #include <smacc2/smacc.hpp>
   
   namespace sm_dance_bot_warehouse_3
   {
   namespace cl_lidar
   {
   using namespace std::chrono_literals;
   
   class CpForwardObstacleDetector : public smacc2::ISmaccComponent
   {
   public:
     sensor_msgs::msg::LaserScan lastScanMessage_;
   
     const float SECURITY_DISTANCE = 0.4;  // meters
   
     void onInitialize() override
     {
       auto client_ =
         dynamic_cast<smacc2::client_bases::SmaccSubscriberClient<sensor_msgs::msg::LaserScan> *>(
           owner_);
       client_->onMessageReceived(&CpForwardObstacleDetector::MessageCallbackStoreDistanceToWall, this);
     }
   
     int modulo_Euclidean(int a, int b)
     {
       int m = a % b;
       if (m < 0)
       {
         // m += (b < 0) ? -b : b; // avoid this form: -b is UB when b == INT_MIN
         m = (b < 0) ? m - b : m + b;
       }
       return m;
     }
   
     float getForwardDistance(int raysWidthCount = 0)
     {
       //auto fwdist = scanmsg.ranges[scanmsg.ranges.size() / 2] /*meter*/;
       auto fwdist = lastScanMessage_.ranges[0] /*meter*/;
   
       std::stringstream ss;
       //check rays around main ray
       for (int i = 0; i < raysWidthCount; i++)
       {
         // int baseindex = 0;
         int scanindex =  (- raysWidthCount / 2 + i) % raysWidthCount;
   
         if(scanindex < 0)
         {
           scanindex = lastScanMessage_.ranges.size() + scanindex;
         }
   
         float fwdist2 = lastScanMessage_.ranges[scanindex];
   
         RCLCPP_INFO_STREAM(
           getLogger(), "[" << getName() << "]"
                            << "range[" << scanindex << "] = " << fwdist2);
   
         if (fwdist2 > 0.01 && fwdist2 < fwdist)
         {
           RCLCPP_INFO_STREAM(
             getLogger(), "[" << getName() << "]"
                              << "updated range[0] = " << fwdist2);
   
           fwdist = fwdist2;
         }
       }
   
       RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "]" << ss.str());
   
       /*if the distance is infinity or nan, use max range distance*/
       if (fwdist == std::numeric_limits<float>::infinity() || fwdist != fwdist)
       {
         fwdist = lastScanMessage_.range_max - SECURITY_DISTANCE /*meters*/;
         RCLCPP_INFO_THROTTLE(
           getLogger(), *(getNode()->get_clock()), 1000,
           "[CpForwardObstacleDetector] Distance to forward obstacle is not a number, setting default value "
           "to: %lf",
           lastScanMessage_.range_max);
       }
       else
       {
         RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] fwdist: " << fwdist);
         fwdist = std::max(fwdist - SECURITY_DISTANCE, 0.0F);
         RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] fwdist: " << fwdist);
       }
   
       return fwdist;
     }
   
     void MessageCallbackStoreDistanceToWall(const sensor_msgs::msg::LaserScan & scanmsg)
     {
       this->lastScanMessage_ = scanmsg;
     }
   };
   }  // namespace cl_lidar
   }  // namespace sm_dance_bot_warehouse_3