Skip to content

Commit

Permalink
fix(default_ad_api): fix autoware state to add wait time (autowarefou…
Browse files Browse the repository at this point in the history 
…ndation#2407)

* fix(default_ad_api): fix autoware state to add wait time

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Update system/default_ad_api/src/compatibility/autoware_state.cpp

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
  • Loading branch information
@isamu-takagi @kenji-miyake
isamu-takagi and kenji-miyake committed Dec 6, 2022
1 parent f984fbb commit c422485
Show file tree
Hide file tree
Showing 2 changed files with 112 additions and 4 deletions.
112 changes: 108 additions & 4 deletions system/default_ad_api/src/compatibility/autoware_state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,86 @@
#include "autoware_state.hpp"

#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

namespace transition
{

using State = autoware_auto_system_msgs::msg::AutowareState;
using Value = State::_state_type;
struct PairHash
{
size_t operator()(const std::pair<Value, Value> & pair) const noexcept
{
// This hash only works for uint8 pairs.
const auto prev = static_cast<size_t>(pair.first);
const auto curr = static_cast<size_t>(pair.second);
return (prev << 8) | (curr << 0);
}
};

double get_wait_time(const Value state)
{
if (state == State::INITIALIZING) {
return 1.0;
}
if (state == State::PLANNING) {
return 3.0;
}
if (state == State::ARRIVED_GOAL) {
return 2.0;
}
return 0.0;
}

const double arrived_goal_timeout = get_wait_time(State::ARRIVED_GOAL);

// clang-format off
std::unordered_map<std::pair<Value, Value>, Value, PairHash> matrix =
{
{{State::INITIALIZING , State::INITIALIZING }, State::INITIALIZING },
{{State::INITIALIZING , State::WAITING_FOR_ROUTE }, State::WAITING_FOR_ROUTE },
{{State::INITIALIZING , State::PLANNING }, State::WAITING_FOR_ROUTE },
{{State::INITIALIZING , State::WAITING_FOR_ENGAGE}, State::WAITING_FOR_ROUTE },
{{State::INITIALIZING , State::DRIVING }, State::WAITING_FOR_ROUTE },
{{State::INITIALIZING , State::ARRIVED_GOAL }, State::WAITING_FOR_ROUTE },
{{State::WAITING_FOR_ROUTE , State::INITIALIZING }, State::WAITING_FOR_ROUTE },
{{State::WAITING_FOR_ROUTE , State::WAITING_FOR_ROUTE }, State::WAITING_FOR_ROUTE },
{{State::WAITING_FOR_ROUTE , State::PLANNING }, State::PLANNING },
{{State::WAITING_FOR_ROUTE , State::WAITING_FOR_ENGAGE}, State::PLANNING },
{{State::WAITING_FOR_ROUTE , State::DRIVING }, State::DRIVING },
{{State::WAITING_FOR_ROUTE , State::ARRIVED_GOAL }, State::DRIVING },
{{State::PLANNING , State::INITIALIZING }, State::PLANNING },
{{State::PLANNING , State::WAITING_FOR_ROUTE }, State::WAITING_FOR_ROUTE },
{{State::PLANNING , State::PLANNING }, State::PLANNING },
{{State::PLANNING , State::WAITING_FOR_ENGAGE}, State::WAITING_FOR_ENGAGE},
{{State::PLANNING , State::DRIVING }, State::WAITING_FOR_ENGAGE},
{{State::PLANNING , State::ARRIVED_GOAL }, State::WAITING_FOR_ENGAGE},
{{State::WAITING_FOR_ENGAGE, State::INITIALIZING }, State::WAITING_FOR_ENGAGE},
{{State::WAITING_FOR_ENGAGE, State::WAITING_FOR_ROUTE }, State::WAITING_FOR_ROUTE },
{{State::WAITING_FOR_ENGAGE, State::PLANNING }, State::PLANNING },
{{State::WAITING_FOR_ENGAGE, State::WAITING_FOR_ENGAGE}, State::WAITING_FOR_ENGAGE},
{{State::WAITING_FOR_ENGAGE, State::DRIVING }, State::DRIVING },
{{State::WAITING_FOR_ENGAGE, State::ARRIVED_GOAL }, State::ARRIVED_GOAL },
{{State::DRIVING , State::INITIALIZING }, State::DRIVING },
{{State::DRIVING , State::WAITING_FOR_ROUTE }, State::WAITING_FOR_ROUTE },
{{State::DRIVING , State::PLANNING }, State::PLANNING },
{{State::DRIVING , State::WAITING_FOR_ENGAGE}, State::WAITING_FOR_ENGAGE},
{{State::DRIVING , State::DRIVING }, State::DRIVING },
{{State::DRIVING , State::ARRIVED_GOAL }, State::ARRIVED_GOAL },
{{State::ARRIVED_GOAL , State::INITIALIZING }, State::ARRIVED_GOAL },
{{State::ARRIVED_GOAL , State::WAITING_FOR_ROUTE }, State::WAITING_FOR_ROUTE },
{{State::ARRIVED_GOAL , State::PLANNING }, State::WAITING_FOR_ROUTE },
{{State::ARRIVED_GOAL , State::WAITING_FOR_ENGAGE}, State::WAITING_FOR_ROUTE },
{{State::ARRIVED_GOAL , State::DRIVING }, State::WAITING_FOR_ROUTE },
{{State::ARRIVED_GOAL , State::ARRIVED_GOAL }, State::ARRIVED_GOAL }
};
// clang-format on

} // namespace transition

namespace default_ad_api
{

Expand Down Expand Up @@ -55,6 +133,9 @@ AutowareStateNode::AutowareStateNode(const rclcpp::NodeOptions & options)
localization_state_.state = LocalizationState::UNKNOWN;
routing_state_.state = RoutingState::UNKNOWN;
operation_mode_state_.mode = OperationModeState::UNKNOWN;

previous_stamp_ = now();
previous_state_ = std::nullopt;
}

void AutowareStateNode::on_localization(const LocalizationState::ConstSharedPtr msg)
Expand Down Expand Up @@ -129,15 +210,38 @@ void AutowareStateNode::on_timer()

// Update routing state to reproduce old logic.
if (routing_state_.state == RoutingState::ARRIVED) {
const auto duration = now() - rclcpp::Time(routing_state_.stamp);
if (2.0 < duration.seconds()) {
const auto duration = (now() - rclcpp::Time(routing_state_.stamp)).seconds();
if (transition::arrived_goal_timeout < duration) {
routing_state_.state = RoutingState::UNSET;
}
}

// Change next state to reproduce old logic.
auto current_state = convert_state();
if (previous_state_) {
const auto pair = std::make_pair(previous_state_.value(), current_state);
if (transition::matrix.count(pair)) {
current_state = transition::matrix.at(pair);
}
}

// Wait state change for sync to reproduce old logic.
const auto current_stamp = now();
if (previous_state_) {
const auto duration = (current_stamp - previous_stamp_).seconds();
const auto previous = previous_state_.value();
if (duration < transition::get_wait_time(previous)) {
current_state = previous;
}
}
if (current_state != previous_state_) {
previous_stamp_ = current_stamp;
previous_state_ = current_state;
}

AutowareState msg;
msg.stamp = now();
msg.state = convert_state();
msg.stamp = current_stamp;
msg.state = current_state;
pub_autoware_state_->publish(msg);
}

Expand Down
4 changes: 4 additions & 0 deletions system/default_ad_api/src/compatibility/autoware_state.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <std_srvs/srv/trigger.hpp>
#include <tier4_system_msgs/msg/mode_change_available.hpp>

#include <optional>
#include <vector>

// This file should be included after messages.
Expand Down Expand Up @@ -60,6 +61,9 @@ class AutowareStateNode : public rclcpp::Node
RoutingState routing_state_;
OperationModeState operation_mode_state_;

rclcpp::Time previous_stamp_;
std::optional<AutowareState::_state_type> previous_state_;

void on_timer();
void on_localization(const LocalizationState::ConstSharedPtr msg);
void on_routing(const RoutingState::ConstSharedPtr msg);
Expand Down

0 comments on commit c422485

Please sign in to comment.