feat(perception_rviz_plugin): add 2d dummy bus with height (autowaref…

…oundation#903)

* feat(perception_rviz_plugin): add 2d dummy bus with height

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

common/tier4_perception_rviz_plugin/plugins/plugin_description.xml

@@ -7,6 +7,10 @@
          type="rviz_plugins::CarInitialPoseTool"
          base_class_type="rviz_common::Tool">
   </class>
+  <class name="rviz_plugins/BusInitialPoseTool"
+         type="rviz_plugins::BusInitialPoseTool"
+         base_class_type="rviz_common::Tool">
+  </class>
   <class name="rviz_plugins/UnknownInitialPoseTool"
          type="rviz_plugins::UnknownInitialPoseTool"
          base_class_type="rviz_common::Tool">

common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp

@@ -80,6 +80,12 @@ CarInitialPoseTool::CarInitialPoseTool()
   std_dev_theta_ = new rviz_common::properties::FloatProperty(
     "Theta std deviation", 5.0 * M_PI / 180.0, "Theta standard deviation for initial pose [rad]",
     getPropertyContainer());
+  length_ = new rviz_common::properties::FloatProperty(
+    "L vehicle length", 4.0, "L length for vehicle [m]", getPropertyContainer());
+  width_ = new rviz_common::properties::FloatProperty(
+    "W vehicle width", 1.8, "W width for vehicle [m]", getPropertyContainer());
+  height_ = new rviz_common::properties::FloatProperty(
+    "H vehicle height", 2.0, "H height for vehicle [m]", getPropertyContainer());
   position_z_ = new rviz_common::properties::FloatProperty(
     "Z position", 0.0, "Z position for initial pose [m]", getPropertyContainer());
   velocity_ = new rviz_common::properties::FloatProperty(
@@ -113,11 +119,9 @@ Object CarInitialPoseTool::createObjectMsg() const
 
   // shape
   object.shape.type = Shape::BOUNDING_BOX;
-  const double width = 1.8;
-  const double length = 4.0;
-  object.shape.dimensions.x = length;
-  object.shape.dimensions.y = width;
-  object.shape.dimensions.z = 2.0;
+  object.shape.dimensions.x = length_->getFloat();
+  object.shape.dimensions.y = width_->getFloat();
+  object.shape.dimensions.z = height_->getFloat();
 
   // initial state
   object.initial_state.pose_covariance.pose.position.z = position_z_->getFloat();
@@ -136,7 +140,94 @@ Object CarInitialPoseTool::createObjectMsg() const
 
   return object;
 }
+
+BusInitialPoseTool::BusInitialPoseTool()
+{
+  // short cut below k
+  shortcut_key_ = 'b';
+
+  enable_interactive_property_ = new rviz_common::properties::BoolProperty(
+    "Interactive", false, "Enable/Disable interactive action by manipulating mouse.",
+    getPropertyContainer());
+  property_frame_ = new rviz_common::properties::TfFrameProperty(
+    "Target Frame", rviz_common::properties::TfFrameProperty::FIXED_FRAME_STRING,
+    "The TF frame where the point cloud is output.", getPropertyContainer(), nullptr, true);
+  topic_property_ = new rviz_common::properties::StringProperty(
+    "Pose Topic", "/simulation/dummy_perception_publisher/object_info",
+    "The topic on which to publish dummy object info.", getPropertyContainer(), SLOT(updateTopic()),
+    this);
+  std_dev_x_ = new rviz_common::properties::FloatProperty(
+    "X std deviation", 0.03, "X standard deviation for initial pose [m]", getPropertyContainer());
+  std_dev_y_ = new rviz_common::properties::FloatProperty(
+    "Y std deviation", 0.03, "Y standard deviation for initial pose [m]", getPropertyContainer());
+  std_dev_z_ = new rviz_common::properties::FloatProperty(
+    "Z std deviation", 0.03, "Z standard deviation for initial pose [m]", getPropertyContainer());
+  std_dev_theta_ = new rviz_common::properties::FloatProperty(
+    "Theta std deviation", 5.0 * M_PI / 180.0, "Theta standard deviation for initial pose [rad]",
+    getPropertyContainer());
+  length_ = new rviz_common::properties::FloatProperty(
+    "L vehicle length", 10.5, "L length for vehicle [m]", getPropertyContainer());
+  width_ = new rviz_common::properties::FloatProperty(
+    "W vehicle width", 2.5, "W width for vehicle [m]", getPropertyContainer());
+  height_ = new rviz_common::properties::FloatProperty(
+    "H vehicle height", 3.5, "H height for vehicle [m]", getPropertyContainer());
+  position_z_ = new rviz_common::properties::FloatProperty(
+    "Z position", 0.0, "Z position for initial pose [m]", getPropertyContainer());
+  velocity_ = new rviz_common::properties::FloatProperty(
+    "Velocity", 0.0, "velocity [m/s]", getPropertyContainer());
+  std_dev_x_->setMin(0);
+  std_dev_y_->setMin(0);
+  std_dev_z_->setMin(0);
+  std_dev_theta_->setMin(0);
+  position_z_->setMin(0);
+}
+
+void BusInitialPoseTool::onInitialize()
+{
+  PoseTool::onInitialize();
+  setName("2D Dummy Bus");
+  updateTopic();
+}
+
+Object BusInitialPoseTool::createObjectMsg() const
+{
+  Object object{};
+  std::string fixed_frame = context_->getFixedFrame().toStdString();
+
+  // header
+  object.header.frame_id = fixed_frame;
+  object.header.stamp = clock_->now();
+
+  // semantic
+  object.classification.label = ObjectClassification::BUS;
+  object.classification.probability = 1.0;
+
+  // shape
+  object.shape.type = Shape::BOUNDING_BOX;
+  object.shape.dimensions.x = length_->getFloat();
+  object.shape.dimensions.y = width_->getFloat();
+  object.shape.dimensions.z = height_->getFloat();
+
+  // initial state
+  object.initial_state.pose_covariance.pose.position.z = position_z_->getFloat();
+  object.initial_state.pose_covariance.covariance[0] =
+    std_dev_x_->getFloat() * std_dev_x_->getFloat();
+  object.initial_state.pose_covariance.covariance[7] =
+    std_dev_y_->getFloat() * std_dev_y_->getFloat();
+  object.initial_state.pose_covariance.covariance[14] =
+    std_dev_z_->getFloat() * std_dev_z_->getFloat();
+  object.initial_state.pose_covariance.covariance[35] =
+    std_dev_theta_->getFloat() * std_dev_theta_->getFloat();
+
+  std::mt19937 gen(std::random_device{}());
+  std::independent_bits_engine<std::mt19937, 8, uint8_t> bit_eng(gen);
+  std::generate(object.id.uuid.begin(), object.id.uuid.end(), bit_eng);
+
+  return object;
+}
+
 }  // end namespace rviz_plugins
 
 #include <pluginlib/class_list_macros.hpp>
 PLUGINLIB_EXPORT_CLASS(rviz_plugins::CarInitialPoseTool, rviz_common::Tool)
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::BusInitialPoseTool, rviz_common::Tool)

common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp

@@ -65,6 +65,14 @@ class CarInitialPoseTool : public InteractiveObjectTool
   Object createObjectMsg() const override;
 };
 
+class BusInitialPoseTool : public InteractiveObjectTool
+{
+public:
+  BusInitialPoseTool();
+  void onInitialize();
+  Object createObjectMsg() const override;
+};
+
 }  // namespace rviz_plugins
 
 #endif  // TOOLS__CAR_POSE_HPP_

common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp

@@ -151,6 +151,9 @@ protected Q_SLOTS:
   rviz_common::properties::FloatProperty * std_dev_x_;
   rviz_common::properties::FloatProperty * std_dev_y_;
   rviz_common::properties::FloatProperty * std_dev_z_;
+  rviz_common::properties::FloatProperty * width_;
+  rviz_common::properties::FloatProperty * length_;
+  rviz_common::properties::FloatProperty * height_;
   rviz_common::properties::FloatProperty * std_dev_theta_;
   rviz_common::properties::FloatProperty * position_z_;
   rviz_common::properties::FloatProperty * velocity_;