fix(perception): add autoware_planning_msgs dependencies to traffic l…

…ight nodes

Signed-off-by: David Wong <david.wong@tier4.jp>

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

@@ -600,43 +600,6 @@ double calcSignedArcLength(
   return signed_length_on_traj - signed_length_src_offset;
 }
 
-/**
- * @brief calculate length of 2D distance between two points, specified by start pose and end point
- * index of points container.
- * @param points points of trajectory, path, ...
- * @param src_pose start pose
- * @param dst_idx index of end point
- * @param max_dist max distance, used to search for nearest segment index to start pose
- * @param max_yaw max yaw, used to search for nearest segment index to start pose
- * @return length of distance between two points.
- * Length is positive if destination point associated to dst_idx is greater that point associated to
- * src_pose (i.e. after it in trajectory, path, ...) and negative otherwise.
- */
-template <class T>
-boost::optional<double> calcSignedArcLength(
-  const T & points, const geometry_msgs::msg::Pose & src_pose, const size_t dst_idx,
-  const double max_dist = std::numeric_limits<double>::max(),
-  const double max_yaw = std::numeric_limits<double>::max())
-{
-  try {
-    validateNonEmpty(points);
-  } catch (const std::exception & e) {
-    std::cerr << e.what() << std::endl;
-    return {};
-  }
-
-  const auto src_seg_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw);
-  if (!src_seg_idx) {
-    return boost::none;
-  }
-
-  const double signed_length_on_traj = calcSignedArcLength(points, *src_seg_idx, dst_idx);
-  const double signed_length_src_offset =
-    calcLongitudinalOffsetToSegment(points, *src_seg_idx, src_pose.position);
-
-  return signed_length_on_traj - signed_length_src_offset;
-}
-
 /**
  * @brief calculate length of 2D distance between two points, specified by start index of points
  * container and end point.
@@ -696,47 +659,6 @@ double calcSignedArcLength(
   return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset;
 }
 
-/**
- * @brief calculate length of 2D distance between two points, specified by start pose and end point.
- * @param points points of trajectory, path, ...
- * @param src_pose start pose
- * @param dst_point end point
- * @param max_dist max distance, used to search for nearest segment index to start pose
- * @param max_yaw max yaw, used to search for nearest segment index to start pose
- * @return length of distance between two points.
- * Length is positive if destination point is greater that source point associated to src_pose (i.e.
- * after it in trajectory, path, ...) and negative otherwise.
- */
-template <class T>
-boost::optional<double> calcSignedArcLength(
-  const T & points, const geometry_msgs::msg::Pose & src_pose,
-  const geometry_msgs::msg::Point & dst_point,
-  const double max_dist = std::numeric_limits<double>::max(),
-  const double max_yaw = std::numeric_limits<double>::max())
-{
-  try {
-    validateNonEmpty(points);
-  } catch (const std::exception & e) {
-    std::cerr << e.what() << std::endl;
-    return {};
-  }
-
-  const auto src_seg_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw);
-  if (!src_seg_idx) {
-    return boost::none;
-  }
-
-  const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point);
-
-  const double signed_length_on_traj = calcSignedArcLength(points, *src_seg_idx, dst_seg_idx);
-  const double signed_length_src_offset =
-    calcLongitudinalOffsetToSegment(points, *src_seg_idx, src_pose.position);
-  const double signed_length_dst_offset =
-    calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point);
-
-  return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset;
-}
-
 /**
  * @brief calculate length of 2D distance for whole points container, from its start to its end.
  * @param points points of trajectory, path, ...
@@ -838,55 +760,6 @@ boost::optional<double> calcDistanceToForwardStopPoint(
   return std::max(0.0, calcSignedArcLength(points_with_twist, src_idx, *closest_stop_idx));
 }
 
-/**
- * @brief calculate length of 2D distance between given pose and first point in container with zero
- * longitudinal velocity
- * @param points_with_twist points of trajectory, path, ... (with velocity)
- * @param pose given pose to start the distance calculation from
- * @param max_dist max distance, used to search for nearest segment index in points container to the
- * given pose
- * @param max_yaw max yaw, used to search for nearest segment index in points container to the given
- * pose
- * @return Length of 2D distance between given pose and first point in container with zero
- * longitudinal velocity
- */
-template <class T>
-boost::optional<double> calcDistanceToForwardStopPoint(
-  const T & points_with_twist, const geometry_msgs::msg::Pose & pose,
-  const double max_dist = std::numeric_limits<double>::max(),
-  const double max_yaw = std::numeric_limits<double>::max())
-{
-  try {
-    validateNonEmpty(points_with_twist);
-  } catch (const std::exception & e) {
-    std::cerr << e.what() << std::endl;
-    return {};
-  }
-
-  const auto nearest_segment_idx =
-    motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw);
-
-  if (!nearest_segment_idx) {
-    return boost::none;
-  }
-
-  const auto stop_idx = motion_utils::searchZeroVelocityIndex(
-    points_with_twist, *nearest_segment_idx + 1, points_with_twist.size());
-
-  if (!stop_idx) {
-    return boost::none;
-  }
-
-  const auto closest_stop_dist =
-    motion_utils::calcSignedArcLength(points_with_twist, pose, *stop_idx, max_dist, max_yaw);
-
-  if (!closest_stop_dist) {
-    return boost::none;
-  }
-
-  return std::max(0.0, *closest_stop_dist);
-}
-
 /**
  * @brief calculate the point offset from source point index along the trajectory (or path) (points
  * container)
@@ -1617,6 +1490,52 @@ size_t findFirstNearestSegmentIndexWithSoftConstraints(
 
   return nearest_idx;
 }
+
+/**
+ * @brief calculate the point offset from source point along the trajectory (or path)
+ * @brief calculate length of 2D distance between given pose and first point in container with zero
+ * longitudinal velocity
+ * @param points_with_twist points of trajectory, path, ... (with velocity)
+ * @param pose given pose to start the distance calculation from
+ * @param max_dist max distance, used to search for nearest segment index in points container to the
+ * given pose
+ * @param max_yaw max yaw, used to search for nearest segment index in points container to the given
+ * pose
+ * @return Length of 2D distance between given pose and first point in container with zero
+ * longitudinal velocity
+ */
+template <class T>
+boost::optional<double> calcDistanceToForwardStopPoint(
+  const T & points_with_twist, const geometry_msgs::msg::Pose & pose,
+  const double max_dist = std::numeric_limits<double>::max(),
+  const double max_yaw = std::numeric_limits<double>::max())
+{
+  try {
+    validateNonEmpty(points_with_twist);
+  } catch (const std::exception & e) {
+    std::cerr << e.what() << std::endl;
+    return {};
+  }
+
+  const auto nearest_segment_idx =
+    motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw);
+
+  if (!nearest_segment_idx) {
+    return boost::none;
+  }
+
+  const auto stop_idx = motion_utils::searchZeroVelocityIndex(
+    points_with_twist, *nearest_segment_idx + 1, points_with_twist.size());
+
+  if (!stop_idx) {
+    return boost::none;
+  }
+
+  const auto closest_stop_dist =
+    calcSignedArcLength(points_with_twist, pose.position, *nearest_segment_idx, *stop_idx);
+
+  return std::max(0.0, closest_stop_dist);
+}
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_

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

