Skip to content

Commit

Permalink
feat: add support auto mode status (#111)
Browse files Browse the repository at this point in the history 
* feat: add support auto mode status

Signed-off-by: tkhmy <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: tkhmy <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@tkhmy @pre-commit-ci
tkhmy and pre-commit-ci[bot] authored Dec 20, 2023
1 parent a58468e commit 2f341cc
Show file tree
Hide file tree
Showing 2 changed files with 60 additions and 0 deletions.
48 changes: 48 additions & 0 deletions autoware_iv_external_api_adaptor/src/rtc_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,10 @@ RTCModule::RTCModule(rclcpp::Node * node, const std::string & name)
cooperate_status_namespace_ + "/" + name, rclcpp::QoS(1),
std::bind(&RTCModule::moduleCallback, this, _1));

auto_mode_sub_ = node->create_subscription<AutoModeStatus>(
auto_mode_status_namespace_ + "/" + name, rclcpp::QoS(1),
std::bind(&RTCModule::autoModeCallback, this, _1));

cli_set_module_ = proxy.create_client<CooperateCommands>(
cooperate_commands_namespace_ + "/" + name, rmw_qos_profile_services_default);

Expand All @@ -39,12 +43,23 @@ void RTCModule::moduleCallback(const CooperateStatusArray::ConstSharedPtr messag
module_statuses_ = message->statuses;
}

void RTCModule::autoModeCallback(const AutoModeStatus::ConstSharedPtr message)
{
auto_mode_status_.module = message->module;
auto_mode_status_.is_auto_mode = message->is_auto_mode;
}

void RTCModule::insertMessage(std::vector<CooperateStatus> & cooperate_statuses)
{
cooperate_statuses.insert(
cooperate_statuses.end(), module_statuses_.begin(), module_statuses_.end());
}

void RTCModule::insertAutoModeMessage(std::vector<AutoModeStatus> & auto_mode_status)
{
auto_mode_status.insert(auto_mode_status.begin(), auto_mode_status_);
}

void RTCModule::callService(
CooperateCommands::Request::SharedPtr request,
const CooperateCommands::Response::SharedPtr & responses)
Expand Down Expand Up @@ -101,6 +116,8 @@ RTCController::RTCController(const rclcpp::NodeOptions & options)

rtc_status_pub_ =
create_publisher<CooperateStatusArray>("/api/external/get/rtc_status", rclcpp::QoS(1));
auto_mode_pub_ =
create_publisher<AutoModeStatusArray>("/api/external/get/rtc_auto_mode", rclcpp::QoS(1));

group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
srv_set_rtc_ = proxy.create_service<CooperateCommands>(
Expand All @@ -111,6 +128,8 @@ RTCController::RTCController(const rclcpp::NodeOptions & options)
rmw_qos_profile_services_default, group_);

timer_ = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&RTCController::onTimer, this));
auto_mode_timer_ = rclcpp::create_timer(
this, get_clock(), 100ms, std::bind(&RTCController::onAutoModeTimer, this));
}

void RTCController::insertionSortAndValidation(std::vector<CooperateStatus> & statuses_vector)
Expand Down Expand Up @@ -259,6 +278,35 @@ void RTCController::setRTC(
}
}

void RTCController::onAutoModeTimer()
{
std::vector<AutoModeStatus> auto_mode_statuses;
blind_spot_->insertAutoModeMessage(auto_mode_statuses);
crosswalk_->insertAutoModeMessage(auto_mode_statuses);
detection_area_->insertAutoModeMessage(auto_mode_statuses);
intersection_->insertAutoModeMessage(auto_mode_statuses);
intersection_occlusion_->insertAutoModeMessage(auto_mode_statuses);
no_stopping_area_->insertAutoModeMessage(auto_mode_statuses);
occlusion_spot_->insertAutoModeMessage(auto_mode_statuses);
traffic_light_->insertAutoModeMessage(auto_mode_statuses);
virtual_traffic_light_->insertAutoModeMessage(auto_mode_statuses);
lane_change_left_->insertAutoModeMessage(auto_mode_statuses);
lane_change_right_->insertAutoModeMessage(auto_mode_statuses);
ext_request_lane_change_left_->insertAutoModeMessage(auto_mode_statuses);
ext_request_lane_change_right_->insertAutoModeMessage(auto_mode_statuses);
avoidance_left_->insertAutoModeMessage(auto_mode_statuses);
avoidance_right_->insertAutoModeMessage(auto_mode_statuses);
avoidance_by_lc_left_->insertAutoModeMessage(auto_mode_statuses);
avoidance_by_lc_right_->insertAutoModeMessage(auto_mode_statuses);
goal_planner_->insertAutoModeMessage(auto_mode_statuses);
start_planner_->insertAutoModeMessage(auto_mode_statuses);

AutoModeStatusArray msg;
msg.stamp = now();
msg.statuses = auto_mode_statuses;
auto_mode_pub_->publish(msg);
}

