feat: add marker of cmd gate filter (autowarefoundation#4741)

* feat: add marker of cmd gate filter

Signed-off-by: tomoya.kimura <[email protected]>

* feat: add sphere marker

Signed-off-by: tomoya.kimura <[email protected]>

* misc

Signed-off-by: tomoya.kimura <[email protected]>

---------

Signed-off-by: tomoya.kimura <[email protected]>

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>(
@@ -564,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;
 }
@@ -731,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