add CAM TS access functions

etsi_its_msgs_utils/include/etsi_its_msgs_utils/cam_ts_access.h

@@ -0,0 +1,43 @@
+/*
+=============================================================================
+MIT License
+
+Copyright (c) 2023-2024 Institute for Automotive Engineering (ika), RWTH Aachen University
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+=============================================================================
+*/
+
+/**
+ * @file cam_ts_access.h
+ * @brief Main CAM access header to include in ROS 1 projects
+ */
+
+#pragma once
+
+// Messages
+#include <etsi_its_cam_ts_msgs/CAM.h>
+#include <geometry_msgs/PointStamped.h>
+
+namespace etsi_its_cam_ts_msgs {
+    namespace gm = geometry_msgs;
+}
+
+// Implementation
+#include <etsi_its_msgs_utils/impl/cam/cam_ts_access.h>

etsi_its_msgs_utils/include/etsi_its_msgs_utils/cam_ts_access.hpp

@@ -0,0 +1,44 @@
+/*
+=============================================================================
+MIT License
+
+Copyright (c) 2023-2024 Institute for Automotive Engineering (ika), RWTH Aachen University
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+=============================================================================
+*/
+
+/**
+ * @file cam_ts_access.hpp
+ * @brief Main CAM access header to include in ROS 2 projects
+ */
+
+#pragma once
+
+// Messages
+#include <etsi_its_cam_ts_msgs/msg/cam.hpp>
+#include <geometry_msgs/msg/point_stamped.hpp>
+
+namespace etsi_its_cam_ts_msgs {
+    using namespace msg;
+    namespace gm = geometry_msgs::msg;
+}
+
+// Implementation
+#include <etsi_its_msgs_utils/impl/cam/cam_ts_access.h>

etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_access.h

@@ -0,0 +1,45 @@
+/*
+=============================================================================
+MIT License
+
+Copyright (c) 2023-2024 Institute for Automotive Engineering (ika), RWTH Aachen University
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+=============================================================================
+*/
+
+/**
+ * @file impl/cam/cam_ts_access.h
+ * @brief Main CAM TS access implementation header
+ */
+
+#pragma once
+
+#include <cstring>
+#include <iostream>
+#include <map>
+
+#include <GeographicLib/UTMUPS.hpp>
+
+#include <etsi_its_msgs_utils/impl/cam/cam_ts_getters.h>
+#include <etsi_its_msgs_utils/impl/cam/cam_ts_setters.h>
+
+namespace etsi_its_cam_ts_msgs::access {
+#include <etsi_its_msgs_utils/impl/cam/cam_utils.h>
+} // namespace etsi_its_cam_ts_msgs::access

etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_getters.h

@@ -0,0 +1,37 @@
+/*
+=============================================================================
+MIT License
+
+Copyright (c) 2023-2024 Institute for Automotive Engineering (ika), RWTH Aachen University
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+=============================================================================
+*/
+
+/**
+ * @file impl/cam/cam_ts_getters.h
+ * @brief Getter functions for the ETSI ITS CAM (TS)
+ */
+
+#pragma once
+
+namespace etsi_its_cam_ts_msgs::access {
+#include <etsi_its_msgs_utils/impl/cdd/cdd_v2_getters.h>
+#include <etsi_its_msgs_utils/impl/cam/cam_getters_common.h>
+} // namespace etsi_its_cam_ts_msgs::access

etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cam/cam_ts_setters.h

@@ -0,0 +1,52 @@
+/*
+=============================================================================
+MIT License
+
+Copyright (c) 2023-2024 Institute for Automotive Engineering (ika), RWTH Aachen University
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+=============================================================================
+*/
+
+/**
+ * @file impl/cam/cam_ts_setters.h
+ * @brief Setter functions for the ETSI ITS CAM (TS)
+ */
+
+#pragma once
+
+#include <etsi_its_msgs_utils/impl/constants.h>
+
+namespace etsi_its_cam_ts_msgs::access {
+
+#include <etsi_its_msgs_utils/impl/cdd/cdd_v2_setters.h>
+#include <etsi_its_msgs_utils/impl/cam/cam_setters_common.h>
+
+  /**
+   * @brief Set the ItsPduHeader-object for a CAM
+   *
+   * @param cam CAM-Message to set the ItsPduHeader
+   * @param station_id
+   * @param protocol_version
+   */
+  inline void setItsPduHeader(CAM& cam, const uint32_t station_id, const uint8_t protocol_version = 0){
+    setItsPduHeader(cam.header, MessageId::CAM, station_id, protocol_version);
+  }
+
+} // namespace etsi_its_cam_ts_msgs::access

etsi_its_msgs_utils/include/etsi_its_msgs_utils/impl/cdd/cdd_v2_getters.h

@@ -0,0 +1,82 @@
+/*
+=============================================================================
+MIT License
+
+Copyright (c) 2023-2024 Institute for Automotive Engineering (ika), RWTH Aachen University
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+=============================================================================
+*/
+
+/**
+ * @file impl/cdd/cdd_v2_getters.h
+ * @brief Getter functions for the ETSI ITS Common Data Dictionary (CDD) v2.1.1
+ */
+#pragma once
+
+#include <GeographicLib/UTMUPS.hpp>
+#include <etsi_its_msgs_utils/impl/cdd/cdd_getters_common.h>
+
+/**
+ * @brief Get the lateral acceleration
+ *
+ * @param longitudinalAcceleration to get the lateral acceleration from
+ * @return lateral acceleration in m/s^2 as decimal number (left is positive)
+ */
+inline double getLongitudinalAcceleration(const AccelerationComponent& longitudinal_acceleration){
+  return ((int16_t)longitudinal_acceleration.value.value)*1e-1;
+}
+
+/**
+ * @brief Get the lateral acceleration
+ *
+ * @param lateralAcceleration to get the lateral acceleration from
+ * @return lateral acceleration in m/s^2 as decimal number (left is positive)
+ */
+inline double getLateralAcceleration(const AccelerationComponent& lateral_acceleration){
+  return ((int16_t)lateral_acceleration.value.value)*1e-1;
+}
+
+/**
+ * @brief Get the UTM Position defined by the given ReferencePosition
+ *
+ * The position is transformed into UTM by using GeographicLib::UTMUPS
+ * The altitude value is directly used as z-Coordinate
+ *
+ * @param[in] reference_position ReferencePosition to get the UTM Position from
+ * @param[out] zone the UTM zone (zero means UPS)
+ * @param[out] northp hemisphere (true means north, false means south)
+ * @return gm::PointStamped geometry_msgs::PointStamped of the given position
+ */
+inline gm::PointStamped getUTMPosition(const ReferencePositionWithConfidence& reference_position, int& zone, bool& northp){
+  gm::PointStamped utm_point;
+  double latitude = getLatitude(reference_position.latitude);
+  double longitude = getLongitude(reference_position.longitude);
+  utm_point.point.z = getAltitude(reference_position.altitude);
+  try {
+    GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, utm_point.point.x, utm_point.point.y);
+    std::string hemisphere;
+    if(northp) hemisphere="N";
+    else hemisphere="S";
+    utm_point.header.frame_id="utm_"+std::to_string(zone)+hemisphere;
+  } catch (GeographicLib::GeographicErr& e) {
+    throw std::invalid_argument(e.what());
+  }
+  return utm_point;
+}