RJD-1057: Reorganize ostream operators

simulation/traffic_simulator/CMakeLists.txt

@@ -46,6 +46,7 @@ ament_auto_add_library(traffic_simulator SHARED
   src/entity/vehicle_entity.cpp
   src/hdmap_utils/hdmap_utils.cpp
   src/helper/helper.cpp
+  src/helper/ostream_helpers.cpp
   src/job/job.cpp
   src/job/job_list.cpp
   src/lanelet_wrapper/lanelet_loader.cpp

simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp

@@ -15,15 +15,8 @@
 #ifndef TRAFFIC_SIMULATOR__DATA_TYPE__LANELET_POSE_HPP_
 #define TRAFFIC_SIMULATOR__DATA_TYPE__LANELET_POSE_HPP_
 
-#include <lanelet2_core/geometry/Lanelet.h>
-
-#include <geometry_msgs/msg/pose.hpp>
-#include <optional>
-#include <scenario_simulator_exception/exception.hpp>
-#include <traffic_simulator/data_type/routing_configuration.hpp>
-#include <traffic_simulator/data_type/routing_graph_type.hpp>
 #include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
-#include <traffic_simulator_msgs/msg/lanelet_pose.hpp>
+#include <traffic_simulator/helper/ostream_helpers.hpp>
 
 namespace traffic_simulator
 {
@@ -81,6 +74,26 @@ class CanonicalizedLaneletPose
   DEFINE_COMPARISON_OPERATOR(>)
 #undef DEFINE_COMPARISON_OPERATOR
 
+  friend std::ostream & operator<<(std::ostream & os, const CanonicalizedLaneletPose & obj)
+  {
+    os << "CanonicalizedLaneletPose(\n";
+    os << obj.lanelet_pose_ << "\n";
+    if (obj.lanelet_poses_.size() == 1) {
+      os << "  alternative from lanelet_poses_: " << obj.lanelet_poses_.front() << "\n";
+    } else if (obj.lanelet_poses_.size() > 1) {
+      os << "  lanelet_poses_: [\n";
+      for (const auto & pose : obj.lanelet_poses_) {
+        os << "    - " << pose << "\n";
+      }
+      os << "  ]\n";
+    }
+    os << "  map_pose_: " << obj.map_pose_ << "\n";
+    os << "  consider_pose_by_road_slope_: "
+       << (CanonicalizedLaneletPose::consider_pose_by_road_slope_ ? "true" : "false") << "\n";
+    os << ")";
+    return os;
+  }
+
 private:
   auto adjustOrientationAndOzPosition() -> void;
   LaneletPose lanelet_pose_;

simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp

@@ -153,17 +153,6 @@ const simulation_api_schema::DetectionSensorConfiguration constructDetectionSens
 }  // namespace helper
 }  // namespace traffic_simulator
 
-std::ostream & operator<<(
-  std::ostream & os, const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose);
-
-std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Point & point);
-
-std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Vector3 & vector);
-
-std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Quaternion & quat);
-
-std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Pose & pose);
-
 template <typename T>
 auto operator+(const std::vector<T> & v0, const std::vector<T> & v1) -> decltype(auto)
 {
@@ -189,5 +178,4 @@ auto sortAndUnique(const std::vector<T> & data) -> std::vector<T>
   ret.erase(std::unique(ret.begin(), ret.end()), ret.end());
   return ret;
 }
-
 #endif  // TRAFFIC_SIMULATOR__HELPER__HELPER_HPP_

simulation/traffic_simulator/include/traffic_simulator/helper/ostream_helpers.hpp