@@ -688,48 +688,6 @@ TEST(trajectory, calcSignedArcLengthFromPointToIndex)
   EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(4.3, 7.0, 0.0), 2), -2.3, epsilon);
 }
 
-TEST(trajectory, calcSignedArcLengthFromPoseToIndex_DistThreshold)
-{
-  using motion_utils::calcSignedArcLength;
-
-  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
-
-  // Out of threshold
-  EXPECT_FALSE(calcSignedArcLength(traj.points, createPose(0.0, 0.6, 0.0, 0.0, 0.0, 0.0), 3, 0.5));
-
-  // On threshold
-  EXPECT_NEAR(
-    *calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 0.0), 3, 0.5), 3.0,
-    epsilon);
-
-  // Within threshold
-  EXPECT_NEAR(
-    *calcSignedArcLength(traj.points, createPose(0.0, 0.4, 0.0, 0.0, 0.0, 0.0), 3, 0.5), 3.0,
-    epsilon);
-}
-
-TEST(trajectory, calcSignedArcLengthFromPoseToIndex_YawThreshold)
-{
-  using motion_utils::calcSignedArcLength;
-
-  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0);
-  const auto max_d = std::numeric_limits<double>::max();
-
-  // Out of threshold
-  EXPECT_FALSE(
-    calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 1.1), 3, max_d, 1.0));
-
-  // On threshold
-  EXPECT_NEAR(
-    *calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 1.0), 3, max_d, 1.0), 3.0,
-    epsilon);
-
-  // Within threshold
-  EXPECT_NEAR(
-    *calcSignedArcLength(traj.points, createPose(0.0, 0.5, 0.0, 0.0, 0.0, 0.9), 3, max_d, 1.0), 3.0,
-    epsilon);
-}
-
 TEST(trajectory, calcSignedArcLengthFromIndexToPoint)
 {
   using motion_utils::calcSignedArcLength;

common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_automatic_goal_rviz_plugin)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Qt5 REQUIRED Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+add_definitions(-DQT_NO_KEYWORDS)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/automatic_goal_sender.cpp
+  src/automatic_goal_append_tool.cpp
+  src/automatic_goal_panel.cpp
+)
+
+ament_auto_add_executable(automatic_goal_sender
+  src/automatic_goal_sender.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${QT_LIBRARIES}
+)
+
+# Export the plugin to be imported by rviz2
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  icons
+  plugins
+)

