Merge branch 'main' into refactor/start_planner_rss

Signed-off-by: kyoichi-sugahara <[email protected]>
autowarefoundation · Nov 30, 2023 · 5ba2173 · 5ba2173
2 parents a2ba80e + 4887bdc
commit 5ba2173
Show file tree

Hide file tree

Showing 238 changed files with 13,273 additions and 5,921 deletions.
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
diff --git a/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml b/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml
@@ -1,3 +1,4 @@
 /**:
   ros__parameters:
     update_rate: 10.0
+    oneshot: false
diff --git a/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml b/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml
@@ -1,8 +1,6 @@
 <launch>
-  <arg name="oneshot" default="false"/>
   <arg name="config_file" default="$(find-pkg-share goal_distance_calculator)/config/goal_distance_calculator.param.yaml"/>
   <node pkg="goal_distance_calculator" exec="goal_distance_calculator_node" name="goal_distance_calculator" output="screen">
     <param from="$(var config_file)"/>
-    <param name="oneshot" value="$(var oneshot)"/>
   </node>
 </launch>
diff --git a/common/goal_distance_calculator/schema/goal_distance_calculator.schema.json b/common/goal_distance_calculator/schema/goal_distance_calculator.schema.json
@@ -0,0 +1,36 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Goal Distance Calculator Node",
+  "type": "object",
+  "definitions": {
+    "goal_distance_calculator": {
+      "type": "object",
+      "properties": {
+        "update_rate": {
+          "type": "number",
+          "default": "10.0",
+          "exclusiveMinimum": 0,
+          "description": "Timer callback period. [Hz]"
+        },
+        "oneshot": {
+          "type": "boolean",
+          "default": "false",
+          "description": "Publish deviations just once or repeatedly."
+        }
+      },
+      "required": ["update_rate", "oneshot"]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/goal_distance_calculator"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}
diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp
@@ -40,8 +40,8 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions
   durable_qos.transient_local();
 
   // Node Parameter
-  node_param_.update_rate = declare_parameter("update_rate", 10.0);
-  node_param_.oneshot = declare_parameter("oneshot", true);
+  node_param_.update_rate = declare_parameter<double>("update_rate");
+  node_param_.oneshot = declare_parameter<bool>("oneshot");
 
   // Core
   goal_distance_calculator_ = std::make_unique<GoalDistanceCalculator>();

diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp
@@ -462,7 +462,8 @@ bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose)
  * pose.
  */
 geometry_msgs::msg::Pose calcOffsetPose(
-  const geometry_msgs::msg::Pose & p, const double x, const double y, const double z);
+  const geometry_msgs::msg::Pose & p, const double x, const double y, const double z,
+  const double yaw = 0.0);
 
 /**
  * @brief Calculate a point by linear interpolation.

diff --git a/common/tier4_autoware_utils/src/geometry/geometry.cpp b/common/tier4_autoware_utils/src/geometry/geometry.cpp
@@ -327,12 +327,13 @@ double calcCurvature(
  * pose.
  */
 geometry_msgs::msg::Pose calcOffsetPose(
-  const geometry_msgs::msg::Pose & p, const double x, const double y, const double z)
+  const geometry_msgs::msg::Pose & p, const double x, const double y, const double z,
+  const double yaw)
 {
   geometry_msgs::msg::Pose pose;
   geometry_msgs::msg::Transform transform;
   transform.translation = createTranslation(x, y, z);
-  transform.rotation = createQuaternion(0.0, 0.0, 0.0, 1.0);
+  transform.rotation = createQuaternionFromYaw(yaw);
   tf2::Transform tf_pose;
   tf2::Transform tf_offset;
   tf2::fromMsg(transform, tf_offset);
@@ -378,6 +379,7 @@ std::optional<geometry_msgs::msg::Point> intersect(
   geometry_msgs::msg::Point intersect_point;
   intersect_point.x = t * p1.x + (1.0 - t) * p2.x;
   intersect_point.y = t * p1.y + (1.0 - t) * p2.y;
+  intersect_point.z = t * p1.z + (1.0 - t) * p2.z;
   return intersect_point;
 }
 

diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp
@@ -1138,6 +1138,7 @@ TEST(geometry, calcOffsetPose)
   using tier4_autoware_utils::createQuaternionFromRPY;
   using tier4_autoware_utils::deg2rad;
 
+  // Only translation
   {
     geometry_msgs::msg::Pose p_in;
     p_in.position = createPoint(1.0, 2.0, 3.0);
@@ -1185,6 +1186,40 @@ TEST(geometry, calcOffsetPose)
     EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.25881904510252068);
     EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.96592582628906831);
   }
