.. _program_listing_file__tmp_ws_src_smacc2_smacc2_sm_reference_library_sm_dance_bot_strikes_back_include_sm_dance_bot_strikes_back_clients_cl_lidar_components_cp_lidar_data.hpp: Program Listing for File cp_lidar_data.hpp ========================================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/smacc2/smacc2_sm_reference_library/sm_dance_bot_strikes_back/include/sm_dance_bot_strikes_back/clients/cl_lidar/components/cp_lidar_data.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 * ******************************************************************************************************************/ #include namespace sm_dance_bot_strikes_back { namespace cl_lidar { using namespace std::chrono_literals; class CpLidarSensorData : public smacc2::ISmaccComponent { public: sensor_msgs::msg::LaserScan lastMessage_; float forwardObstacleDistance; const float SECURITY_DISTANCE = 1; // meters void onInitialize() override { auto client_ = dynamic_cast *>( owner_); client_->onMessageReceived(&CpLidarSensorData::MessageCallbackStoreDistanceToWall, this); } void 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::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