Skip to content

Commit e0531b8

Browse files
isamu-takagiisamu-takagi
authored
feat(autoware_default_adapi): release adapi v1.6.0 (#9704)
* feat: reject clearing route during autonomous mode Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * feat: modify check and relay door service Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix door condition Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix error and add option Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * update v1.6.0 Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
1 parent d2a9531 commit e0531b8

File tree

9 files changed

+90
-20
lines changed

9 files changed

+90
-20
lines changed

‎common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@ struct VehicleStatus
4141
using Message = autoware_adapi_v1_msgs::msg::VehicleStatus;
4242
static constexpr char name[] = "/api/vehicle/status";
4343
static constexpr size_t depth = 1;
44-
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
44+
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
4545
static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
4646
};
4747

‎system/autoware_default_adapi/config/default_adapi.param.yaml

+4
Original file line numberDiff line numberDiff line change
@@ -6,3 +6,7 @@
66
ros__parameters:
77
require_accept_start: false
88
stop_check_duration: 1.0
9+
10+
/adapi/node/vehicle_door:
11+
ros__parameters:
12+
check_autoware_control: true

‎system/autoware_default_adapi/launch/test_default_adapi.launch.xml

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
<launch>
2+
<arg name="vehicle_model" default="sample_vehicle"/>
3+
<include file="$(find-pkg-share autoware_global_parameter_loader)/launch/global_params.launch.py"/>
4+
<include file="$(find-pkg-share autoware_default_adapi)/launch/default_adapi.launch.py"/>
5+
</launch>

‎system/autoware_default_adapi/src/interface.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interf
2121
{
2222
const auto on_interface_version = [](auto, auto res) {
2323
res->major = 1;
24-
res->minor = 5;
24+
res->minor = 6;
2525
res->patch = 0;
2626
};
2727

‎system/autoware_default_adapi/src/routing.cpp

+10-1
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,7 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing",
6868
adaptor.init_cli(cli_operation_mode_, group_cli_);
6969
adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode);
7070

71+
is_autoware_control_ = false;
7172
is_auto_mode_ = false;
7273
state_.state = State::Message::UNKNOWN;
7374
}
@@ -85,6 +86,7 @@ void RoutingNode::change_stop_mode()
8586

8687
void RoutingNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg)
8788
{
89+
is_autoware_control_ = msg->is_autoware_control_enabled;
8890
is_auto_mode_ = msg->mode == OperationModeState::Message::AUTONOMOUS;
8991
}
9092

@@ -119,7 +121,14 @@ void RoutingNode::on_clear_route(
119121
const autoware::adapi_specs::routing::ClearRoute::Service::Request::SharedPtr req,
120122
const autoware::adapi_specs::routing::ClearRoute::Service::Response::SharedPtr res)
121123
{
122-
change_stop_mode();
124+
// For safety, do not clear the route while it is in use.
125+
// https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/clear_route/
126+
if (is_auto_mode_ && is_autoware_control_) {
127+
res->status.success = false;
128+
res->status.code = ResponseStatus::UNKNOWN;
129+
res->status.message = "The route cannot be cleared while it is in use.";
130+
return;
131+
}
123132
res->status = conversion::convert_call(cli_clear_route_, req);
124133
}
125134

‎system/autoware_default_adapi/src/routing.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ class RoutingNode : public rclcpp::Node
5252
Cli<autoware::component_interface_specs::planning::ClearRoute> cli_clear_route_;
5353
Cli<autoware::component_interface_specs::system::ChangeOperationMode> cli_operation_mode_;
5454
Sub<autoware::component_interface_specs::system::OperationModeState> sub_operation_mode_;
55+
bool is_autoware_control_;
5556
bool is_auto_mode_;
5657
State::Message state_;
5758

‎system/autoware_default_adapi/src/vehicle_door.cpp

+39-7
Original file line numberDiff line numberDiff line change
@@ -24,18 +24,50 @@ VehicleDoorNode::VehicleDoorNode(const rclcpp::NodeOptions & options)
2424
{
2525
const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this);
2626
group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
27-
adaptor.relay_service(cli_command_, srv_command_, group_cli_, 3.0);
28-
adaptor.relay_service(cli_layout_, srv_layout_, group_cli_, 3.0);
27+
adaptor.relay_service(cli_layout_, srv_layout_, group_cli_);
28+
adaptor.init_cli(cli_command_, group_cli_);
29+
adaptor.init_srv(srv_command_, this, &VehicleDoorNode::on_command);
2930
adaptor.init_pub(pub_status_);
3031
adaptor.init_sub(sub_status_, this, &VehicleDoorNode::on_status);
32+
adaptor.init_sub(sub_operation_mode_, this, &VehicleDoorNode::on_operation_mode);
33+
34+
check_autoware_control_ = declare_parameter<bool>("check_autoware_control");
35+
is_autoware_control_ = false;
36+
is_stop_mode_ = false;
37+
}
38+
39+
void VehicleDoorNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg)
40+
{
41+
is_autoware_control_ = msg->is_autoware_control_enabled;
42+
is_stop_mode_ = msg->mode == OperationModeState::Message::STOP;
43+
}
44+
void VehicleDoorNode::on_status(InternalDoorStatus::Message::ConstSharedPtr msg)
45+
{
46+
utils::notify(pub_status_, status_, *msg, utils::ignore_stamp<InternalDoorStatus::Message>);
3147
}
3248