+
+  // Only rotation
+  {
+    geometry_msgs::msg::Pose p_in;
+    p_in.position = createPoint(2.0, 1.0, 1.0);
+    p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30));
+
+    const auto p_out = calcOffsetPose(p_in, 0.0, 0.0, 0.0, deg2rad(20));
+
+    EXPECT_DOUBLE_EQ(p_out.position.x, 2.0);
+    EXPECT_DOUBLE_EQ(p_out.position.y, 1.0);
+    EXPECT_DOUBLE_EQ(p_out.position.z, 1.0);
+    EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+    EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+    EXPECT_NEAR(p_out.orientation.z, 0.42261826174069944, epsilon);
+    EXPECT_NEAR(p_out.orientation.w, 0.9063077870366499, epsilon);
+  }
+
+  // Both translation and rotation
+  {
+    geometry_msgs::msg::Pose p_in;
+    p_in.position = createPoint(2.0, 1.0, 1.0);
+    p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30));
+
+    const auto p_out = calcOffsetPose(p_in, 2.0, 0.0, -1.0, deg2rad(20));
+
+    EXPECT_DOUBLE_EQ(p_out.position.x, 3.73205080756887729);
+    EXPECT_DOUBLE_EQ(p_out.position.y, 2.0);
+    EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
+    EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
+    EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
+    EXPECT_NEAR(p_out.orientation.z, 0.42261826174069944, epsilon);
+    EXPECT_NEAR(p_out.orientation.w, 0.9063077870366499, epsilon);
+  }
 }
 
 TEST(geometry, isDrivingForward)

diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml
@@ -85,3 +85,24 @@ Control:
       logger_name: control.vehicle_cmd_gate
     - node_name: /control/vehicle_cmd_gate
       logger_name: tier4_autoware_utils
+
+# ============================================================
+# common
+# ============================================================
+
+Common:
+  tier4_autoware_utils:
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+      logger_name: tier4_autoware_utils
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner
+      logger_name: tier4_autoware_utils
+    - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner
+      logger_name: tier4_autoware_utils
+    - node_name: /planning/scenario_planning/lane_driving/motion_planning/path_smoother
+      logger_name: tier4_autoware_utils
+    - node_name: /planning/scenario_planning/motion_velocity_smoother
+      logger_name: tier4_autoware_utils
+    - node_name: /control/trajectory_follower/controller_node_exe
+      logger_name: tier4_autoware_utils
+    - node_name: /control/vehicle_cmd_gate
+      logger_name: tier4_autoware_utils
diff --git a/control/obstacle_collision_checker/schema/obstacle_collision_checker.json b/control/obstacle_collision_checker/schema/obstacle_collision_checker.json
@@ -0,0 +1,60 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for obstacle collision checker",
+  "type": "object",
+  "definitions": {
+    "vehicle_cmd_gate": {
+      "type": "object",
+      "properties": {
+        "delay_time": {
+          "type": "number",
+          "default": 0.3,
+          "description": "Delay time of vehicles."
+        },
+        "update_rate": {
+          "type": "number"
+        },
+        "footprint_margin": {
+          "type": "number",
+          "default": 0.0,
+          "description": "Foot print margin."
+        },
+        "max_deceleration": {
+          "type": "number",
+          "default": 2.0,
+          "description": "Max deceleration for ego vehicle to stop."
+        },
+        "resample_interval": {
+          "type": "number",
+          "default": 0.3,
+          "description": "Interval for resampling trajectory."
+        },
+        "search_radius": {
+          "type": "number",
+          "default": 5.0,
+          "description": "Search distance from trajectory to point cloud"
+        }
+      },
+      "required": [
+        "delay_time",
+        "footprint_margin",
+        "max_deceleration",
+        "resample_interval",
+        "search_radius",
+        "update_rate"
+      ]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/obstacle_collision_checker"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}
diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md
@@ -83,6 +83,8 @@ For the backward compatibility (to be removed):
 
 ## Parameters
 
+{{ json_to_markdown("control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json") }}
+
 | Name                               | Type     | Description                                                                                                                                                                                                                                                                                                                                                                                                                   | Default value |
 | :--------------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
 | `transition_timeout`               | `double` | If the state transition is not completed within this time, it is considered a transition failure.                                                                                                                                                                                                                                                                                                                             | 10.0          |