diff --git a/common/tier4_perception_rviz_plugin/icons/classes/BikeInitialPoseTool.png b/common/tier4_perception_rviz_plugin/icons/classes/BikeInitialPoseTool.png
new file mode 100644
index 0000000000000..6a67573717ae1
Binary files /dev/null and b/common/tier4_perception_rviz_plugin/icons/classes/BikeInitialPoseTool.png differ
diff --git a/common/tier4_perception_rviz_plugin/plugins/plugin_description.xml b/common/tier4_perception_rviz_plugin/plugins/plugin_description.xml
index 3e75efc966985..853a471c7c6e4 100644
--- 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">
+
+
diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp
index 654683ea925eb..cf07eeed99b3a 100644
--- 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 bit_eng(gen);
+ std::generate(object.id.uuid.begin(), object.id.uuid.end(), bit_eng);
+
+ return object;
+}
+
} // end namespace rviz_plugins
#include
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
index 43afe7db95dcd..4fc6aeb71771d 100644
--- 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
index 6f0b382eb0b58..fc40b08be822b 100644
--- 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
#include
#include
+#include
#include
#include
#include