@@ -0,0 +1,60 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TRAFFIC_SIMULATOR_HELPER__OSTREAM_HELPERS_HPP_
+#define TRAFFIC_SIMULATOR_HELPER__OSTREAM_HELPERS_HPP_
+
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <traffic_simulator_msgs/msg/axle.hpp>
+#include <traffic_simulator_msgs/msg/axles.hpp>
+#include <traffic_simulator_msgs/msg/bounding_box.hpp>
+#include <traffic_simulator_msgs/msg/entity_subtype.hpp>
+#include <traffic_simulator_msgs/msg/lanelet_pose.hpp>
+#include <traffic_simulator_msgs/msg/performance.hpp>
+#include <traffic_simulator_msgs/msg/vehicle_parameters.hpp>
+
+namespace traffic_simulator
+{
+std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Point & point);
+
+std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Vector3 & vector);
+
+std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Quaternion & quat);
+
+std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Pose & pose);
+
+std::ostream & operator<<(
+  std::ostream & os, const traffic_simulator_msgs::msg::LaneletPose & ll_pose);
+
+std::ostream & operator<<(
+  std::ostream & os, const traffic_simulator_msgs::msg::EntitySubtype & subtype);
+
+std::ostream & operator<<(std::ostream & os, const traffic_simulator_msgs::msg::Axle & axle);
+
+std::ostream & operator<<(std::ostream & os, const traffic_simulator_msgs::msg::Axles & axles);
+
+std::ostream & operator<<(
+  std::ostream & os, const traffic_simulator_msgs::msg::BoundingBox & bounding_box);
+
+std::ostream & operator<<(
+  std::ostream & os, const traffic_simulator_msgs::msg::Performance & performance);
+
+std::ostream & operator<<(
+  std::ostream & os, const traffic_simulator_msgs::msg::VehicleParameters & params);
+}  // namespace traffic_simulator
+
+#endif  // TRAFFIC_SIMULATOR_HELPER__OSTREAM_HELPERS_HPP_

simulation/traffic_simulator/src/helper/helper.cpp

@@ -186,38 +186,3 @@ const simulation_api_schema::LidarConfiguration constructLidarConfiguration(
 
 }  // namespace helper
 }  // namespace traffic_simulator
-
-std::ostream & operator<<(
-  std::ostream & os, const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose)
-{
-  os << "lanelet id : " << lanelet_pose.lanelet_id << "\ns : " << lanelet_pose.s;
-  return os;
-}
-
-std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Point & point)
-{
-  os << "x : " << point.x << ",y : " << point.y << ",z : " << point.z << std::endl;
-  return os;
-}
-
-std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Vector3 & vector)
-{
-  os << "x : " << vector.x << ",y : " << vector.y << ",z : " << vector.z << std::endl;
-  return os;
-}
-
-std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Quaternion & quat)
-{
-  os << "x : " << quat.x << ",y : " << quat.y << ",z : " << quat.z << ",w : " << quat.w
-     << std::endl;
-  return os;
-}
-
-std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Pose & pose)
-{
-  os << "position : " << std::endl;
-  os << pose.position << std::endl;
-  os << "orientation : " << std::endl;
-  os << pose.orientation << std::endl;
-  return os;
-}

simulation/traffic_simulator/src/helper/ostream_helpers.cpp

