Merge branch 'main' into modelzoo-artifacts

.github/workflows/build-and-test-differential.yaml

@@ -80,7 +80,7 @@ jobs:
 
       - name: Get modified files
         id: get-modified-files
-        uses: tj-actions/changed-files@v31
+        uses: tj-actions/changed-files@v32
         with:
           files: |
             **/*.cpp

common/autoware_ad_api_specs/include/autoware_ad_api_specs/operation_mode.hpp

@@ -0,0 +1,73 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// 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 AUTOWARE_AD_API_SPECS__OPERATION_MODE_HPP_
+#define AUTOWARE_AD_API_SPECS__OPERATION_MODE_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
+
+namespace autoware_ad_api::operation_mode
+{
+
+struct ChangeToStop
+{
+  using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/api/operation_mode/change_to_stop";
+};
+
+struct ChangeToAutonomous
+{
+  using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/api/operation_mode/change_to_autonomous";
+};
+
+struct ChangeToLocal
+{
+  using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/api/operation_mode/change_to_local";
+};
+
+struct ChangeToRemote
+{
+  using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/api/operation_mode/change_to_remote";
+};
+
+struct EnableAutowareControl
+{
+  using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/api/operation_mode/enable_autoware_control";
+};
+
+struct DisableAutowareControl
+{
+  using Service = autoware_adapi_v1_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/api/operation_mode/disable_autoware_control";
+};
+
+struct OperationModeState
+{
+  using Message = autoware_adapi_v1_msgs::msg::OperationModeState;
+  static constexpr char name[] = "/api/operation_mode/state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware_ad_api::operation_mode
+
+#endif  // AUTOWARE_AD_API_SPECS__OPERATION_MODE_HPP_

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp

@@ -127,6 +127,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     m_marker_common.addMessage(marker_ptr);
   }
 
+  void add_marker(visualization_msgs::msg::MarkerArray::ConstSharedPtr markers_ptr)
+  {
+    m_marker_common.addMessage(markers_ptr);
+  }
+
 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

@@ -18,12 +18,14 @@
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 
+#include <boost/functional/hash.hpp>
 #include <boost/uuid/uuid.hpp>
 #include <boost/uuid/uuid_generators.hpp>
 
+#include <condition_variable>
 #include <list>
-#include <map>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 namespace autoware
@@ -93,10 +95,23 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
     return id_map.at(uuid);
   }
 
-  std::map<boost::uuids::uuid, int32_t> id_map;
+  std::vector<visualization_msgs::msg::Marker::SharedPtr> createMarkers(
+    PredictedObjects::ConstSharedPtr msg);
+  void workerThread();
+
+  void update(float wall_dt, float ros_dt) override;
+
+  std::unordered_map<boost::uuids::uuid, int32_t, boost::hash<boost::uuids::uuid>> id_map;
+  // std::unordered_map<boost::uuids::uuid, int32_t> id_map;
   std::list<int32_t> unused_marker_ids;
   int32_t marker_id = 0;
   const int32_t PATH_ID_CONSTANT = 1e3;
+
+  PredictedObjects::ConstSharedPtr msg;
+  bool consumed{false};
+  std::mutex mutex;
+  std::condition_variable condition;
+  std::vector<visualization_msgs::msg::Marker::SharedPtr> markers;
 };
 
 }  // namespace object_detection

common/autoware_auto_perception_rviz_plugin/package.xml

@@ -5,7 +5,8 @@
   <version>1.0.0</version>
   <description>Contains plugins to visualize object detection outputs</description>
   <maintainer email="[email protected]">Apex.AI, Inc.</maintainer>
-  <maintainer email="[email protected]">Taichi Higashide</maintainer>
+  <maintainer email="[email protected]">Satoshi Tanaka</maintainer>
+  <maintainer email="[email protected]">Taiki Tanaka</maintainer>
   <license>Apache 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp

@@ -22,13 +22,38 @@ namespace rviz_plugins
 {
 namespace object_detection
 {
-PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") {}
+PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("tracks")
+{
+  std::thread worker(&PredictedObjectsDisplay::workerThread, this);
+  worker.detach();
+}
 
-void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr msg)
+void PredictedObjectsDisplay::workerThread()
+{
+  while (true) {
+    std::unique_lock<std::mutex> lock(mutex);
+    condition.wait(lock, [this] { return this->msg; });
+
+    auto tmp_msg = this->msg;
+    this->msg.reset();
+
+    lock.unlock();
+
+    auto tmp_markers = createMarkers(tmp_msg);
+    lock.lock();
+    markers = tmp_markers;
+
+    consumed = true;
+  }
+}
+
+std::vector<visualization_msgs::msg::Marker::SharedPtr> PredictedObjectsDisplay::createMarkers(
+  PredictedObjects::ConstSharedPtr msg)
 {
-  clear_markers();
   update_id_map(msg);
 
+  std::vector<visualization_msgs::msg::Marker::SharedPtr> markers;
+
   for (const auto & object : msg->objects) {
     // Get marker for shape
     auto shape_marker = get_shape_marker_ptr(
@@ -39,7 +64,7 @@ void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr ms
       auto shape_marker_ptr = shape_marker.value();
       shape_marker_ptr->header = msg->header;
       shape_marker_ptr->id = uuid_to_marker_id(object.object_id);
-      add_marker(shape_marker_ptr);
+      markers.push_back(shape_marker_ptr);
     }
 
     // Get marker for label
@@ -50,7 +75,7 @@ void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr ms
       auto label_marker_ptr = label_marker.value();
       label_marker_ptr->header = msg->header;
       label_marker_ptr->id = uuid_to_marker_id(object.object_id);
-      add_marker(label_marker_ptr);
+      markers.push_back(label_marker_ptr);
     }
 
     // Get marker for id
@@ -65,7 +90,7 @@ void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr ms
       auto id_marker_ptr = id_marker.value();
       id_marker_ptr->header = msg->header;
       id_marker_ptr->id = uuid_to_marker_id(object.object_id);
-      add_marker(id_marker_ptr);
+      markers.push_back(id_marker_ptr);
     }
 
     // Get marker for pose with covariance
@@ -75,7 +100,7 @@ void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr ms
       auto pose_with_covariance_marker_ptr = pose_with_covariance_marker.value();
       pose_with_covariance_marker_ptr->header = msg->header;
       pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id);
-      add_marker(pose_with_covariance_marker_ptr);
+      markers.push_back(pose_with_covariance_marker_ptr);
     }
 
     // Get marker for velocity text
@@ -90,7 +115,7 @@ void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr ms
       auto velocity_text_marker_ptr = velocity_text_marker.value();
       velocity_text_marker_ptr->header = msg->header;
       velocity_text_marker_ptr->id = uuid_to_marker_id(object.object_id);
-      add_marker(velocity_text_marker_ptr);
+      markers.push_back(velocity_text_marker_ptr);
     }
 
     // Get marker for twist
@@ -101,7 +126,7 @@ void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr ms
       auto twist_marker_ptr = twist_marker.value();
       twist_marker_ptr->header = msg->header;
       twist_marker_ptr->id = uuid_to_marker_id(object.object_id);
-      add_marker(twist_marker_ptr);
+      markers.push_back(twist_marker_ptr);
     }
 
     // Add marker for each candidate path
@@ -115,8 +140,8 @@ void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr ms
         predicted_path_marker_ptr->header = msg->header;
         predicted_path_marker_ptr->id =
           uuid_to_marker_id(object.object_id) + path_count * PATH_ID_CONSTANT;
-        add_marker(predicted_path_marker_ptr);
         path_count++;
+        markers.push_back(predicted_path_marker_ptr);
       }
     }
 
@@ -133,11 +158,41 @@ void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr ms
         path_confidence_marker_ptr->header = msg->header;
         path_confidence_marker_ptr->id =
           uuid_to_marker_id(object.object_id) + path_count * PATH_ID_CONSTANT;
-        add_marker(path_confidence_marker_ptr);
         path_count++;
+        markers.push_back(path_confidence_marker_ptr);
       }
     }
   }
+
+  return markers;
+}
+
+void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr msg)
+{
+  std::unique_lock<std::mutex> lock(mutex);
+
+  this->msg = msg;
+  condition.notify_one();
+}
+
+void PredictedObjectsDisplay::update(float wall_dt, float ros_dt)
+{
+  std::unique_lock<std::mutex> lock(mutex);
+
+  if (!markers.empty()) {
+    clear_markers();
+
+    for (const auto & marker : markers) {
+      add_marker(marker);
+    }
+
+    markers.clear();
+  }
+
+  lock.unlock();
+
+  ObjectPolygonDisplayBase<autoware_auto_perception_msgs::msg::PredictedObjects>::update(
+    wall_dt, ros_dt);
 }
 
 }  // namespace object_detection

common/component_interface_specs/include/component_interface_specs/system.hpp

@@ -0,0 +1,50 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// 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 COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_
+#define COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <tier4_system_msgs/srv/change_autoware_control.hpp>
+#include <tier4_system_msgs/srv/change_operation_mode.hpp>
+
+namespace system_interface
+{
+
+struct ChangeAutowareControl
+{
+  using Service = tier4_system_msgs::srv::ChangeAutowareControl;
+  static constexpr char name[] = "/system/operation_mode/change_autoware_control";
+};
+
+struct ChangeOperationMode
+{
+  using Service = tier4_system_msgs::srv::ChangeOperationMode;
+  static constexpr char name[] = "/system/operation_mode/change_operation_mode";
+};
+
+struct OperationModeState
+{
+  using Message = autoware_adapi_v1_msgs::msg::OperationModeState;
+  static constexpr char name[] = "/system/operation_mode/state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace system_interface
+
+#endif  // COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_

common/component_interface_specs/package.xml

@@ -11,6 +11,8 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

common/component_interface_utils/CMakeLists.txt

@@ -3,5 +3,5 @@ project(component_interface_utils)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
-
+ament_export_dependencies(tier4_system_msgs)
 ament_auto_package()