Skip to content

Commit b539910

Browse files
isamu-takagiisamu-takagi
committed
Merge branch 'main' into feat/mrm-route-rviz-plugin
2 parents 87ccfba + e329fda commit b539910

File tree

405 files changed

+5944
-21334
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

405 files changed

+5944
-21334
lines changed

‎.cspell-partial.json

+1-1
Original file line numberDiff line numberDiff line change
@@ -5,5 +5,5 @@
55
"perception/bytetrack/lib/**"
66
],
77
"ignoreRegExpList": [],
8-
"words": ["dltype", "tvmgen", "quantizer", "imageio", "mimsave"]
8+
"words": ["dltype", "tvmgen"]
99
}

‎.github/CODEOWNERS

+11-13
Original file line numberDiff line numberDiff line change
@@ -6,10 +6,10 @@ common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.w
66
common/autoware_auto_geometry/** satoshi.ota@tier4.jp
77
common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp
88
common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
9+
common/autoware_overlay_rviz_plugin/autoware_overlay_msgs/** team-spatzenhirn@uni-ulm.de
10+
common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai
911
common/autoware_point_types/** taichi.higashide@tier4.jp
1012
common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
11-
common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/** khalil@leodrive.ai
12-
common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/** team-spatzenhirn@uni-ulm.de
1313
common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp
1414
common/component_interface_specs/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp
1515
common/component_interface_tools/** isamu.takagi@tier4.jp
@@ -58,7 +58,7 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo
5858
common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp
5959
common/traffic_light_utils/** mingyu.li@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp
6060
common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp
61-
control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp
61+
control/autonomous_emergency_braking/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp
6262
control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
6363
control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
6464
control/external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
@@ -67,7 +67,7 @@ control/lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier
6767
control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
6868
control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
6969
control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp
70-
control/pid_longitudinal_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
70+
control/pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
7171
control/predicted_path_checker/** berkay@leodrive.ai
7272
control/pure_pursuit/** takamasa.horibe@tier4.jp
7373
control/shift_decider/** takamasa.horibe@tier4.jp
@@ -81,7 +81,7 @@ evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.j
8181
evaluator/tier4_metrics_rviz_plugin/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp
8282
launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp
8383
launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
84-
launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
84+
launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
8585
launch/tier4_map_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
8686
launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
8787
launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp zulfaqar.azmi@tier4.jp
@@ -98,6 +98,7 @@ localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.
9898
localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
9999
localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
100100
localization/pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
101+
localization/pose_estimator_arbiter/** kento.yabuuchi.2@tier4.jp
101102
localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
102103
localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
103104
localization/stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
@@ -112,7 +113,7 @@ map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuu
112113
map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
113114
map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
114115
map/map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
115-
map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp
116+
map/util/lanelet2_map_preprocessor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
116117
perception/bytetrack/** manato.hirabayashi@tier4.jp
117118
perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
118119
perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp
@@ -122,9 +123,7 @@ perception/detected_object_validation/** dai.nguyen@tier4.jp shunsuke.miura@tier
122123
perception/detection_by_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
123124
perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp
124125
perception/euclidean_cluster/** yukihiro.saito@tier4.jp
125-
perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp
126126
perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
127-
perception/heatmap_visualizer/** kotaro.uetake@tier4.jp
128127
perception/image_projection_based_fusion/** dai.nguyen@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
129128
perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp
130129
perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp
@@ -146,7 +145,6 @@ perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura
146145
perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
147146
perception/simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp
148147
perception/tensorrt_classifier/** mingyu.li@tier4.jp
149-
perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp
150148
perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp
151149
perception/tracking_object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
152150
perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp
@@ -155,16 +153,16 @@ perception/traffic_light_fine_detector/** shunsuke.miura@tier4.jp tao.zhong@tier
155153
perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp
156154
perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp
157155
perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp
158-
perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp
159156
perception/traffic_light_visualization/** yukihiro.saito@tier4.jp
160157
planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
161-
planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
158+
planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
162159
planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp
163160
planning/behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
164161
planning/behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
165-
planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
162+
planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
166163
planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
167164
planning/behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
165+
planning/behavior_path_sampling_planner_module/** daniel.sanchez@tier4.jp maxime.clement@tier4.jp
168166
planning/behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
169167
planning/behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
170168
planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
@@ -237,7 +235,7 @@ system/diagnostic_graph_aggregator/** isamu.takagi@tier4.jp
237235
system/dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp
238236
system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp
239237
system/duplicated_node_checker/** shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp
240-
system/emergency_handler/** makoto.kurihara@tier4.jp
238+
system/emergency_handler/** makoto.kurihara@tier4.jp ryuta.kambe@tier4.jp tetsuhiro.kawaguchi@tier4.jp
241239
system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp
242240
system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp
243241
system/system_error_monitor/** fumihito.ito@tier4.jp

‎common/tier4_adapi_rviz_plugin/src/route_panel.cpp

+99-18
Original file line numberDiff line numberDiff line change
@@ -14,11 +14,13 @@
1414

1515
#include "route_panel.hpp"
1616

17+
#include <QGridLayout>
1718
#include <QHBoxLayout>
1819
#include <QVBoxLayout>
1920
#include <rviz_common/display_context.hpp>
2021

2122
#include <memory>
23+
#include <string>
2224

2325
namespace tier4_adapi_rviz_plugins
2426
{
@@ -28,6 +30,11 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent)
2830
waypoints_mode_ = new QPushButton("mode");
2931
waypoints_reset_ = new QPushButton("reset");
3032
waypoints_apply_ = new QPushButton("apply");
33+
adapi_clear_ = new QPushButton("clear");
34+
adapi_set_ = new QPushButton("set");
35+
adapi_change_ = new QPushButton("change");
36+
adapi_response_ = new QLabel("the response will be displayed here");
37+
adapi_auto_clear_ = new QCheckBox("auto clear");
3138
allow_goal_modification_ = new QCheckBox("allow goal modification");
3239

3340
waypoints_mode_->setCheckable(true);
@@ -37,6 +44,22 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent)
3744
connect(waypoints_reset_, &QPushButton::clicked, this, &RoutePanel::onWaypointsReset);
3845
connect(waypoints_apply_, &QPushButton::clicked, this, &RoutePanel::onWaypointsApply);
3946

47+
const auto buttons = new QButtonGroup(this);
48+
buttons->addButton(adapi_set_);
49+
buttons->addButton(adapi_change_);
50+
buttons->setExclusive(true);
51+
adapi_set_->setCheckable(true);
52+
adapi_change_->setCheckable(true);
53+
adapi_response_->setAlignment(Qt::AlignCenter);
54+
55+
connect(adapi_clear_, &QPushButton::clicked, this, &RoutePanel::clearRoute);
56+
connect(adapi_set_, &QPushButton::clicked, [this] { adapi_mode_ = AdapiMode::Set; });
57+
connect(adapi_change_, &QPushButton::clicked, [this] { adapi_mode_ = AdapiMode::Change; });
58+
59+
adapi_auto_clear_->setChecked(true);
60+
adapi_set_->setChecked(true);
61+
adapi_mode_ = AdapiMode::Set;
62+
4063
const auto layout = new QVBoxLayout();
4164
setLayout(layout);
4265

@@ -52,6 +75,19 @@ RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent)
5275
waypoints_group_ = group;
5376
}
5477

78+
// adapi group
79+
{
80+
const auto group = new QGroupBox("adapi");
81+
const auto local = new QGridLayout();
82+
local->addWidget(adapi_clear_, 0, 0);
83+
local->addWidget(adapi_set_, 0, 1);
84+
local->addWidget(adapi_change_, 0, 2);
85+
local->addWidget(adapi_auto_clear_, 1, 0);
86+
local->addWidget(adapi_response_, 1, 1, 1, 2);
87+
group->setLayout(local);
88+
layout->addWidget(group);
89+
}
90+
5591
// options group
5692
{
5793
const auto group = new QGroupBox("options");
@@ -73,25 +109,14 @@ void RoutePanel::onInitialize()
73109

74110
const auto adaptor = component_interface_utils::NodeAdaptor(node.get());
75111
adaptor.init_cli(cli_clear_);
76-
adaptor.init_cli(cli_route_);
77-
}
78-
79-
void RoutePanel::setRoute(const PoseStamped & pose)
80-
{
81-
const auto req = std::make_shared<ClearRoute::Service::Request>();
82-
cli_clear_->async_send_request(req, [this, pose](auto) {
83-
const auto req = std::make_shared<SetRoutePoints::Service::Request>();
84-
req->header = pose.header;
85-
req->goal = pose.pose;
86-
req->option.allow_goal_modification = allow_goal_modification_->isChecked();
87-
cli_route_->async_send_request(req);
88-
});
112+
adaptor.init_cli(cli_set_);
113+
adaptor.init_cli(cli_change_);
89114
}
90115

91116
void RoutePanel::onPose(const PoseStamped::ConstSharedPtr msg)
92117
{
93118
if (!waypoints_mode_->isChecked()) {
94-
setRoute(*msg);
119+
requestRoute(*msg);
95120
} else {
96121
waypoints_.push_back(*msg);
97122
waypoints_group_->setTitle(QString("waypoints (count: %1)").arg(waypoints_.size()));
@@ -120,18 +145,74 @@ void RoutePanel::onWaypointsApply()
120145
{
121146
if (waypoints_.empty()) return;
122147

123-
const auto req = std::make_shared<ClearRoute::Service::Request>();
124-
cli_clear_->async_send_request(req, [this](auto) {
148+
const auto call = [this] {
125149
const auto req = std::make_shared<SetRoutePoints::Service::Request>();
126150
req->header = waypoints_.back().header;
127151
req->goal = waypoints_.back().pose;
128152
for (size_t i = 0; i + 1 < waypoints_.size(); ++i) {
129153
req->waypoints.push_back(waypoints_[i].pose);
130154
}
131155
req->option.allow_goal_modification = allow_goal_modification_->isChecked();
132-
cli_route_->async_send_request(req);
156+
asyncSendRequest(req);
133157
onWaypointsReset();
134-
});
158+
};
159+
160+
if (adapi_mode_ == AdapiMode::Set && adapi_auto_clear_->isChecked()) {
161+
const auto req = std::make_shared<ClearRoute::Service::Request>();
162+
cli_clear_->async_send_request(req, [call](auto) { call(); });
163+
} else {
164+
call();
165+
}
166+
}
167+
168+
void RoutePanel::requestRoute(const PoseStamped & pose)
169+
{
170+
const auto call = [this, pose] {
171+
const auto req = std::make_shared<SetRoutePoints::Service::Request>();
172+
req->header = pose.header;
173+
req->goal = pose.pose;
174+
req->option.allow_goal_modification = allow_goal_modification_->isChecked();
175+
asyncSendRequest(req);
176+
};
177+
178+
if (adapi_mode_ == AdapiMode::Set && adapi_auto_clear_->isChecked()) {
179+
const auto req = std::make_shared<ClearRoute::Service::Request>();
180+
cli_clear_->async_send_request(req, [call](auto) { call(); });
181+
} else {
182+
call();
183+
}
184+
}
185+
186+
void RoutePanel::clearRoute()
187+
{
188+
const auto callback = [this](auto future) {
189+
const auto status = future.get()->status;
190+
std::string text = status.success ? "OK" : "Error";
191+
text += "[" + std::to_string(status.code) + "]";
192+
text += " " + status.message;
193+
adapi_response_->setText(QString::fromStdString(text));
194+
};
195+
196+
const auto req = std::make_shared<ClearRoute::Service::Request>();
197+
cli_clear_->async_send_request(req, callback);
198+
}
199+
200+
void RoutePanel::asyncSendRequest(SetRoutePoints::Service::Request::SharedPtr req)
201+
{
202+
const auto callback = [this](auto future) {
203+
const auto status = future.get()->status;
204+
std::string text = status.success ? "OK" : "Error";
205+
text += "[" + std::to_string(status.code) + "]";
206+
text += " " + status.message;
207+
adapi_response_->setText(QString::fromStdString(text));
208+
};
209+
210+
if (adapi_mode_ == AdapiMode::Set) {
211+
cli_set_->async_send_request(req, callback);
212+
}
213+
if (adapi_mode_ == AdapiMode::Change) {
214+
cli_change_->async_send_request(req, callback);
215+
}
135216
}
136217

137218
} // namespace tier4_adapi_rviz_plugins

0 commit comments

Comments
 (0)