[pull] main from autowarefoundation:main

.cspell-partial.json

@@ -1,5 +1,9 @@
 {
- "ignorePaths": ["**/perception/**", "**/sensing/**", "perception/bytetrack/lib/**"],
+ "ignorePaths": [
+ "**/perception/**",
+ "sensing/tier4_pcl_extensions/include/**",
+ "perception/bytetrack/lib/**"
+ ],
  "ignoreRegExpList": [],
  "words": []
 }

.github/sync-files.yaml

@@ -42,20 +42,6 @@
  - \"\"
  - -cuda
  include:" {source}
- - source: .github/workflows/build-and-test-differential.yaml
- pre-commands: |
- sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source}
-
- sd -s 'container: ${{ matrix.container }}' 'container: ${{ matrix.container }}${{ matrix.container-suffix }}' {source}
- sd -- \
- " include:" \
- " container-suffix:
- - \"\"
- - -cuda
- include:" {source}
-
- sd "^ container: ghcr.io/autowarefoundation/autoware-universe:(\w+)-latest\$" \
- " container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {source}
  - source: .github/workflows/build-and-test-differential-self-hosted.yaml
  pre-commands: |
  sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source}

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp

@@ -146,6 +146,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
  m_marker_common.addMessage(markers_ptr);
  }
 
+ void deleteMarker(rviz_default_plugins::displays::MarkerID marker_id)
+ {
+ m_marker_common.deleteMarker(marker_id);
+ }
+
 protected:
  /// \brief Convert given shape msg into a Marker
  /// \tparam ClassificationContainerT List type with ObjectClassificationMsg

common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp

@@ -24,6 +24,7 @@
 
 #include <condition_variable>
 #include <list>
+#include <set>
 #include <string>
 #include <unordered_map>
 #include <vector>
@@ -112,6 +113,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
  std::mutex mutex;
  std::condition_variable condition;
  std::vector<visualization_msgs::msg::Marker::SharedPtr> markers;
+ std::set<rviz_default_plugins::displays::MarkerID> existing_marker_ids;
 };
 
 } // namespace object_detection

common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp

@@ -15,6 +15,7 @@
 #include <object_detection/predicted_objects_display.hpp>
 
 #include <memory>
+#include <set>
 
 namespace autoware
 {
@@ -130,7 +131,7 @@ std::vector<visualization_msgs::msg::Marker::SharedPtr> PredictedObjectsDisplay:
  auto acceleration_text_marker_ptr = acceleration_text_marker.value();
  acceleration_text_marker_ptr->header = msg->header;
  acceleration_text_marker_ptr->id = uuid_to_marker_id(object.object_id);
- add_marker(acceleration_text_marker_ptr);
+ markers.push_back(acceleration_text_marker_ptr);
  }
 
  // Get marker for twist
@@ -195,11 +196,19 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt)
  std::unique_lock<std::mutex> lock(mutex);
 
  if (!markers.empty()) {
- clear_markers();
-
+ std::set new_marker_ids = std::set<rviz_default_plugins::displays::MarkerID>();
  for (const auto & marker : markers) {
+ rviz_default_plugins::displays::MarkerID marker_id =
+ rviz_default_plugins::displays::MarkerID(marker->ns, marker->id);
  add_marker(marker);
+ new_marker_ids.insert(marker_id);
+ }
+ for (auto itr = existing_marker_ids.begin(); itr != existing_marker_ids.end(); itr++) {
+ if (new_marker_ids.find(*itr) == new_marker_ids.end()) {
+ deleteMarker(*itr);
+ }
  }
+ existing_marker_ids = new_marker_ids;
 
  markers.clear();
  }

common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp

