feat(tier4_perception_rviz_plugin): add bicycle panel (autowarefounda…

…tion#3157) select motorcycle or bicycle and publish Signed-off-by: Mamoru Sobue <[email protected]>
soblin · May 7, 2023 · d0d0ceb · d0d0ceb
1 parent 3de1967
commit d0d0ceb
Show file tree

Hide file tree

Showing 5 changed files with 112 additions and 0 deletions.
diff --git a/common/tier4_perception_rviz_plugin/icons/classes/BikeInitialPoseTool.png b/common/tier4_perception_rviz_plugin/icons/classes/BikeInitialPoseTool.png
diff --git a/common/tier4_perception_rviz_plugin/plugins/plugin_description.xml b/common/tier4_perception_rviz_plugin/plugins/plugin_description.xml
@@ -11,6 +11,10 @@
          type="rviz_plugins::BusInitialPoseTool"
          base_class_type="rviz_common::Tool">
   </class>
+  <class name="rviz_plugins/BikeInitialPoseTool"
+         type="rviz_plugins::BikeInitialPoseTool"
+         base_class_type="rviz_common::Tool">
+  </class>
   <class name="rviz_plugins/UnknownInitialPoseTool"
          type="rviz_plugins::UnknownInitialPoseTool"
          base_class_type="rviz_common::Tool">

diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp
@@ -238,8 +238,104 @@ Object BusInitialPoseTool::createObjectMsg() const
   return object;
 }
 
+BikeInitialPoseTool::BikeInitialPoseTool()
+{
+  // short cut below cyclist
+  shortcut_key_ = 'c';
+
+  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", 1.5, "L length for vehicle [m]", getPropertyContainer());
+  width_ = new rviz_common::properties::FloatProperty(
+    "W vehicle width", 0.5, "W width for vehicle [m]", getPropertyContainer());
+  height_ = new rviz_common::properties::FloatProperty(
+    "H vehicle height", 1.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(
+    "Velocity", 0.0, "velocity [m/s]", getPropertyContainer());
+  accel_ = new rviz_common::properties::FloatProperty(
+    "Acceleration", 0.0, "acceleration [m/s^2]", getPropertyContainer());
+  max_velocity_ = new rviz_common::properties::FloatProperty(
+    "Max velocity", 33.3, "Max velocity [m/s]", getPropertyContainer());
+  min_velocity_ = new rviz_common::properties::FloatProperty(
+    "Min velocity", -33.3, "Min velocity [m/s]", getPropertyContainer());
+  label_ = new rviz_common::properties::EnumProperty(
+    "Type", "BICYCLE", "BICYCLE or MOTORCYCLE", 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);
+  label_->addOption("BICYCLE", ObjectClassification::BICYCLE);
+  label_->addOption("MOTORCYCLE", ObjectClassification::MOTORCYCLE);
+}
+
+void BikeInitialPoseTool::onInitialize()
+{
+  rviz_plugins::InteractiveObjectTool::onInitialize();
+  setName("2D Dummy Bicycle");
+  updateTopic();
+}
+
+Object BikeInitialPoseTool::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 = label_->getOptionInt();
+  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)
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::BikeInitialPoseTool, rviz_common::Tool)
diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp
@@ -73,6 +73,17 @@ class BusInitialPoseTool : public InteractiveObjectTool
   [[nodiscard]] Object createObjectMsg() const override;
 };
 
+class BikeInitialPoseTool : public InteractiveObjectTool
+{
+public:
+  BikeInitialPoseTool();
+  void onInitialize() override;
+  [[nodiscard]] Object createObjectMsg() const override;
+
+private:
+  rviz_common::properties::EnumProperty * label_;
+};
+
 }  // namespace rviz_plugins
 
 #endif  // TOOLS__CAR_POSE_HPP_
diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp
@@ -52,6 +52,7 @@
 #include <QObject>
 #include <rclcpp/node.hpp>
 #include <rviz_common/properties/bool_property.hpp>
+#include <rviz_common/properties/enum_property.hpp>
 #include <rviz_common/properties/float_property.hpp>
 #include <rviz_common/properties/string_property.hpp>
 #include <rviz_common/properties/tf_frame_property.hpp>