LCOV - code coverage report
Current view: top level - src/tools/lidar_integration/src - lidar_lc_integration_listener.hpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 0 62 0.0 %
Date: 2023-03-03 05:44:19 Functions: 0 9 0.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 0 268 0.0 %

           Branch data     Line data    Source code
       1                 :            : // Copyright 2018 the Autoware Foundation
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : //
      15                 :            : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
      16                 :            : 
      17                 :            : 
      18                 :            : #ifndef LIDAR_LC_INTEGRATION_LISTENER_HPP_
      19                 :            : #define LIDAR_LC_INTEGRATION_LISTENER_HPP_
      20                 :            : 
      21                 :            : #include <rclcpp/rclcpp.hpp>
      22                 :            : #include <std_msgs/msg/u_int8_multi_array.hpp>
      23                 :            : #include <lifecycle_msgs/msg/state.hpp>
      24                 :            : #include <lifecycle_msgs/msg/transition.hpp>
      25                 :            : #include <lifecycle_msgs/srv/change_state.hpp>
      26                 :            : #include <lifecycle_msgs/srv/get_state.hpp>
      27                 :            : #include <common/types.hpp>
      28                 :            : #include <lidar_integration/lidar_integration_common.hpp>
      29                 :            : #include <string>
      30                 :            : #include <memory>
      31                 :            : 
      32                 :            : namespace lidar_integration
      33                 :            : {
      34                 :            : 
      35                 :            : using autoware::common::types::float32_t;
      36                 :            : using autoware::common::types::float64_t;
      37                 :            : using autoware::common::types::bool8_t;
      38                 :            : using autoware::common::types::char8_t;
      39                 :            : 
      40                 :            : class Lcm : public rclcpp::Node
      41                 :            : {
      42                 :            : public:
      43                 :          0 :   explicit Lcm(const std::string & node_name)
      44         [ #  # ]:          0 :   : Node(node_name)
      45                 :          0 :   {}
      46                 :            : 
      47                 :          0 :   ~Lcm()
      48                 :          0 :   {
      49         [ #  # ]:          0 :     if (m_thread.joinable()) {
      50                 :            :       m_running.store(false);
      51                 :          0 :       m_thread.join();
      52                 :            :     }
      53                 :          0 :   }
      54                 :            : 
      55                 :          0 :   void start()
      56                 :            :   {
      57                 :            :     m_running.store(true);
      58                 :          0 :     m_thread = std::thread{[this] {task_function();}};
      59                 :          0 :   }
      60                 :            : 
      61                 :          0 :   void init()
      62                 :            :   {
      63   [ #  #  #  #  :          0 :     client_get_state_ = this->create_client<lifecycle_msgs::srv::GetState>(
          #  #  #  #  #  
                      # ]
      64                 :            :       node_get_state_topic);
      65   [ #  #  #  #  :          0 :     client_change_state_ = this->create_client<lifecycle_msgs::srv::ChangeState>(
          #  #  #  #  #  
                      # ]
      66                 :            :       node_change_state_topic);
      67                 :            :     const char8_t * const driver_action_topic = "lidar_driver_action";
      68                 :            :     const char8_t * const fuser_action_topic = "lidar_fuser_action";
      69                 :            :     driver_action_pub_ptr =
      70   [ #  #  #  #  :          0 :       this->create_publisher<std_msgs::msg::UInt8MultiArray>(driver_action_topic, 10);
          #  #  #  #  #  
                      # ]
      71                 :            :     fuser_action_pub_ptr =
      72   [ #  #  #  #  :          0 :       this->create_publisher<std_msgs::msg::UInt8MultiArray>(fuser_action_topic, 10);
             #  #  #  # ]
      73                 :          0 :   }
      74                 :          0 :   uint8_t get_state()
      75                 :            :   {
      76                 :            :     auto request = std::make_shared<lifecycle_msgs::srv::GetState::Request>();
      77                 :            : 
      78                 :            :     // We send the service request for asking the current
      79   [ #  #  #  #  :          0 :     auto future_result = client_get_state_->async_send_request(request);
                   #  # ]
      80                 :            : 
      81   [ #  #  #  # ]:          0 :     if (future_result.wait_for(std::chrono::seconds(1)) != std::future_status::ready) {
      82   [ #  #  #  #  :          0 :       LIDAR_INTEGRATION_ERROR("Server time out while getting current state.");
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
                #  #  # ]
      83                 :          0 :       return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
      84                 :            :     }
      85                 :            : 
      86                 :            :     // We have an succesful answer.
      87         [ #  # ]:          0 :     if (future_result.get()) {
      88                 :          0 :       return future_result.get()->current_state.id;
      89                 :            :     } else {
      90   [ #  #  #  #  :          0 :       LIDAR_INTEGRATION_ERROR("Failed to get current state.");
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
             #  #  #  #  
                      # ]
      91                 :            : 
      92                 :          0 :       return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
      93                 :            :     }
      94                 :            :   }
      95                 :            : 
      96                 :          0 :   void change_state(std::uint8_t transition)
      97                 :            :   {
      98                 :            :     auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
      99                 :          0 :     request->transition.id = transition;
     100                 :            : 
     101                 :            :     // We send the request with the transition we want to invoke.
     102   [ #  #  #  #  :          0 :     auto future_result = client_change_state_->async_send_request(request);
                   #  # ]
     103                 :            : 
     104   [ #  #  #  # ]:          0 :     if (future_result.wait_for(std::chrono::seconds(10)) != std::future_status::ready) {
     105                 :            :       auto * lifecycle_node = "lidar_detector_managed";
     106   [ #  #  #  #  :          0 :       LIDAR_INTEGRATION_ERROR(
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
                #  #  # ]
     107                 :            :         "Server time out while getting current state for node: %s",
     108                 :            :         lifecycle_node);
     109                 :            :     }
     110                 :            : 
     111                 :            :     // if its not sucess
     112         [ #  # ]:          0 :     if (!future_result.get()->success) {
     113   [ #  #  #  #  :          0 :       LIDAR_INTEGRATION_ERROR("Failed to trigger transition %u", static_cast<uint32_t>(transition));
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
          #  #  #  #  #  
             #  #  #  #  
                      # ]
     114                 :            :     }
     115                 :          0 :   }
     116                 :            : 
     117         [ #  # ]:          0 :   void publish_actions()
     118                 :            :   {
     119                 :            :     // start the drivers
     120                 :          0 :     std_msgs::msg::UInt8MultiArray msg;
     121         [ #  # ]:          0 :     msg.data.resize(2U);
     122                 :          0 :     msg.data[0] = 1U;  // restart
     123         [ #  # ]:          0 :     msg.data[1] = 0U;  // nothing
     124         [ #  # ]:          0 :     driver_action_pub_ptr->publish(msg);
     125                 :          0 :     msg.data[0] = 0U;  // nothing
     126         [ #  # ]:          0 :     msg.data[1] = 1U;  // restart
     127         [ #  # ]:          0 :     driver_action_pub_ptr->publish(msg);
     128                 :            : 
     129                 :            :     // revive fusers
     130                 :          0 :     msg.data[0] = 3U;  // revive
     131         [ #  # ]:          0 :     msg.data[1] = 0U;  // nothing
     132         [ #  # ]:          0 :     fuser_action_pub_ptr->publish(msg);
     133                 :          0 :     msg.data[0] = 0U;  // nothing
     134         [ #  # ]:          0 :     msg.data[1] = 3U;  // revive
     135         [ #  # ]:          0 :     fuser_action_pub_ptr->publish(msg);
     136                 :          0 :   }
     137                 :            : 
     138                 :            : private:
     139                 :            :   /// \brief Fills BoundingBoxArray message with boxes, and publishes
     140                 :          0 :   void task_function()
     141                 :            :   {
     142                 :            :     using namespace std::chrono_literals;  // NOLINT
     143                 :            : 
     144                 :            :     auto count = 0;
     145                 :            :     auto once = true;
     146         [ #  # ]:          0 :     while (m_running.load(std::memory_order_relaxed)) {
     147         [ #  # ]:          0 :       if (count == 5) {
     148                 :            :         // configure (will be done by ros2 launch)
     149                 :          0 :         change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
     150         [ #  # ]:          0 :       } else if (count == 10) {
     151                 :            :         // activate (will be done by ros2 launch)
     152                 :          0 :         change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
     153         [ #  # ]:          0 :       } else if (once) {
     154         [ #  # ]:          0 :         if (get_state() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
     155                 :          0 :           publish_actions();
     156                 :            :           // reset the flag
     157                 :            :           once = false;
     158                 :            :         }
     159                 :            :       }
     160                 :          0 :       count++;
     161                 :            : 
     162                 :          0 :       std::this_thread::sleep_for(10ms);
     163                 :            :     }
     164                 :          0 :   }
     165                 :            : 
     166                 :            :   static constexpr char8_t const * node_get_state_topic = "/lidar_detector_managed/get_state";
     167                 :            :   static constexpr char8_t const * node_change_state_topic = "/lidar_detector_managed/change_state";
     168                 :            : 
     169                 :            :   std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::GetState>> client_get_state_;
     170                 :            :   std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> client_change_state_;
     171                 :            :   std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8MultiArray>> driver_action_pub_ptr;
     172                 :            :   std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8MultiArray>> fuser_action_pub_ptr;
     173                 :            : 
     174                 :            :   std::atomic_bool m_running{false};
     175                 :            :   std::thread m_thread;
     176                 :            : };
     177                 :            : }  // namespace lidar_integration
     178                 :            : #endif  // LIDAR_LC_INTEGRATION_LISTENER_HPP_

Generated by: LCOV version 1.14