@@ -45,56 +45,6 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox;
 
 namespace tf2
 {
-
-/*************/
-/** Point32 **/
-/*************/
-
-/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Point32 type.
- * This function is a specialization of the doTransform template defined in tf2/convert.h.
- * \param t_in The point to transform, as a Point32 message.
- * \param t_out The transformed point, as a Point32 message.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
-template <>
-inline void doTransform(
- const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out,
- const geometry_msgs::msg::TransformStamped & transform)
-{
- const KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z);
- t_out.x = static_cast<float>(v_out[0]);
- t_out.y = static_cast<float>(v_out[1]);
- t_out.z = static_cast<float>(v_out[2]);
-}
-
-/*************/
-/** Polygon **/
-/*************/
-
-/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Polygon type.
- * This function is a specialization of the doTransform template defined in tf2/convert.h.
- * \param t_in The polygon to transform.
- * \param t_out The transformed polygon.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
-template <>
-inline void doTransform(
- const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out,
- const geometry_msgs::msg::TransformStamped & transform)
-{
- // Don't call the Point32 doTransform to avoid doing this conversion every time
- const auto kdl_frame = gmTransformToKDL(transform);
- // We don't use std::back_inserter to allow aliasing between t_in and t_out
- t_out.points.resize(t_in.points.size());
- for (size_t i = 0; i < t_in.points.size(); ++i) {
- const KDL::Vector v_out =
- kdl_frame * KDL::Vector(t_in.points[i].x, t_in.points[i].y, t_in.points[i].z);
- t_out.points[i].x = static_cast<float>(v_out[0]);
- t_out.points[i].y = static_cast<float>(v_out[1]);
- t_out.points[i].z = static_cast<float>(v_out[2]);
- }
-}
-
 /******************/
 /** Quaternion32 **/
 /******************/

common/component_interface_specs/CMakeLists.txt

@@ -4,4 +4,38 @@ project(component_interface_specs)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+include_directories(
+ include
+ SYSTEM
+ ${rclcpp_INCLUDE_DIRS}
+ ${rosidl_runtime_cpp_INCLUDE_DIRS}
+ ${rcl_INCLUDE_DIRS}
+ ${autoware_adapi_v1_msgs_INCLUDE_DIRS}
+ ${autoware_auto_planning_msgs_INCLUDE_DIRS}
+ ${autoware_planning_msgs_INCLUDE_DIRS}
+ ${autoware_auto_vehicle_msgs_INCLUDE_DIRS}
+ ${tier4_control_msgs_INCLUDE_DIRS}
+ ${nav_msgs_INCLUDE_DIRS}
+ ${tier4_system_msgs_INCLUDE_DIRS}
+ ${tier4_vehicle_msgs_INCLUDE_DIRS}
+ ${autoware_auto_perception_msgs_INCLUDE_DIRS}
+ ${tier4_map_msgs_INCLUDE_DIRS}
+)
+
+if(BUILD_TESTING)
+ ament_add_ros_isolated_gtest(test_component_interface_specs
+ test/gtest_main.cpp
+ test/test_planning.cpp
+ test/test_control.cpp
+ test/test_localization.cpp
+ test/test_system.cpp
+ test/test_map.cpp
+ test/test_perception.cpp
+ test/test_vehicle.cpp
+ )
+ target_include_directories(test_component_interface_specs
+ PRIVATE include
+ )
+endif()
+
 ament_auto_package()

common/component_interface_specs/package.xml

@@ -11,13 +11,28 @@
  <license>Apache License 2.0</license>
 
  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+ <buildtool_depend>ament_cmake_core</buildtool_depend>
+ <buildtool_depend>ament_cmake_export_dependencies</buildtool_depend>
+ <buildtool_depend>ament_cmake_test</buildtool_depend>
  <buildtool_depend>autoware_cmake</buildtool_depend>
 
+ <buildtool_export_depend>ament_cmake_core</buildtool_export_depend>
+ <buildtool_export_depend>ament_cmake_test</buildtool_export_depend>
+
  <depend>autoware_adapi_v1_msgs</depend>
+ <depend>autoware_auto_perception_msgs</depend>
+ <depend>autoware_auto_planning_msgs</depend>
  <depend>autoware_auto_vehicle_msgs</depend>
+ <depend>autoware_planning_msgs</depend>
+ <depend>nav_msgs</depend>
+ <depend>rcl</depend>
+ <depend>rclcpp</depend>
+ <depend>rosidl_runtime_cpp</depend>
+ <depend>tier4_control_msgs</depend>
  <depend>tier4_map_msgs</depend>