common/tier4_automatic_goal_rviz_plugin/README.md

@@ -0,0 +1,80 @@
+# tier4_automatic_goal_rviz_plugin
+
+## Purpose
+
+1. Defining a `GoalsList` by adding goals using `RvizTool` (Pose on the map).
+
+2. Automatic execution of the created `GoalsList` from the selected goal - it can be stopped and restarted.
+
+3. Looping the current `GoalsList`.
+
+4. Saving achieved goals to a file.
+
+5. Plan the route to one (single) selected goal and starting that route - it can be stopped and restarted.
+
+6. Remove any goal from the list or clear the current route.
+
+7. Save the current `GoalsList` to a file and load the list from the file.
+
+8. The application enables/disables access to options depending on the current state.
+
+9. The saved `GoalsList` can be executed without using a plugin - using a node `automatic_goal_sender`.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                         | Type                                              | Description                                      |
+| ---------------------------- | ------------------------------------------------- | ------------------------------------------------ |
+| `/api/operation_mode/state`  | `autoware_adapi_v1_msgs::msg::OperationModeState` | The topic represents the state of operation mode |
+| `/api/routing/state`         | `autoware_adapi_v1_msgs::msg::RouteState`         | The topic represents the state of route          |
+| `/rviz2/automatic_goal/goal` | `geometry_msgs::msgs::PoseStamped`                | The topic for adding goals to GoalsList          |
+
+### Output
+
+| Name                                       | Type                                               | Description                                        |
+| ------------------------------------------ | -------------------------------------------------- | -------------------------------------------------- |
+| `/api/operation_mode/change_to_autonomous` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to autonomous |
+| `/api/operation_mode/change_to_stop`       | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to stop       |
+| `/api/routing/set_route_points`            | `autoware_adapi_v1_msgs::srv::SetRoutePoints`      | The service to set route                           |
+| `/api/routing/clear_route`                 | `autoware_adapi_v1_msgs::srv::ClearRoute`          | The service to clear route state                   |
+| `/rviz2/automatic_goal/markers`            | `visualization_msgs::msg::MarkerArray`             | The topic to visualize goals as rviz markers       |
+
+## HowToUse
+
+1. Start rviz and select panels/Add new panel.
+
+   ![select_panel](./images/select_panels.png)
+
+2. Select `tier4_automatic_goal_rviz_plugin/AutowareAutomaticGoalPanel` and press OK.
+
+3. Select Add a new tool.
+
+   ![select_tool](./images/select_tool.png)
+
+4. Select `tier4_automatic_goal_rviz_plugin/AutowareAutomaticGoalTool` and press OK.
+
+5. Add goals visualization as markers to `Displays`.
+
+   ![markers](./images/markers.png)
+
+6. Append goals to the `GoalsList` to be achieved using `2D Append Goal` - in such a way that routes can be planned.
+
+7. Start sequential planning and goal achievement by clicking `Send goals automatically`
+
+   ![panel](./images/panel.png)
+
+8. You can save `GoalsList` by clicking `Save to file`.
+
+9. After saving, you can run the `GoalsList` without using a plugin also:
+   - example: `ros2 launch tier4_automatic_goal_rviz_plugin automatic_goal_sender.launch.xml goals_list_file_path:="/tmp/goals_list.yaml" goals_achieved_dir_path:="/tmp/"`
+     - `goals_list_file_path` - is the path to the saved `GoalsList` file to be loaded
+     - `goals_achieved_dir_path` - is the path to the directory where the file `goals_achieved.log` will be created and the achieved goals will be written to it
+
+### Hints
+
+If the application (Engagement) goes into `ERROR` mode (usually returns to `EDITING` later), it means that one of the services returned a calling error (`code!=0`).
+In this situation, check the terminal output for more information.
+
+- Often it is enough to try again.
+- Sometimes a clearing of the current route is required before retrying.

common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml

@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="goals_list_file_path" description="" default=""/>
+  <arg name="goals_achieved_dir_path" description="" default=""/>
+
+  <node pkg="tier4_automatic_goal_rviz_plugin" exec="automatic_goal_sender" name="automatic_goal_sender" output="screen">
+    <param name="goals_list_file_path" value="$(var goals_list_file_path)"/>
+    <param name="goals_achieved_dir_path" value="$(var goals_achieved_dir_path)"/>
+  </node>
+</launch>