Merge branch 'feature/Trajectories' into 'master'

Trajectories

See merge request eclipse/simopenpass/scenario_api!14

MantleAPI/include/MantleAPI/Common/poly_line.h

+/*******************************************************************************
+ * 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

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

MantleAPI/include/MantleAPI/Common/trajectory.h

+/*******************************************************************************
+ * 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

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
@@ -130,8 +132,6 @@ struct FollowLateralOffsetSplineControlStrategy : public ControlStrategy
     std::vector<mantle_api::SplineSection> lateral_offset_splines;
 };
 
-// TODO: Create new control strategy for following 3D trajectories with shapes NURBS, polyline, clothoid
-
 struct FollowRouteControlStrategy : public ControlStrategy
 {
     FollowRouteControlStrategy()
@@ -164,7 +164,7 @@ struct TransitionDynamics
 {
     Dimension dimension{Dimension::kUndefined};
     Shape shape{Shape::kUndefined};
-    double value{0};
+    double value{0.0};
 };
 
 struct AcquireLaneOffsetControlStrategy : public ControlStrategy
@@ -181,6 +181,33 @@ struct AcquireLaneOffsetControlStrategy : public ControlStrategy
     TransitionDynamics transition_dynamics;
 };
 
+enum class ReferenceContext
+{
+    kAbsolute = 0,
+    kRelative
+};
+
+struct FollowTrajectoryControlStrategy : public ControlStrategy
+{
+    // TODO: Extend the FollowTrajectoryControlStrategy to support shapes like NURBS and clothoid
+
+    struct TrajectoryTimeReference
+    {
+        ReferenceContext domainAbsoluteRelative;
+        double scale{0.0};
+        double offset{0.0};
+    };
+    
+    FollowTrajectoryControlStrategy()
+    {
+        movement_domain = MovementDomain::kBoth;
+        type = ControlStrategyType::kFollowTrajectory;
+    }
+
+    Trajectory trajectory;
+    std::optional<TrajectoryTimeReference> timeReference;
+};
+
 }  // namespace mantle_api
 
 #endif  // MANTLEAPI_TRAFFIC_CONTROLSTRATEGY_H