From 79ac383ba09fd84ada26bb1a0941edc746d059b5 Mon Sep 17 00:00:00 2001
From: Christoph Kochendoerfer <christoph.kochendoerfer@in-tech.com>
Date: Fri, 23 Jul 2021 07:41:17 +0200
Subject: [PATCH] Added Trajectory and Polyline
---
.../include/MantleAPI/Common/poly_line.h | 57 +++++++++++++++++++
MantleAPI/include/MantleAPI/Common/pose.h | 23 ++++++++
.../include/MantleAPI/Common/trajectory.h | 53 +++++++++++++++++
.../MantleAPI/Traffic/control_strategy.h | 23 +++++++-
4 files changed, 155 insertions(+), 1 deletion(-)
create mode 100644 MantleAPI/include/MantleAPI/Common/poly_line.h
create mode 100644 MantleAPI/include/MantleAPI/Common/trajectory.h
diff --git a/MantleAPI/include/MantleAPI/Common/poly_line.h b/MantleAPI/include/MantleAPI/Common/poly_line.h
new file mode 100644
index 00000000..7a00d8cd
--- /dev/null
+++ b/MantleAPI/include/MantleAPI/Common/poly_line.h
@@ -0,0 +1,57 @@
+/*******************************************************************************
+ * Copyright (c) 2021, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ *
+ * This program and the accompanying materials are made
+ * available under the terms of the Eclipse Public License 2.0
+ * which is available at https://www.eclipse.org/legal/epl-2.0/
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *******************************************************************************/
+
+//-----------------------------------------------------------------------------
+/** @file poly_line.h */
+//-----------------------------------------------------------------------------
+
+#ifndef MANTLEAPI_COMMON_POLY_LINE_H
+#define MANTLEAPI_COMMON_POLY_LINE_H
+
+#include <MantleAPI/Common/pose.h>
+#include <MantleAPI/Common/time_utils.h>
+
+#include <optional>
+#include <vector>
+
+namespace mantle_api
+{
+
+struct PolyLinePoint
+{
+ Pose pose{};
+ std::optional<Time> time{};
+
+ bool operator== (const PolyLinePoint& other) const
+ {
+ return other.time == time
+ && other.pose == pose;
+ }
+
+ friend std::ostream& operator<<(std::ostream& os, const PolyLinePoint& polyLinePoint);
+};
+
+inline std::ostream& operator<<(std::ostream& os, const PolyLinePoint& polyLinePoint)
+{
+ os << polyLinePoint.pose;
+
+ if(polyLinePoint.time.has_value())
+ {
+ os << ", time in ms " << polyLinePoint.time.value().count();
+ }
+
+ return os;
+}
+
+using PolyLine = std::vector<PolyLinePoint>;
+
+} // namespace mantle_api
+
+#endif // MANTLEAPI_COMMON_POLY_LINE_H
\ No newline at end of file
diff --git a/MantleAPI/include/MantleAPI/Common/pose.h b/MantleAPI/include/MantleAPI/Common/pose.h
index 0cbdd8e6..85779cb8 100644
--- a/MantleAPI/include/MantleAPI/Common/pose.h
+++ b/MantleAPI/include/MantleAPI/Common/pose.h
@@ -17,6 +17,7 @@
#include <MantleAPI/Common/orientation.h>
#include <MantleAPI/Common/vector.h>
+#include <iostream>
namespace mantle_api
{
@@ -24,8 +25,30 @@ struct Pose
{
Vec3d position{};
Orientation3d orientation{};
+
+ bool operator== (const Pose& other) const
+ {
+ return other.position == position
+ && other.orientation == orientation;
+ }
+
+ friend inline std::ostream& operator<<(std::ostream& os, const Pose& pose);
};
+std::ostream& operator<<(std::ostream& os, const Pose& pose)
+{
+ os << "position ("
+ << pose.position.x
+ << ", " << pose.position.y
+ << ", " << pose.position.z
+ << "), orientation (" << pose.orientation.yaw
+ << ", " << pose.orientation.pitch
+ << ", " << pose.orientation.roll
+ << ")";
+
+ return os;
+}
+
} // namespace mantle_api
#endif // MANTLEAPI_COMMON_POSE_H
diff --git a/MantleAPI/include/MantleAPI/Common/trajectory.h b/MantleAPI/include/MantleAPI/Common/trajectory.h
new file mode 100644
index 00000000..29291b42
--- /dev/null
+++ b/MantleAPI/include/MantleAPI/Common/trajectory.h
@@ -0,0 +1,53 @@
+/*******************************************************************************
+ * Copyright (c) 2021, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ *
+ * This program and the accompanying materials are made
+ * available under the terms of the Eclipse Public License 2.0
+ * which is available at https://www.eclipse.org/legal/epl-2.0/
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *******************************************************************************/
+
+//-----------------------------------------------------------------------------
+/** @file trajectory.h */
+//-----------------------------------------------------------------------------
+
+#ifndef MANTLEAPI_COMMON_TRAJECTORY_H
+#define MANTLEAPI_COMMON_TRAJECTORY_H
+
+#include <string>
+#include <variant>
+
+#include <MantleAPI/Common/poly_line.h>
+
+namespace mantle_api
+{
+struct Trajectory
+{
+ std::string name;
+ std::variant<PolyLine> type;
+
+ friend std::ostream& operator<<(std::ostream& os, const Trajectory& trajectory);
+};
+
+inline std::ostream& operator<<(std::ostream& os, const Trajectory& trajectory)
+{
+ os << "Trajectory \"" << trajectory.name;
+
+ if(std::holds_alternative<PolyLine>(trajectory.type))
+ {
+ const auto &polyLine = std::get<PolyLine>(trajectory.type);
+ for (const auto &polyLinePoint : polyLine)
+ {
+ os << polyLinePoint;
+ }
+ }
+
+ os << "\"\n";
+
+ return os;
+}
+
+} // namespace mantle_api
+
+#endif // MANTLEAPI_COMMON_TRAJECTORY_H
\ No newline at end of file
diff --git a/MantleAPI/include/MantleAPI/Traffic/control_strategy.h b/MantleAPI/include/MantleAPI/Traffic/control_strategy.h
index 42a736d4..7c551a8c 100644
--- a/MantleAPI/include/MantleAPI/Traffic/control_strategy.h
+++ b/MantleAPI/include/MantleAPI/Traffic/control_strategy.h
@@ -17,6 +17,7 @@
#include <MantleAPI/Common/spline.h>
#include <MantleAPI/Common/vector.h>
+#include <MantleAPI/Common/trajectory.h>
#include <vector>
@@ -40,7 +41,8 @@ enum class ControlStrategyType
kFollowLateralOffsetSpline,
kFollowVelocitySpline,
kFollowRoute,
- kAcquireLaneOffset
+ kAcquireLaneOffset,
+ kFollowTrajectory
};
struct ControlStrategy
@@ -181,6 +183,25 @@ struct AcquireLaneOffsetControlStrategy : public ControlStrategy
TransitionDynamics transition_dynamics;
};
+struct FollowTrajectoryControlStrategy : public ControlStrategy
+{
+ struct TrajectoryTimeReference
+ {
+ std::string domainAbsoluteRelative;
+ double scale;
+ double offset;
+ };
+
+ FollowTrajectoryControlStrategy()
+ {
+ movement_domain = MovementDomain::kBoth;
+ type = ControlStrategyType::kAcquireLaneOffset;
+ }
+
+ Trajectory trajectory;
+ std::optional<TrajectoryTimeReference> timeReference;
+};
+
} // namespace mantle_api
#endif // MANTLEAPI_TRAFFIC_CONTROLSTRATEGY_H
--
GitLab