|
| 1 | +// Copyright 2024 The Autoware Contributors |
| 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 | + |
| 16 | +#include "rclcpp/rclcpp.hpp" |
| 17 | +#include "availability_converter.hpp" |
| 18 | + |
| 19 | +namespace leader_election_converter |
| 20 | +{ |
| 21 | + |
| 22 | +AvailabilityConverter::AvailabilityConverter(rclcpp::Node * node) |
| 23 | +: node_(node) {} |
| 24 | + |
| 25 | +void AvailabilityConverter::setUdpSender(const std::string & dest_ip, const std::string & dest_port) |
| 26 | +{ |
| 27 | + udp_availability_sender_ = std::make_unique<UdpSender<Availability>>(dest_ip, dest_port); |
| 28 | +} |
| 29 | + |
| 30 | +void AvailabilityConverter::setSubscriber() |
| 31 | +{ |
| 32 | + const auto qos = rclcpp::QoS(1).transient_local(); |
| 33 | + availability_callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); |
| 34 | + rclcpp::SubscriptionOptions availability_options = rclcpp::SubscriptionOptions(); |
| 35 | + availability_options.callback_group = availability_callback_group_; |
| 36 | + |
| 37 | + control_mode_callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); |
| 38 | + rclcpp::SubscriptionOptions control_mode_options = rclcpp::SubscriptionOptions(); |
| 39 | + control_mode_options.callback_group = control_mode_callback_group_; |
| 40 | + auto not_executed_callback = []([[maybe_unused]] const typename autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) {}; |
| 41 | + |
| 42 | + sub_operation_mode_availability_ = node_->create_subscription<tier4_system_msgs::msg::OperationModeAvailability>( |
| 43 | + "~/input/operation_mode_availability", qos, |
| 44 | + std::bind(&AvailabilityConverter::convertToUdp, this, std::placeholders::_1), availability_options); |
| 45 | + |
| 46 | + sub_control_mode_ = node_-> create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>( |
| 47 | + "~/input/control_mode", qos, not_executed_callback, control_mode_options); |
| 48 | + |
| 49 | +} |
| 50 | + |
| 51 | +void AvailabilityConverter::convertToUdp( |
| 52 | + const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr availability_msg) |
| 53 | +{ |
| 54 | + auto control_mode_report = std::make_shared<autoware_auto_vehicle_msgs::msg::ControlModeReport>(); |
| 55 | + rclcpp::MessageInfo message_info; |
| 56 | + const bool success = sub_control_mode_->take(*control_mode_report, message_info); |
| 57 | + if (success) { |
| 58 | + Availability availability; |
| 59 | + availability.mode = control_mode_report->mode; |
| 60 | + availability.stop = availability_msg->stop; |
| 61 | + availability.autonomous = availability_msg->autonomous; |
| 62 | + availability.local = availability_msg->local; |
| 63 | + availability.remote = availability_msg->remote; |
| 64 | + availability.emergency_stop = availability_msg->emergency_stop; |
| 65 | + availability.comfortable_stop = availability_msg->comfortable_stop; |
| 66 | + availability.pull_over = availability_msg->pull_over; |
| 67 | + udp_availability_sender_->send(availability); |
| 68 | + } else { |
| 69 | + RCLCPP_ERROR(node_->get_logger(), "Failed to take control mode report"); |
| 70 | + } |
| 71 | +} |
| 72 | + |
| 73 | +} // namespace leader_election_converter |
0 commit comments