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_