chore: sync upstream

control/control_validator/src/utils.cpp

@@ -152,8 +152,8 @@ double calcMaxLateralDistance(
     // find nearest segment
     const size_t nearest_segment_idx =
       motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0);
-    double temp_dist =
-      motion_utils::calcLateralOffset(reference_trajectory.points, p0, nearest_segment_idx);
+    const double temp_dist = std::abs(
+      motion_utils::calcLateralOffset(reference_trajectory.points, p0, nearest_segment_idx));
     if (temp_dist > max_dist) {
       max_dist = temp_dist;
     }

control/trajectory_follower_node/param/lateral/mpc.param.yaml

@@ -65,6 +65,7 @@
     keep_steer_control_until_converged: true
     new_traj_duration_time: 1.0
     new_traj_end_dist: 0.3
+    mpc_converged_threshold_rps:  0.01 # threshold of mpc convergence check [rad/s]
 
     # steer offset
     steering_offset:

control/vehicle_cmd_gate/package.xml

@@ -34,6 +34,7 @@
   <depend>tier4_system_msgs</depend>
   <depend>tier4_vehicle_msgs</depend>
   <depend>vehicle_info_util</depend>
+  <depend>visualization_msgs</depend>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
 

control/vehicle_cmd_gate/src/marker_helper.hpp

@@ -0,0 +1,119 @@
+// Copyright 2020 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 MARKER_HELPER_HPP_
+#define MARKER_HELPER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <string>
+
+namespace vehicle_cmd_gate
+{
+inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z)
+{
+  geometry_msgs::msg::Point point;
+
+  point.x = x;
+  point.y = y;
+  point.z = z;
+
+  return point;
+}
+
+inline geometry_msgs::msg::Quaternion createMarkerOrientation(
+  double x, double y, double z, double w)
+{
+  geometry_msgs::msg::Quaternion quaternion;
+
+  quaternion.x = x;
+  quaternion.y = y;
+  quaternion.z = z;
+  quaternion.w = w;
+
+  return quaternion;
+}
+
+inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z)
+{
+  geometry_msgs::msg::Vector3 scale;
+
+  scale.x = x;
+  scale.y = y;
+  scale.z = z;
+
+  return scale;
+}
+
+inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a)
+{
+  std_msgs::msg::ColorRGBA color;
+
+  color.r = r;
+  color.g = g;
+  color.b = b;
+  color.a = a;
+
+  return color;
+}
+
+inline visualization_msgs::msg::Marker createMarker(
+  const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type,
+  geometry_msgs::msg::Point point, geometry_msgs::msg::Vector3 scale,
+  const std_msgs::msg::ColorRGBA & color)
+{
+  visualization_msgs::msg::Marker marker;
+
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = rclcpp::Time(0);
+  marker.ns = ns;
+  marker.id = id;
+  marker.type = type;
+  marker.action = visualization_msgs::msg::Marker::ADD;
+  marker.lifetime = rclcpp::Duration::from_seconds(0.2);
+  marker.pose.position = point;
+  marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0);
+  marker.scale = scale;
+  marker.color = color;
+  marker.frame_locked = false;
+
+  return marker;
+}
+
+inline visualization_msgs::msg::Marker createStringMarker(
+  const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type,
+  geometry_msgs::msg::Point point, geometry_msgs::msg::Vector3 scale,
+  const std_msgs::msg::ColorRGBA & color, const std::string text)
+{
+  visualization_msgs::msg::Marker marker;
+
+  marker = createMarker(frame_id, ns, id, type, point, scale, color);
+  marker.text = text;
+
+  return marker;
+}
+
+inline void appendMarkerArray(
+  const visualization_msgs::msg::MarkerArray & additional_marker_array,
+  visualization_msgs::msg::MarkerArray * marker_array)
+{
+  for (const auto & marker : additional_marker_array.markers) {
+    marker_array->markers.push_back(marker);
+  }
+}
+}  // namespace vehicle_cmd_gate
+
+#endif  // MARKER_HELPER_HPP_

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -14,6 +14,8 @@
 
 #include "vehicle_cmd_gate.hpp"
 
+#include "marker_helper.hpp"
+
 #include <rclcpp/logging.hpp>
 #include <tier4_api_utils/tier4_api_utils.hpp>
 
@@ -73,6 +75,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
 
   is_filter_activated_pub_ =
     create_publisher<IsFilterActivated>("~/is_filter_activated", durable_qos);
+  filter_activated_marker_pub_ =
+    create_publisher<MarkerArray>("~/is_filter_activated/marker", durable_qos);
 
   // Subscriber
   external_emergency_stop_heartbeat_sub_ = create_subscription<Heartbeat>(
@@ -419,8 +423,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
   // Check pause. Place this check after all other checks as it needs the final output.
   adapi_pause_->update(filtered_commands.control);
   if (adapi_pause_->is_paused()) {
-    filtered_commands.control.longitudinal.speed = 0.0;
-    filtered_commands.control.longitudinal.acceleration = stop_hold_acceleration_;
+    filtered_commands.control = createStopControlCmd();
   }
 
   // Check if command filtering option is enable
@@ -565,6 +568,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont
 
   is_filter_activated.stamp = now();
   is_filter_activated_pub_->publish(is_filter_activated);
+  filter_activated_marker_pub_->publish(createMarkerArray(is_filter_activated));
 
   return out;
 }
