diff --git a/common/tier4_perception_rviz_plugin/icons/classes/BusInitialPoseTool.png b/common/tier4_perception_rviz_plugin/icons/classes/BusInitialPoseTool.png
new file mode 100644
index 0000000000000..6a67573717ae1
Binary files /dev/null and b/common/tier4_perception_rviz_plugin/icons/classes/BusInitialPoseTool.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 5ab5eaa461eba..3e75efc966985 100644
--- a/common/tier4_perception_rviz_plugin/plugins/plugin_description.xml
+++ b/common/tier4_perception_rviz_plugin/plugins/plugin_description.xml
@@ -7,6 +7,10 @@
type="rviz_plugins::CarInitialPoseTool"
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 9c16651863487..0a9ea758877ea 100644
--- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp
+++ b/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 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)
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 6dfb06ee87bd1..1fa78da223a1f 100644
--- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp
+++ b/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_
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 4eb879d1584ee..9dbe5335ecc8d 100644
--- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp
+++ b/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_;