+ <depend>tier4_system_msgs</depend>
  <depend>tier4_vehicle_msgs</depend>
-
+ <test_depend>ament_cmake_ros</test_depend>
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>autoware_lint_common</test_depend>
 

common/motion_utils/src/motion_utils.cpp → common/component_interface_specs/test/gtest_main.cpp

@@ -1,4 +1,4 @@
-// Copyright 2022 Tier IV, Inc.
+// Copyright 2023 The Autoware Contributors
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -12,4 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "motion_utils/motion_utils.hpp"
+#include "gtest/gtest.h"
+
+int main(int argc, char * argv[])
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}

common/component_interface_specs/test/test_control.cpp

@@ -0,0 +1,46 @@
+// Copyright 2023 The Autoware Contributors
+//
+// 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 "component_interface_specs/control.hpp"
+#include "gtest/gtest.h"
+
+TEST(control, interface)
+{
+ {
+ using control_interface::IsPaused;
+ IsPaused is_paused;
+ size_t depth = 1;
+ EXPECT_EQ(is_paused.depth, depth);
+ EXPECT_EQ(is_paused.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+ EXPECT_EQ(is_paused.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+ }
+
+ {
+ using control_interface::IsStartRequested;
+ IsStartRequested is_start_requested;
+ size_t depth = 1;
+ EXPECT_EQ(is_start_requested.depth, depth);
+ EXPECT_EQ(is_start_requested.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+ EXPECT_EQ(is_start_requested.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+ }
+
+ {
+ using control_interface::IsStopped;
+ IsStopped is_stopped;
+ size_t depth = 1;
+ EXPECT_EQ(is_stopped.depth, depth);
+ EXPECT_EQ(is_stopped.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+ EXPECT_EQ(is_stopped.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+ }
+}

common/component_interface_specs/test/test_localization.cpp

@@ -0,0 +1,46 @@
+// Copyright 2023 The Autoware Contributors
+//
+// 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 "component_interface_specs/localization.hpp"
+#include "gtest/gtest.h"
+
+TEST(localization, interface)
+{
+ {
+ using localization_interface::InitializationState;
+ InitializationState initialization_state;
+ size_t depth = 1;
+ EXPECT_EQ(initialization_state.depth, depth);
+ EXPECT_EQ(initialization_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+ EXPECT_EQ(initialization_state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+ }
+
+ {
+ using localization_interface::KinematicState;
+ KinematicState kinematic_state;
+ size_t depth = 1;
+ EXPECT_EQ(kinematic_state.depth, depth);
+ EXPECT_EQ(kinematic_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+ EXPECT_EQ(kinematic_state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+ }
+
+ {
+ using localization_interface::Acceleration;
+ Acceleration acceleration;
+ size_t depth = 1;
+ EXPECT_EQ(acceleration.depth, depth);
+ EXPECT_EQ(acceleration.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+ EXPECT_EQ(acceleration.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+ }
+}

common/component_interface_specs/test/test_map.cpp

@@ -0,0 +1,28 @@
+// Copyright 2023 The Autoware Contributors
+//
+// 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 "component_interface_specs/map.hpp"
+#include "gtest/gtest.h"
+
+TEST(map, interface)
+{
+ {
+ using map_interface::MapProjectorInfo;
+ MapProjectorInfo map_projector;
+ size_t depth = 1;
+ EXPECT_EQ(map_projector.depth, depth);
+ EXPECT_EQ(map_projector.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+ EXPECT_EQ(map_projector.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+ }
+}

common/component_interface_specs/test/test_perception.cpp

@@ -0,0 +1,28 @@
+// Copyright 2023 The Autoware Contributors
+//
+// 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 "component_interface_specs/perception.hpp"
+#include "gtest/gtest.h"
+
+TEST(perception, interface)
+{
+ {
+ using perception_interface::ObjectRecognition;
+ ObjectRecognition object_recognition;
+ size_t depth = 1;
+ EXPECT_EQ(object_recognition.depth, depth);
+ EXPECT_EQ(object_recognition.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+ EXPECT_EQ(object_recognition.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE);
+ }
+}