@@ -732,6 +736,53 @@ void VehicleCmdGate::checkExternalEmergencyStop(diagnostic_updater::DiagnosticSt
   stat.summary(status.level, status.message);
 }
 
+MarkerArray VehicleCmdGate::createMarkerArray(const IsFilterActivated & filter_activated)
+{
+  MarkerArray msg;
+
+  if (!filter_activated.is_activated) {
+    return msg;
+  }
+
+  /* add string marker */
+  bool first_msg = true;
+  std::string reason = "filter activated on";
+
+  if (filter_activated.is_activated_on_acceleration) {
+    reason += " lon_acc";
+    first_msg = false;
+  }
+  if (filter_activated.is_activated_on_jerk) {
+    reason += first_msg ? " jerk" : ", jerk";
+    first_msg = false;
+  }
+  if (filter_activated.is_activated_on_speed) {
+    reason += first_msg ? " speed" : ", speed";
+    first_msg = false;
+  }
+  if (filter_activated.is_activated_on_steering) {
+    reason += first_msg ? " steer" : ", steer";
+    first_msg = false;
+  }
+  if (filter_activated.is_activated_on_steering_rate) {
+    reason += first_msg ? " steer_rate" : ", steer_rate";
+    first_msg = false;
+  }
+
+  msg.markers.emplace_back(createStringMarker(
+    "base_link", "msg", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+    createMarkerPosition(0.0, 0.0, 1.0), createMarkerScale(0.0, 0.0, 1.0),
+    createMarkerColor(1.0, 0.0, 0.0, 1.0), reason));
+
+  /* add sphere marker */
+  msg.markers.emplace_back(createMarker(
+    "base_link", "sphere", 0, visualization_msgs::msg::Marker::SPHERE,
+    createMarkerPosition(0.0, 0.0, 0.0), createMarkerScale(3.0, 3.0, 3.0),
+    createMarkerColor(1.0, 0.0, 0.0, 0.3)));
+
+  return msg;
+}
+
 }  // namespace vehicle_cmd_gate
 
 #include <rclcpp_components/register_node_macro.hpp>

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -44,6 +44,7 @@
 #include <tier4_external_api_msgs/srv/set_emergency.hpp>
 #include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
 #include <tier4_vehicle_msgs/msg/vehicle_emergency_stamped.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <memory>
 
@@ -66,6 +67,7 @@ using tier4_external_api_msgs::srv::SetEmergency;
 using tier4_system_msgs::msg::MrmBehaviorStatus;
 using tier4_vehicle_msgs::msg::VehicleEmergencyStamped;
 using vehicle_cmd_gate::msg::IsFilterActivated;
+using visualization_msgs::msg::MarkerArray;
 
 using diagnostic_msgs::msg::DiagnosticStatus;
 using nav_msgs::msg::Odometry;
@@ -102,6 +104,7 @@ class VehicleCmdGate : public rclcpp::Node
   rclcpp::Publisher<EngageMsg>::SharedPtr engage_pub_;
   rclcpp::Publisher<OperationModeState>::SharedPtr operation_mode_pub_;
   rclcpp::Publisher<IsFilterActivated>::SharedPtr is_filter_activated_pub_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr filter_activated_marker_pub_;
 
   // Subscription
   rclcpp::Subscription<Heartbeat>::SharedPtr external_emergency_stop_heartbeat_sub_;
@@ -229,6 +232,9 @@ class VehicleCmdGate : public rclcpp::Node
   // stop checker
   std::unique_ptr<VehicleStopChecker> vehicle_stop_checker_;
   double stop_check_duration_;
+
+  // debug
+  MarkerArray createMarkerArray(const IsFilterActivated & filter_activated);
 };
 
 }  // namespace vehicle_cmd_gate

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml

@@ -1,8 +1,25 @@
 <?xml version="1.0"?>
 <launch>
+  <!-- Interface param -->
+  <arg name="output/objects" default="objects"/>
+
+  <!-- LiDAR param -->
   <arg name="input/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
-  <arg name="output/objects" default="objects"/>
+  <arg name="container_name" default="pointcloud_container"/>
+
+  <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
+  <arg name="score_threshold" default="0.35"/>
+
+  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+  <arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
+  <arg name="use_object_filter" default="true" description="use object filter"/>
+  <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
+  <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
+  <arg name="remove_unknown" default="true"/>
+  <arg name="trust_distance" default="30.0"/>
+
+  <!-- Camera param -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
   <arg name="camera_info0" default="/camera_info" description="camera info topic name"/>
   <arg name="detection_rois0" default="/perception/object_recognition/detection/rois0" description="detection rois output topic name"/>
@@ -28,16 +45,6 @@
   <arg name="camera_info7" default="/camera_info7"/>
   <arg name="detection_rois7" default="/perception/object_recognition/detection/rois7"/>
   <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
-  <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
-  <arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
-  <arg name="use_object_filter" default="true" description="use object filter"/>
-  <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
-  <arg name="container_name" default="pointcloud_container"/>
-  <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
-  <arg name="score_threshold" default="0.35"/>
-  <arg name="remove_unknown" default="true"/>
-  <arg name="trust_distance" default="30.0"/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">