@@ -0,0 +1,133 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <traffic_simulator/helper/ostream_helpers.hpp>
+
+namespace traffic_simulator
+{
+// basic ros types
+std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Point & point)
+{
+  os << "Point(x: " << point.x << ", y: " << point.y << ", z: " << point.z << ")";
+  return os;
+}
+
+std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Vector3 & vector)
+{
+  os << "Vector3(x: " << vector.x << ", y: " << vector.y << ", z: " << vector.z << ")";
+  return os;
+}
+
+std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Quaternion & quat)
+{
+  os << "Quaternion(x: " << quat.x << ", y: " << quat.y << ", z: " << quat.z << ", w: " << quat.w
+     << ")";
+  return os;
+}
+
+std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Pose & pose)
+{
+  os << "Pose(\n";
+  os << "  position: " << pose.position << "\n";
+  os << "  orientation: " << pose.orientation << "\n";
+  os << ")";
+  return os;
+}
+
+// traffic_simulator_msgs
+std::ostream & operator<<(
+  std::ostream & os, const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose)
+{
+  os << "LaneletPose(lanelet_id: " << lanelet_pose.lanelet_id << ", s: " << lanelet_pose.s
+     << ", offset: " << lanelet_pose.offset << ", rpy: " << lanelet_pose.rpy << ")";
+  return os;
+}
+
+std::ostream & operator<<(
+  std::ostream & os, const traffic_simulator_msgs::msg::EntitySubtype & subtype)
+{
+  using EntitySubtype = traffic_simulator_msgs::msg::EntitySubtype;
+  static const std::unordered_map<uint8_t, std::string> entity_names = {
+    {EntitySubtype::UNKNOWN, "UNKNOWN"}, {EntitySubtype::CAR, "CAR"},
+    {EntitySubtype::TRUCK, "TRUCK"},     {EntitySubtype::BUS, "BUS"},
+    {EntitySubtype::TRAILER, "TRAILER"}, {EntitySubtype::MOTORCYCLE, "MOTORCYCLE"},
+    {EntitySubtype::BICYCLE, "BICYCLE"}, {EntitySubtype::PEDESTRIAN, "PEDESTRIAN"}};
+
+  os << "EntitySubtype(";
+  if (const auto & it = entity_names.find(subtype.value); it != entity_names.end()) {
+    os << it->second;
+  } else {
+    os << "UNKNOWN(" << static_cast<int>(subtype.value) << ")";
+  }
+  os << ")";
+  return os;
+}
+
+std::ostream & operator<<(std::ostream & os, const traffic_simulator_msgs::msg::Axle & axle)
+{
+  os << "Axle(\n";
+  os << "  max_steering: " << axle.max_steering << "\n";
+  os << "  wheel_diameter: " << axle.wheel_diameter << "\n";
+  os << "  track_width: " << axle.track_width << "\n";
+  os << "  position_x: " << axle.position_x << "\n";
+  os << "  position_z: " << axle.position_z << "\n";
+  os << ")";
+  return os;
+}
+
+std::ostream & operator<<(std::ostream & os, const traffic_simulator_msgs::msg::Axles & axles)
+{
+  os << "Axles(\n";
+  os << "  front_axle: " << axles.front_axle << "\n";
+  os << "  rear_axle: " << axles.rear_axle << "\n";
+  os << ")";
+  return os;
+}
+
+std::ostream & operator<<(
+  std::ostream & os, const traffic_simulator_msgs::msg::BoundingBox & bounding_box)
+{
+  os << "BoundingBox(\n";
+  os << "  center: " << bounding_box.center << "\n";
+  os << "  dimensions: " << bounding_box.dimensions << "\n";
+  os << ")";
+  return os;
+}
+
+std::ostream & operator<<(
+  std::ostream & os, const traffic_simulator_msgs::msg::Performance & performance)
+{
+  os << "Performance(\n";
+  os << "  max_acceleration: " << performance.max_acceleration << "\n";
+  os << "  max_acceleration_rate: " << performance.max_acceleration_rate << "\n";
+  os << "  max_deceleration: " << performance.max_deceleration << "\n";
+  os << "  max_deceleration_rate: " << performance.max_deceleration_rate << "\n";
+  os << "  max_speed: " << performance.max_speed << "\n";
+  os << ")";
+  return os;
+}
+
+std::ostream & operator<<(
+  std::ostream & os, const traffic_simulator_msgs::msg::VehicleParameters & params)
+{
+  os << "VehicleParameters(\n";
+  os << "  name: " << params.name << "\n";
+  os << "  subtype: " << params.subtype << "\n";
+  os << "  bounding_box: " << params.bounding_box << "\n";
+  os << "  performance: " << params.performance << "\n";
+  os << "  axles: " << params.axles << "\n";
+  os << ")";
+  return os;
+}
+}  // namespace traffic_simulator

simulation/traffic_simulator/test/src/helper/test_helper.cpp

@@ -18,6 +18,7 @@
 #include <regex>
 #include <scenario_simulator_exception/exception.hpp>
 #include <traffic_simulator/helper/helper.hpp>
+#include <traffic_simulator/helper/ostream_helpers.hpp>
 
 #include "../expect_eq_macros.hpp"
 
@@ -43,6 +44,8 @@ TEST(helper, constructActionStatus)
  */
 TEST(helper, constructLaneletPose)
 {
+  using traffic_simulator::operator<<;
+
   const auto actual_lanelet_pose =
     traffic_simulator_msgs::build<traffic_simulator::LaneletPose>()
       .lanelet_id(11LL)
@@ -56,7 +59,9 @@ TEST(helper, constructLaneletPose)
   std::stringstream ss;
   ss << result_lanelet_pose;
   EXPECT_LANELET_POSE_EQ(result_lanelet_pose, actual_lanelet_pose);
-  EXPECT_STREQ(ss.str().c_str(), "lanelet id : 11\ns : 13");
+  EXPECT_STREQ(
+    ss.str().c_str(),
+    "LaneletPose(lanelet_id: 11, s: 13, offset: 17, rpy: Vector3(x: 19, y: 23, z: 29))");
 }
 
 /**