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