Merge branch 'main' into refactor/ekf_localizer_covariance

.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@v28
+        uses: tj-actions/changed-files@v31
         with:
           files: |
             **/*.cpp

common/autoware_ad_api_specs/include/autoware_ad_api_specs/motion.hpp

@@ -0,0 +1,43 @@
+// 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__MOTION_HPP_
+#define AUTOWARE_AD_API_SPECS__MOTION_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_ad_api_msgs/msg/motion_state.hpp>
+#include <autoware_ad_api_msgs/srv/accept_start.hpp>
+
+namespace autoware_ad_api::motion
+{
+
+struct AcceptStart
+{
+  using Service = autoware_ad_api_msgs::srv::AcceptStart;
+  static constexpr char name[] = "/api/motion/accept_start";
+};
+
+struct State
+{
+  using Message = autoware_ad_api_msgs::msg::MotionState;
+  static constexpr char name[] = "/api/motion/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::motion
+
+#endif  // AUTOWARE_AD_API_SPECS__MOTION_HPP_

common/component_interface_specs/include/component_interface_specs/control.hpp

@@ -0,0 +1,53 @@
+// 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__CONTROL_HPP_
+#define COMPONENT_INTERFACE_SPECS__CONTROL_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <tier4_control_msgs/msg/is_paused.hpp>
+#include <tier4_control_msgs/msg/is_start_requested.hpp>
+#include <tier4_control_msgs/srv/set_pause.hpp>
+
+namespace control_interface
+{
+
+struct SetPause
+{
+  using Service = tier4_control_msgs::srv::SetPause;
+  static constexpr char name[] = "/control/vehicle_cmd_gate/set_pause";
+};
+
+struct IsPaused
+{
+  using Message = tier4_control_msgs::msg::IsPaused;
+  static constexpr char name[] = "/control/vehicle_cmd_gate/is_paused";
+  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;
+};
+
+struct IsStartRequested
+{
+  using Message = tier4_control_msgs::msg::IsStartRequested;
+  static constexpr char name[] = "/control/vehicle_cmd_gate/is_start_requested";
+  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 control_interface
+
+#endif  // COMPONENT_INTERFACE_SPECS__CONTROL_HPP_

common/component_interface_utils/include/component_interface_utils/rclcpp.hpp

@@ -32,6 +32,10 @@ class NodeAdaptor
 private:
   using CallbackGroup = rclcpp::CallbackGroup::SharedPtr;
 
+  template <class SharedPtrT, class InstanceT>
+  using MessageCallback =
+    void (InstanceT::*)(const typename SharedPtrT::element_type::SpecType::Message::ConstSharedPtr);
+
   template <class SharedPtrT, class InstanceT>
   using ServiceCallback = void (InstanceT::*)(
     const typename SharedPtrT::element_type::SpecType::Service::Request::SharedPtr,
@@ -92,14 +96,25 @@ class NodeAdaptor
       srv, [cli, timeout](auto req, auto res) { *res = *cli->call(req, timeout); }, group);
   }
 
+  /// Create a subscription wrapper.
+  template <class SharedPtrT, class InstanceT>
+  void init_sub(
+    SharedPtrT & sub, InstanceT * instance,
+    MessageCallback<SharedPtrT, InstanceT> && callback) const
+  {
+    using std::placeholders::_1;
+    init_sub(sub, std::bind(callback, instance, _1));
+  }
+
   /// Create a service wrapper for logging.
   template <class SharedPtrT, class InstanceT>
   void init_srv(
-    SharedPtrT & srv, InstanceT * instance, ServiceCallback<SharedPtrT, InstanceT> callback,
+    SharedPtrT & srv, InstanceT * instance, ServiceCallback<SharedPtrT, InstanceT> && callback,
     CallbackGroup group = nullptr) const
   {
-    init_srv(
-      srv, [instance, callback](auto req, auto res) { (instance->*callback)(req, res); }, group);
+    using std::placeholders::_1;
+    using std::placeholders::_2;
+    init_srv(srv, std::bind(callback, instance, _1, _2), group);
   }
 
 private:

common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp

@@ -34,6 +34,15 @@ class ServiceException : public std::runtime_error
   {
     code_ = code;
   }
+
+  template <class T>
+  void set(T & status) const
+  {
+    status.success = false;
+    status.code = code_;
+    status.message = what();
+  }
+
   ResponseStatus status() const
   {
     ResponseStatus status;

common/component_interface_utils/include/component_interface_utils/rclcpp/service_server.hpp

@@ -70,7 +70,7 @@ class Service
         try {
           callback(request, response);
         } catch (const ServiceException & error) {
-          response->status = error.status();
+          error.set(response->status);
         }
       }
       RCLCPP_INFO_STREAM(logger, "service exit: " << SpecT::name << "\n" << to_yaml(*response));

common/component_interface_utils/include/component_interface_utils/status.hpp

@@ -0,0 +1,31 @@
+// 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_UTILS__STATUS_HPP_
+#define COMPONENT_INTERFACE_UTILS__STATUS_HPP_
+
+namespace component_interface_utils::status
+{
+
+template <class T1, class T2>
+void copy(const T1 & src, T2 & dst)  // NOLINT(build/include_what_you_use): cpplint false positive
+{
+  dst->status.success = src->status.success;
+  dst->status.code = src->status.code;
+  dst->status.message = src->status.message;
+}
+
+}  // namespace component_interface_utils::status
+
+#endif  // COMPONENT_INTERFACE_UTILS__STATUS_HPP_

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -347,7 +347,8 @@ boost::optional<size_t> findNearestSegmentIndex(
  */
 template <class T>
 double calcLateralOffset(
-  const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false)
+  const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx,
+  const bool throw_exception = false)
 {
   const auto overlap_removed_points = removeOverlapPoints(points, 0);
 
@@ -371,8 +372,6 @@ double calcLateralOffset(
     return std::nan("");
   }
 
-  const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target);
-
   const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx));
   const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1));
 