void RTCController::setRTCAutoMode(
const AutoModeWithModule::Request::SharedPtr request,
const AutoModeWithModule::Response::SharedPtr response)
Expand Down
12 changes: 12 additions & 0 deletions autoware_iv_external_api_adaptor/src/rtc_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,8 @@
#include <rclcpp/rclcpp.hpp>
#include <tier4_api_utils/tier4_api_utils.hpp>

#include "tier4_rtc_msgs/msg/auto_mode_status.hpp"
#include "tier4_rtc_msgs/msg/auto_mode_status_array.hpp"
#include "tier4_rtc_msgs/msg/cooperate_command.hpp"
#include "tier4_rtc_msgs/msg/cooperate_status.hpp"
#include "tier4_rtc_msgs/msg/cooperate_status_array.hpp"
Expand All @@ -33,6 +35,8 @@
using CooperateCommands = tier4_rtc_msgs::srv::CooperateCommands;
using AutoMode = tier4_rtc_msgs::srv::AutoMode;
using AutoModeWithModule = tier4_rtc_msgs::srv::AutoModeWithModule;
using AutoModeStatusArray = tier4_rtc_msgs::msg::AutoModeStatusArray;
using AutoModeStatus = tier4_rtc_msgs::msg::AutoModeStatus;
using CooperateStatusArray = tier4_rtc_msgs::msg::CooperateStatusArray;
using CooperateStatus = tier4_rtc_msgs::msg::CooperateStatus;
using Module = tier4_rtc_msgs::msg::Module;
Expand All @@ -42,15 +46,20 @@ class RTCModule
public:
std::string cooperate_status_namespace_ = "/planning/cooperate_status";
std::string cooperate_commands_namespace_ = "/planning/cooperate_commands";
std::string auto_mode_status_namespace_ = "/planning/auto_mode_status";
std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode";
std::vector<CooperateStatus> module_statuses_;
AutoModeStatus auto_mode_status_;
rclcpp::Subscription<CooperateStatusArray>::SharedPtr module_sub_;
rclcpp::Subscription<AutoModeStatus>::SharedPtr auto_mode_sub_;
tier4_api_utils::Client<CooperateCommands>::SharedPtr cli_set_module_;
tier4_api_utils::Client<AutoMode>::SharedPtr cli_set_auto_mode_;

RTCModule(rclcpp::Node * node, const std::string & name);
void moduleCallback(const CooperateStatusArray::ConstSharedPtr message);
void autoModeCallback(const AutoModeStatus::ConstSharedPtr message);
void insertMessage(std::vector<CooperateStatus> & cooperate_statuses);
void insertAutoModeMessage(std::vector<AutoModeStatus> & auto_mode_status);
void callService(
CooperateCommands::Request::SharedPtr request,
const CooperateCommands::Response::SharedPtr & responses);
Expand Down Expand Up @@ -88,13 +97,15 @@ class RTCController : public rclcpp::Node

/* publishers */
rclcpp::Publisher<CooperateStatusArray>::SharedPtr rtc_status_pub_;
rclcpp::Publisher<AutoModeStatusArray>::SharedPtr auto_mode_pub_;
/* service from external */
rclcpp::CallbackGroup::SharedPtr group_;
tier4_api_utils::Service<CooperateCommands>::SharedPtr srv_set_rtc_;
tier4_api_utils::Service<AutoModeWithModule>::SharedPtr srv_set_rtc_auto_mode_;

/* Timer */
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::TimerBase::SharedPtr auto_mode_timer_;

void insertionSortAndValidation(std::vector<CooperateStatus> & statuses_vector);
void checkInfDistance(CooperateStatus & status);
Expand All @@ -108,6 +119,7 @@ class RTCController : public rclcpp::Node

// ros callback
void onTimer();
void onAutoModeTimer();
};

} // namespace external_api
Expand Down

0 comments on commit 2f341cc

Please sign in to comment.