33-
void VehicleDoorNode::on_status(
34-
autoware::component_interface_specs::vehicle::DoorStatus::Message::ConstSharedPtr msg)
49+
void VehicleDoorNode::on_command(
50+
const ExternalDoorCommand::Service::Request::SharedPtr req,
51+
const ExternalDoorCommand::Service::Response::SharedPtr res)
3552
{
36-
utils::notify(
37-
pub_status_, status_, *msg,
38-
utils::ignore_stamp<autoware::component_interface_specs::vehicle::DoorStatus::Message>);
53+
// For safety, do not open the door if the vehicle is not stopped.
54+
// https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/vehicle/doors/command/
55+
if (!is_stop_mode_ || (!is_autoware_control_ && check_autoware_control_)) {
56+
bool is_open = false;
57+
for (const auto & door : req->doors) {
58+
if (door.command == autoware_adapi_v1_msgs::msg::DoorCommand::OPEN) {
59+
is_open = true;
60+
break;
61+
}
62+
}
63+
if (is_open) {
64+
res->status.success = false;
65+
res->status.code = autoware_adapi_v1_msgs::msg::ResponseStatus::UNKNOWN;
66+
res->status.message = "Doors cannot be opened if the vehicle is not stopped.";
67+
return;
68+
}
69+
}
70+
autoware::component_interface_utils::status::copy(cli_command_->call(req), res);
3971
}
4072

4173
} // namespace autoware::default_adapi

‎system/autoware_default_adapi/src/vehicle_door.hpp

+28-9
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,9 @@
1616
#define VEHICLE_DOOR_HPP_
1717

1818
#include <autoware/adapi_specs/vehicle.hpp>
19+
#include <autoware/component_interface_specs/system.hpp>
1920
#include <autoware/component_interface_specs/vehicle.hpp>
21+
#include <autoware/component_interface_utils/status.hpp>
2022
#include <rclcpp/rclcpp.hpp>
2123

2224
#include <optional>
@@ -33,16 +35,33 @@ class VehicleDoorNode : public rclcpp::Node
3335
explicit VehicleDoorNode(const rclcpp::NodeOptions & options);
3436

3537
private:
36-
void on_status(
37-
autoware::component_interface_specs::vehicle::DoorStatus::Message::ConstSharedPtr msg);
38+
using OperationModeState = autoware::component_interface_specs::system::OperationModeState;
39+
using InternalDoorStatus = autoware::component_interface_specs::vehicle::DoorStatus;
40+
using InternalDoorLayout = autoware::component_interface_specs::vehicle::DoorLayout;
41+
using InternalDoorCommand = autoware::component_interface_specs::vehicle::DoorCommand;
42+
using ExternalDoorStatus = autoware::adapi_specs::vehicle::DoorStatus;
43+
using ExternalDoorLayout = autoware::adapi_specs::vehicle::DoorLayout;
44+
using ExternalDoorCommand = autoware::adapi_specs::vehicle::DoorCommand;
45+
46+
void on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg);
47+
void on_status(InternalDoorStatus::Message::ConstSharedPtr msg);
48+
void on_command(
49+
const ExternalDoorCommand::Service::Request::SharedPtr req,
50+
const ExternalDoorCommand::Service::Response::SharedPtr res);
51+
3852
rclcpp::CallbackGroup::SharedPtr group_cli_;
39-
Srv<autoware::adapi_specs::vehicle::DoorCommand> srv_command_;
40-
Srv<autoware::adapi_specs::vehicle::DoorLayout> srv_layout_;
41-
Pub<autoware::adapi_specs::vehicle::DoorStatus> pub_status_;
42-
Cli<autoware::component_interface_specs::vehicle::DoorCommand> cli_command_;
43-
Cli<autoware::component_interface_specs::vehicle::DoorLayout> cli_layout_;
44-
Sub<autoware::component_interface_specs::vehicle::DoorStatus> sub_status_;
45-
std::optional<autoware::component_interface_specs::vehicle::DoorStatus::Message> status_;
53+
Srv<ExternalDoorCommand> srv_command_;
54+
Srv<ExternalDoorLayout> srv_layout_;
55+
Pub<ExternalDoorStatus> pub_status_;
56+
Cli<InternalDoorCommand> cli_command_;
57+
Cli<InternalDoorLayout> cli_layout_;
58+
Sub<InternalDoorStatus> sub_status_;
59+
std::optional<InternalDoorStatus::Message> status_;
60+
61+
bool check_autoware_control_;
62+
bool is_autoware_control_;
63+
bool is_stop_mode_;
64+
Sub<OperationModeState> sub_operation_mode_;
4665
};
4766

4867
} // namespace autoware::default_adapi

‎system/autoware_default_adapi/test/node/interface_version.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@
3030

3131
if response.major != 1:
3232
exit(1)
33-
if response.minor != 5:
33+
if response.minor != 6:
3434
exit(1)
3535
if response.patch != 0:
3636
exit(1)

0 commit comments

Comments
 (0)