@@ -383,6 +382,36 @@ double calcLateralOffset(
   return cross_vec(2) / segment_vec.norm();
 }
 
+template <class T>
+double calcLateralOffset(
+  const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false)
+{
+  const auto overlap_removed_points = removeOverlapPoints(points, 0);
+
+  if (throw_exception) {
+    validateNonEmpty(overlap_removed_points);
+  } else {
+    try {
+      validateNonEmpty(overlap_removed_points);
+    } catch (const std::exception & e) {
+      std::cerr << e.what() << std::endl;
+      return std::nan("");
+    }
+  }
+
+  if (overlap_removed_points.size() == 1) {
+    const std::runtime_error e("Same points are given.");
+    if (throw_exception) {
+      throw e;
+    }
+    std::cerr << e.what() << std::endl;
+    return std::nan("");
+  }
+
+  const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target);
+  return calcLateralOffset(points, p_target, seg_idx, throw_exception);
+}
+
 /**
  * @brief calcSignedArcLength from index to index
  */

common/motion_utils/test/src/trajectory/test_trajectory.cpp

@@ -564,6 +564,67 @@ TEST(trajectory, calcLateralOffset)
   EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0)), -3.0, epsilon);
 }
 
+TEST(trajectory, calcLateralOffset_without_segment_idx)
+{
+  using motion_utils::calcLateralOffset;
+
+  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
+  const bool throw_exception = true;
+
+  // Empty
+  EXPECT_THROW(
+    calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}, throw_exception),
+    std::invalid_argument);
+
+  // Trajectory size is 1
+  {
+    const auto one_point_traj = generateTestTrajectory<Trajectory>(1, 1.0);
+    EXPECT_THROW(
+      calcLateralOffset(
+        one_point_traj.points, geometry_msgs::msg::Point{}, static_cast<size_t>(0),
+        throw_exception),
+      std::runtime_error);
+  }
+
+  // Same close points in trajectory
+  {
+    const auto invalid_traj = generateTestTrajectory<Trajectory>(10, 0.0);
+    const auto p = createPoint(3.0, 0.0, 0.0);
+    EXPECT_THROW(
+      calcLateralOffset(invalid_traj.points, p, static_cast<size_t>(2), throw_exception),
+      std::runtime_error);
+    EXPECT_THROW(
+      calcLateralOffset(invalid_traj.points, p, static_cast<size_t>(3), throw_exception),
+      std::runtime_error);
+  }
+
+  // Point on trajectory
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(3.1, 0.0, 0.0), static_cast<size_t>(3)), 0.0,
+    epsilon);
+
+  // Point before start point
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(-3.9, 3.0, 0.0), static_cast<size_t>(0)), 3.0,
+    epsilon);
+
+  // Point after start point
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(13.3, -10.0, 0.0), static_cast<size_t>(8)), -10.0,
+    epsilon);
+
+  // Random cases
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(4.3, 7.0, 0.0), static_cast<size_t>(4)), 7.0,
+    epsilon);
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0), static_cast<size_t>(0)), -3.0,
+    epsilon);
+  EXPECT_NEAR(
+    calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0), static_cast<size_t>(1)), -3.0,
+    epsilon);
+}
+
 TEST(trajectory, calcLateralOffset_CurveTrajectory)
 {
   using motion_utils::calcLateralOffset;

common/perception_utils/include/perception_utils/object_classification.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 PERCEPTION_UTILS__OBJECT_CLASSIFICATION_HPP_
+#define PERCEPTION_UTILS__OBJECT_CLASSIFICATION_HPP_
+
+#include "autoware_auto_perception_msgs/msg/object_classification.hpp"
+
+namespace perception_utils
+{
+using autoware_auto_perception_msgs::msg::ObjectClassification;
+
+bool isVehicle(const uint8_t object_classification)
+{
+  return object_classification == ObjectClassification::BICYCLE ||
+         object_classification == ObjectClassification::BUS ||
+         object_classification == ObjectClassification::CAR ||
+         object_classification == ObjectClassification::MOTORCYCLE ||
+         object_classification == ObjectClassification::TRAILER ||
+         object_classification == ObjectClassification::TRUCK;
+}
+
+bool isCarLikeVehicle(const uint8_t object_classification)
+{
+  return object_classification == ObjectClassification::BUS ||
+         object_classification == ObjectClassification::CAR ||
+         object_classification == ObjectClassification::TRAILER ||
+         object_classification == ObjectClassification::TRUCK;
+}
+
+bool isLargeVehicle(const uint8_t object_classification)
+{
+  return object_classification == ObjectClassification::BUS ||
+         object_classification == ObjectClassification::TRAILER ||
+         object_classification == ObjectClassification::TRUCK;
+}
+}  // namespace perception_utils
+
+#endif  // PERCEPTION_UTILS__OBJECT_CLASSIFICATION_HPP_

common/perception_utils/package.xml

@@ -5,6 +5,9 @@
   <version>0.1.0</version>
   <description>The perception_utils package</description>
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+  <maintainer email="satoshi.tanaka@tier4.jp">Satoshi Tanaka</maintainer>
+  <maintainer email="yusuke.muramatsu@tier4.jp">Yusuke Muramatsu</maintainer>
+  <maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>