Skip to content

Commit 726f0f9

Browse files
Merge branch 'awf-latest' into fix/remove_front_points
2 parents bb3bb3f + 1121f3b commit 726f0f9

File tree

112 files changed

+997
-4807
lines changed

Some content is hidden

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

112 files changed

+997
-4807
lines changed

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

common/tier4_adapi_rviz_plugin/src/route_panel.hpp

+16-2
Original file line numberDiff line numberDiff line change
@@ -15,8 +15,10 @@
1515
#ifndef ROUTE_PANEL_HPP_
1616
#define ROUTE_PANEL_HPP_
1717

18+
#include <QButtonGroup>
1819
#include <QCheckBox>
1920
#include <QGroupBox>
21+
#include <QLabel>
2022
#include <QPushButton>
2123
#include <autoware_ad_api_specs/routing.hpp>
2224
#include <component_interface_utils/rclcpp.hpp>
@@ -35,6 +37,7 @@ class RoutePanel : public rviz_common::Panel
3537
Q_OBJECT
3638
using ClearRoute = autoware_ad_api::routing::ClearRoute;
3739
using SetRoutePoints = autoware_ad_api::routing::SetRoutePoints;
40+
using ChangeRoutePoints = autoware_ad_api::routing::ChangeRoutePoints;
3841
using PoseStamped = geometry_msgs::msg::PoseStamped;
3942

4043
public:
@@ -45,18 +48,29 @@ class RoutePanel : public rviz_common::Panel
4548
QPushButton * waypoints_mode_;
4649
QPushButton * waypoints_reset_;
4750
QPushButton * waypoints_apply_;
51+
QPushButton * adapi_clear_;
52+
QPushButton * adapi_set_;
53+
QPushButton * adapi_change_;
54+
QLabel * adapi_response_;
55+
QCheckBox * adapi_auto_clear_;
4856
QGroupBox * waypoints_group_;
4957
QCheckBox * allow_goal_modification_;
5058

5159
rclcpp::Subscription<PoseStamped>::SharedPtr sub_pose_;
5260
std::vector<PoseStamped> waypoints_;
5361
void onPose(const PoseStamped::ConstSharedPtr msg);
5462

63+
enum AdapiMode { Set, Change };
64+
AdapiMode adapi_mode_;
65+
5566
component_interface_utils::Client<ClearRoute>::SharedPtr cli_clear_;
56-
component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_route_;
57-
void setRoute(const PoseStamped & pose);
67+
component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_set_;
68+
component_interface_utils::Client<ChangeRoutePoints>::SharedPtr cli_change_;
69+
void requestRoute(const PoseStamped & pose);
70+
void asyncSendRequest(SetRoutePoints::Service::Request::SharedPtr req);
5871

5972
private slots:
73+
void clearRoute();
6074
void onWaypointsMode(bool clicked);
6175
void onWaypointsReset();
6276
void onWaypointsApply();

control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

+18-4
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@
3030

3131
#include <cmath>
3232
#include <limits>
33+
#include <utility>
3334

3435
namespace autoware::motion::control::pid_longitudinal_controller
3536
{
@@ -62,11 +63,11 @@ double getPitchByPose(const Quaternion & quaternion);
6263
* @brief calculate pitch angle from trajectory on map
6364
* NOTE: there is currently no z information so this always returns 0.0
6465
* @param [in] trajectory input trajectory
65-
* @param [in] closest_idx nearest index to current vehicle position
66+
* @param [in] start_idx nearest index to current vehicle position
6667
* @param [in] wheel_base length of wheel base
6768
*/
6869
double getPitchByTraj(
69-
const Trajectory & trajectory, const size_t closest_idx, const double wheel_base);
70+
const Trajectory & trajectory, const size_t start_idx, const double wheel_base);
7071

7172
/**
7273
* @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and
@@ -82,7 +83,7 @@ Pose calcPoseAfterTimeDelay(
8283
* @param [in] point Interpolated point is nearest to this point.
8384
*/
8485
template <class T>
85-
TrajectoryPoint lerpTrajectoryPoint(
86+
std::pair<TrajectoryPoint, size_t> lerpTrajectoryPoint(
8687
const T & points, const Pose & pose, const double max_dist, const double max_yaw)
8788
{
8889
TrajectoryPoint interpolated_point;
@@ -101,6 +102,8 @@ TrajectoryPoint lerpTrajectoryPoint(
101102
points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio);
102103
interpolated_point.pose.position.y = interpolation::lerp(
103104
points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio);
105+
interpolated_point.pose.position.z = interpolation::lerp(
106+
points.at(i).pose.position.z, points.at(i + 1).pose.position.z, interpolate_ratio);
104107
interpolated_point.pose.orientation = interpolation::lerpOrientation(
105108
points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio);
106109
interpolated_point.longitudinal_velocity_mps = interpolation::lerp(
@@ -114,7 +117,7 @@ TrajectoryPoint lerpTrajectoryPoint(
114117
points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio);
115118
}
116119

117-
return interpolated_point;
120+
return std::make_pair(interpolated_point, seg_idx);
118121
}
119122

120123
/**
@@ -138,6 +141,17 @@ double applyDiffLimitFilter(
138141
double applyDiffLimitFilter(
139142
const double input_val, const double prev_val, const double dt, const double max_val,
140143
const double min_val);
144+
145+
/**
146+
* @brief calculate the projected pose after distance from the current index
147+
* @param [in] src_idx current index
148+
* @param [in] distance distance to project
149+
* @param [in] trajectory reference trajectory
150+
*/
151+
geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
152+
const size_t src_idx, const double distance,
153+
const autoware_auto_planning_msgs::msg::Trajectory & trajectory);
154+
141155
} // namespace longitudinal_utils
142156
} // namespace autoware::motion::control::pid_longitudinal_controller
143157

0 commit comments

Comments
 (0)