From aa8840ccf9f13485e99240bebdf10530ddc65798 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 22 Mar 2022 09:50:09 +0900 Subject: [PATCH 1/5] fix(tier4_perception_rviz_plugin): split pedestrian.hpp(cpp) into two parts Signed-off-by: satoshi-ota --- .../CMakeLists.txt | 1 + .../src/tools/interactive_object.cpp | 217 ++++++++++++++++++ .../src/tools/interactive_object.hpp | 116 ++++++++++ .../src/tools/pedestrian_pose.cpp | 173 +------------- .../src/tools/pedestrian_pose.hpp | 42 +--- 5 files changed, 342 insertions(+), 207 deletions(-) create mode 100644 common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp create mode 100644 common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp diff --git a/common/tier4_perception_rviz_plugin/CMakeLists.txt b/common/tier4_perception_rviz_plugin/CMakeLists.txt index 2bac295514315..a686fe78003b4 100644 --- a/common/tier4_perception_rviz_plugin/CMakeLists.txt +++ b/common/tier4_perception_rviz_plugin/CMakeLists.txt @@ -22,6 +22,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) ## Declare a C++ library ament_auto_add_library(tier4_perception_rviz_plugin SHARED src/tools/util.cpp + src/tools/interactive_object.cpp src/tools/pedestrian_pose.cpp src/tools/car_pose.cpp src/tools/unknown_pose.cpp diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp new file mode 100644 index 0000000000000..8c352952960a4 --- /dev/null +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp @@ -0,0 +1,217 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "interactive_object.hpp" + +#include "util.hpp" + +#include +#include +#include +#include + +namespace rviz_plugins +{ +InteractiveObject::InteractiveObject(const Ogre::Vector3 & point) +{ + velocity_ = Ogre::Vector3::ZERO; + point_ = point; + theta_ = 0.0; + + std::mt19937 gen(std::random_device{}()); + std::independent_bits_engine bit_eng(gen); + std::generate(uuid_.begin(), uuid_.end(), bit_eng); +} + +std::array InteractiveObject::uuid() const { return uuid_; } + +void InteractiveObject::twist(geometry_msgs::msg::Twist & twist) const +{ + twist.linear.x = velocity_.length(); + twist.linear.y = 0.0; + twist.linear.z = 0.0; +} + +void InteractiveObject::transform(tf2::Transform & tf_map2object) const +{ + tf2::Transform tf_object_origin2moved_object; + tf2::Transform tf_map2object_origin; + + { + geometry_msgs::msg::Transform ros_object_origin2moved_object; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta_); + ros_object_origin2moved_object.rotation = tf2::toMsg(quat); + tf2::fromMsg(ros_object_origin2moved_object, tf_object_origin2moved_object); + } + + { + geometry_msgs::msg::Transform ros_object_map2object_origin; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, 0.0); + ros_object_map2object_origin.rotation = tf2::toMsg(quat); + ros_object_map2object_origin.translation.x = point_.x; + ros_object_map2object_origin.translation.y = point_.y; + ros_object_map2object_origin.translation.z = point_.z; + tf2::fromMsg(ros_object_map2object_origin, tf_map2object_origin); + } + + tf_map2object = tf_map2object_origin * tf_object_origin2moved_object; +} + +void InteractiveObject::update(const Ogre::Vector3 & point) +{ + velocity_ = point - point_; + point_ = point; + theta_ = + (velocity_.x < 1.0e-3 && velocity_.y < 1.0e-3) ? theta_ : std::atan2(velocity_.y, velocity_.x); +} + +double InteractiveObject::distance(const Ogre::Vector3 & point) { return point_.distance(point); } + +InteractiveObjectCollection::InteractiveObjectCollection() { target_ = nullptr; } + +void InteractiveObjectCollection::reset() { target_ = nullptr; } + +void InteractiveObjectCollection::select(const Ogre::Vector3 & point) +{ + const size_t index = nearest(point); + if (index != objects_.size()) { + target_ = objects_[index].get(); + } +} + +boost::optional> InteractiveObjectCollection::create( + const Ogre::Vector3 & point) +{ + objects_.emplace_back(std::make_unique(point)); + target_ = objects_.back().get(); + + if (target_) { + return target_->uuid(); + } + + return {}; +} + +boost::optional> InteractiveObjectCollection::remove( + const Ogre::Vector3 & point) +{ + const size_t index = nearest(point); + if (index != objects_.size()) { + const auto removed_uuid = objects_[index].get()->uuid(); + objects_.erase(objects_.begin() + index); + return removed_uuid; + } + + return {}; +} + +boost::optional> InteractiveObjectCollection::update( + const Ogre::Vector3 & point) +{ + if (target_) { + target_->update(point); + return target_->uuid(); + } + + return {}; +} + +boost::optional InteractiveObjectCollection::twist( + const std::array & uuid) const +{ + for (const auto & object : objects_) { + if (object->uuid() != uuid) { + continue; + } + + geometry_msgs::msg::Twist ret; + object->twist(ret); + return ret; + } + + return {}; +} + +boost::optional InteractiveObjectCollection::transform( + const std::array & uuid) const +{ + for (const auto & object : objects_) { + if (object->uuid() != uuid) { + continue; + } + + tf2::Transform ret; + object->transform(ret); + return ret; + } + + return {}; +} + +size_t InteractiveObjectCollection::nearest(const Ogre::Vector3 & point) +{ + const size_t npos = objects_.size(); + if (objects_.empty()) { + return npos; + } + + std::vector distances; + for (const auto & object : objects_) { + distances.push_back(object->distance(point)); + } + + const auto compare = [&](int x, int y) { return distances[x] < distances[y]; }; + std::vector indices(distances.size()); + std::iota(indices.begin(), indices.end(), 0); + std::sort(indices.begin(), indices.end(), compare); + + const size_t index = indices[0]; + return distances[index] < 2.0 ? index : npos; +} +} // end namespace rviz_plugins diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp new file mode 100644 index 0000000000000..a669028c802be --- /dev/null +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp @@ -0,0 +1,116 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TOOLS__INTERACTIVE_OBJECT_HPP_ +#define TOOLS__INTERACTIVE_OBJECT_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +#include + +#include + +#include +#include + +#include +#include +#include + +namespace rviz_plugins +{ + +class InteractiveObject +{ +public: + explicit InteractiveObject(const Ogre::Vector3 & point); + ~InteractiveObject() {} + + std::array uuid() const; + void twist(geometry_msgs::msg::Twist & twist) const; + void transform(tf2::Transform & tf_map2object) const; + void update(const Ogre::Vector3 & point); + double distance(const Ogre::Vector3 & point); + +private: + std::array uuid_; + Ogre::Vector3 point_; + Ogre::Vector3 velocity_; + double theta_; +}; + +class InteractiveObjectCollection +{ +public: + InteractiveObjectCollection(); + ~InteractiveObjectCollection() {} + + void reset(); + void select(const Ogre::Vector3 & point); + boost::optional> create(const Ogre::Vector3 & point); + boost::optional> remove(const Ogre::Vector3 & point); + boost::optional> update(const Ogre::Vector3 & point); + boost::optional twist(const std::array & uuid) const; + boost::optional transform(const std::array & uuid) const; + +private: + size_t nearest(const Ogre::Vector3 & point); + InteractiveObject * target_; + std::vector> objects_; +}; +} // namespace rviz_plugins +#endif // TOOLS__INTERACTIVE_OBJECT_HPP_ diff --git a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp index cac7e6ac72f7e..d19166174d335 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp @@ -58,167 +58,6 @@ namespace rviz_plugins { -InteractivePedestrian::InteractivePedestrian(const Ogre::Vector3 & point) -{ - velocity_ = Ogre::Vector3::ZERO; - point_ = point; - theta_ = 0.0; - - std::mt19937 gen(std::random_device{}()); - std::independent_bits_engine bit_eng(gen); - std::generate(uuid_.begin(), uuid_.end(), bit_eng); -} - -std::array InteractivePedestrian::uuid() const { return uuid_; } - -void InteractivePedestrian::twist(geometry_msgs::msg::Twist & twist) const -{ - twist.linear.x = velocity_.length(); - twist.linear.y = 0.0; - twist.linear.z = 0.0; -} - -void InteractivePedestrian::transform(tf2::Transform & tf_map2object) const -{ - tf2::Transform tf_object_origin2moved_object; - tf2::Transform tf_map2object_origin; - - { - geometry_msgs::msg::Transform ros_object_origin2moved_object; - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, theta_); - ros_object_origin2moved_object.rotation = tf2::toMsg(quat); - tf2::fromMsg(ros_object_origin2moved_object, tf_object_origin2moved_object); - } - - { - geometry_msgs::msg::Transform ros_object_map2object_origin; - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, 0.0); - ros_object_map2object_origin.rotation = tf2::toMsg(quat); - ros_object_map2object_origin.translation.x = point_.x; - ros_object_map2object_origin.translation.y = point_.y; - ros_object_map2object_origin.translation.z = point_.z; - tf2::fromMsg(ros_object_map2object_origin, tf_map2object_origin); - } - - tf_map2object = tf_map2object_origin * tf_object_origin2moved_object; -} - -void InteractivePedestrian::update(const Ogre::Vector3 & point) -{ - velocity_ = point - point_; - point_ = point; - theta_ = - (velocity_.x < 1.0e-3 && velocity_.y < 1.0e-3) ? theta_ : std::atan2(velocity_.y, velocity_.x); -} - -double InteractivePedestrian::distance(const Ogre::Vector3 & point) -{ - return point_.distance(point); -} - -InteractivePedestrianCollection::InteractivePedestrianCollection() { target_ = nullptr; } - -void InteractivePedestrianCollection::reset() { target_ = nullptr; } - -void InteractivePedestrianCollection::select(const Ogre::Vector3 & point) -{ - const size_t index = nearest(point); - if (index != objects_.size()) { - target_ = objects_[index].get(); - } -} - -boost::optional> InteractivePedestrianCollection::create( - const Ogre::Vector3 & point) -{ - objects_.emplace_back(std::make_unique(point)); - target_ = objects_.back().get(); - - if (target_) { - return target_->uuid(); - } - - return {}; -} - -boost::optional> InteractivePedestrianCollection::remove( - const Ogre::Vector3 & point) -{ - const size_t index = nearest(point); - if (index != objects_.size()) { - const auto removed_uuid = objects_[index].get()->uuid(); - objects_.erase(objects_.begin() + index); - return removed_uuid; - } - - return {}; -} - -boost::optional> InteractivePedestrianCollection::update( - const Ogre::Vector3 & point) -{ - if (target_) { - target_->update(point); - return target_->uuid(); - } - - return {}; -} - -boost::optional InteractivePedestrianCollection::twist( - const std::array & uuid) const -{ - for (const auto & object : objects_) { - if (object->uuid() != uuid) { - continue; - } - - geometry_msgs::msg::Twist ret; - object->twist(ret); - return ret; - } - - return {}; -} - -boost::optional InteractivePedestrianCollection::transform( - const std::array & uuid) const -{ - for (const auto & object : objects_) { - if (object->uuid() != uuid) { - continue; - } - - tf2::Transform ret; - object->transform(ret); - return ret; - } - - return {}; -} - -size_t InteractivePedestrianCollection::nearest(const Ogre::Vector3 & point) -{ - const size_t npos = objects_.size(); - if (objects_.empty()) { - return npos; - } - - std::vector distances; - for (const auto & object : objects_) { - distances.push_back(object->distance(point)); - } - - const auto compare = [&](int x, int y) { return distances[x] < distances[y]; }; - std::vector indices(distances.size()); - std::iota(indices.begin(), indices.end(), 0); - std::sort(indices.begin(), indices.end(), compare); - - const size_t index = indices[0]; - return distances[index] < 2.0 ? index : npos; -} PedestrianInitialPoseTool::PedestrianInitialPoseTool() { shortcut_key_ = 'l'; @@ -263,8 +102,7 @@ void PedestrianInitialPoseTool::onInitialize() void PedestrianInitialPoseTool::updateTopic() { rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - dummy_object_info_pub_ = raw_node->create_publisher( - topic_property_->getStdString(), 1); + dummy_object_info_pub_ = raw_node->create_publisher(topic_property_->getStdString(), 1); clock_ = raw_node->get_clock(); move_tool_.initialize(context_); property_frame_->setFrameManager(context_->getFrameManager()); @@ -276,7 +114,7 @@ void PedestrianInitialPoseTool::onPoseSet(double x, double y, double theta) return; } - dummy_perception_publisher::msg::Object output_msg; + Object output_msg; std::string fixed_frame = context_->getFixedFrame().toStdString(); // header @@ -284,12 +122,11 @@ void PedestrianInitialPoseTool::onPoseSet(double x, double y, double theta) output_msg.header.stamp = clock_->now(); // semantic - output_msg.classification.label = - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + output_msg.classification.label = ObjectClassification::PEDESTRIAN; output_msg.classification.probability = 1.0; // shape - output_msg.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + output_msg.shape.type = Shape::CYLINDER; const double width = 0.8; const double length = 0.8; output_msg.shape.dimensions.x = length; @@ -324,7 +161,7 @@ void PedestrianInitialPoseTool::onPoseSet(double x, double y, double theta) velocity_->getFloat(), 0.0, 0.0, fixed_frame.c_str()); // action - output_msg.action = dummy_perception_publisher::msg::Object::ADD; + output_msg.action = Object::ADD; // id std::mt19937 gen(std::random_device{}()); diff --git a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp index f7375eb734297..61339c1e4d175 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp @@ -48,6 +48,8 @@ #ifndef TOOLS__PEDESTRIAN_POSE_HPP_ #define TOOLS__PEDESTRIAN_POSE_HPP_ +#include "interactive_object.hpp" + #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 #include #include @@ -78,44 +80,6 @@ using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; -class InteractivePedestrian -{ -public: - explicit InteractivePedestrian(const Ogre::Vector3 & point); - ~InteractivePedestrian() {} - - std::array uuid() const; - void twist(geometry_msgs::msg::Twist & twist) const; - void transform(tf2::Transform & tf_map2object) const; - void update(const Ogre::Vector3 & point); - double distance(const Ogre::Vector3 & point); - -private: - std::array uuid_; - Ogre::Vector3 point_; - Ogre::Vector3 velocity_; - double theta_; -}; - -class InteractivePedestrianCollection -{ -public: - InteractivePedestrianCollection(); - ~InteractivePedestrianCollection() {} - - void reset(); - void select(const Ogre::Vector3 & point); - boost::optional> create(const Ogre::Vector3 & point); - boost::optional> remove(const Ogre::Vector3 & point); - boost::optional> update(const Ogre::Vector3 & point); - boost::optional twist(const std::array & uuid) const; - boost::optional transform(const std::array & uuid) const; - -private: - size_t nearest(const Ogre::Vector3 & point); - InteractivePedestrian * target_; - std::vector> objects_; -}; class PedestrianInitialPoseTool : public rviz_default_plugins::tools::PoseTool { Q_OBJECT @@ -149,7 +113,7 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * velocity_; rviz_common::properties::TfFrameProperty * property_frame_; - InteractivePedestrianCollection objects_; + InteractiveObjectCollection objects_; void publishObjectMsg(const std::array & uuid, const uint32_t action); }; From 61d625d7c287620ebca73cfd8d45e7744166552a Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 22 Mar 2022 10:50:57 +0900 Subject: [PATCH 2/5] fix(tier4_perception_rviz_plugin): modify logic of velocity & pose calculation Signed-off-by: satoshi-ota --- .../src/tools/interactive_object.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp index 8c352952960a4..4f951928ea051 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp @@ -105,10 +105,11 @@ void InteractiveObject::transform(tf2::Transform & tf_map2object) const void InteractiveObject::update(const Ogre::Vector3 & point) { - velocity_ = point - point_; - point_ = point; - theta_ = - (velocity_.x < 1.0e-3 && velocity_.y < 1.0e-3) ? theta_ : std::atan2(velocity_.y, velocity_.x); + velocity_ = (point - point_) * 0.05; + point_ += velocity_; + theta_ = (std::abs(velocity_.x) < 1.0e-6 && std::abs(velocity_.y) < 1.0e-6) + ? theta_ + : std::atan2(velocity_.y, velocity_.x); } double InteractiveObject::distance(const Ogre::Vector3 & point) { return point_.distance(point); } From 31fdc6c9b866871ea6dda32a095d4358e4640712 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 22 Mar 2022 11:06:23 +0900 Subject: [PATCH 3/5] feat(tier4_perception_rviz_plugin): plugin supports 2D Interactive dummy car Signed-off-by: satoshi-ota --- .../src/tools/car_pose.cpp | 127 +++++++++++++++++- .../src/tools/car_pose.hpp | 19 ++- 2 files changed, 139 insertions(+), 7 deletions(-) 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 75038d85619de..426fc044f2e0f 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp @@ -47,6 +47,8 @@ #include "car_pose.hpp" +#include "util.hpp" + #include #include @@ -62,6 +64,12 @@ CarInitialPoseTool::CarInitialPoseTool() { shortcut_key_ = 'k'; + 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()), @@ -96,14 +104,19 @@ void CarInitialPoseTool::onInitialize() void CarInitialPoseTool::updateTopic() { rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - dummy_object_info_pub_ = raw_node->create_publisher( - topic_property_->getStdString(), 1); + dummy_object_info_pub_ = raw_node->create_publisher(topic_property_->getStdString(), 1); clock_ = raw_node->get_clock(); + move_tool_.initialize(context_); + property_frame_->setFrameManager(context_->getFrameManager()); } void CarInitialPoseTool::onPoseSet(double x, double y, double theta) { - dummy_perception_publisher::msg::Object output_msg; + if (enable_interactive_property_->getBool()) { + return; + } + + Object output_msg; std::string fixed_frame = context_->getFixedFrame().toStdString(); // header @@ -111,11 +124,11 @@ void CarInitialPoseTool::onPoseSet(double x, double y, double theta) output_msg.header.stamp = clock_->now(); // semantic - output_msg.classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + output_msg.classification.label = ObjectClassification::CAR; output_msg.classification.probability = 1.0; // shape - output_msg.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + output_msg.shape.type = Shape::BOUNDING_BOX; const double width = 1.8; const double length = 4.0; output_msg.shape.dimensions.x = length; @@ -150,7 +163,7 @@ void CarInitialPoseTool::onPoseSet(double x, double y, double theta) velocity_->getFloat(), 0.0, 0.0, fixed_frame.c_str()); // action - output_msg.action = dummy_perception_publisher::msg::Object::ADD; + output_msg.action = Object::ADD; // id std::mt19937 gen(std::random_device{}()); @@ -160,6 +173,108 @@ void CarInitialPoseTool::onPoseSet(double x, double y, double theta) dummy_object_info_pub_->publish(output_msg); } +void CarInitialPoseTool::publishObjectMsg( + const std::array & uuid, const uint32_t action) +{ + Object output_msg; + std::string fixed_frame = context_->getFixedFrame().toStdString(); + + // header + output_msg.header.frame_id = fixed_frame; + output_msg.header.stamp = clock_->now(); + + // action + output_msg.action = action; + + // id + output_msg.id.uuid = uuid; + + if (action == Object::DELETE) { + dummy_object_info_pub_->publish(output_msg); + return; + } + + const auto object_tf = objects_.transform(uuid); + const auto object_twist = objects_.twist(uuid); + + if (!object_tf || !object_twist) { + return; + } + + // semantic + output_msg.classification.label = ObjectClassification::CAR; + output_msg.classification.probability = 1.0; + + // shape + output_msg.shape.type = Shape::BOUNDING_BOX; + const double width = 1.8; + const double length = 4.0; + output_msg.shape.dimensions.x = length; + output_msg.shape.dimensions.y = width; + output_msg.shape.dimensions.z = 2.0; + + // initial state + // pose + tf2::toMsg(object_tf.get(), output_msg.initial_state.pose_covariance.pose); + output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[0] = + std_dev_x_->getFloat() * std_dev_x_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[7] = + std_dev_y_->getFloat() * std_dev_y_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[14] = + std_dev_z_->getFloat() * std_dev_z_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[35] = + std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); + // twist + output_msg.initial_state.twist_covariance.twist = object_twist.get(); + + dummy_object_info_pub_->publish(output_msg); +} + +int CarInitialPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (!enable_interactive_property_->getBool()) { + return PoseTool::processMouseEvent(event); + } + + if (event.rightDown()) { + const auto point = get_point_from_mouse(event); + if (point) { + if (event.shift()) { + const auto uuid = objects_.create(point.value()); + publishObjectMsg(uuid.get(), Object::ADD); + } else if (event.alt()) { + const auto uuid = objects_.remove(point.value()); + publishObjectMsg(uuid.get(), Object::DELETE); + } else { + objects_.select(point.value()); + } + } + return 0; + } + + if (event.rightUp()) { + objects_.reset(); + return 0; + } + + if (event.right()) { + const auto point = get_point_from_mouse(event); + if (point) { + const auto uuid = objects_.update(point.value()); + publishObjectMsg(uuid.get(), Object::MODIFY); + } + return 0; + } + + return move_tool_.processMouseEvent(event); +} + +int CarInitialPoseTool::processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) +{ + PoseTool::processKeyEvent(event, panel); + return move_tool_.processKeyEvent(event, panel); +} } // end namespace rviz_plugins #include 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 adc3a671676bd..c4c6d32b4b82d 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp @@ -48,6 +48,8 @@ #ifndef TOOLS__CAR_POSE_HPP_ #define TOOLS__CAR_POSE_HPP_ +#include "interactive_object.hpp" + #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 #include #include @@ -61,6 +63,11 @@ namespace rviz_plugins { + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::Shape; +using dummy_perception_publisher::msg::Object; + class CarInitialPoseTool : public rviz_default_plugins::tools::PoseTool { Q_OBJECT @@ -69,6 +76,8 @@ class CarInitialPoseTool : public rviz_default_plugins::tools::PoseTool CarInitialPoseTool(); virtual ~CarInitialPoseTool() {} virtual void onInitialize(); + int processMouseEvent(rviz_common::ViewportMouseEvent & event) override; + int processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) override; protected: virtual void onPoseSet(double x, double y, double theta); @@ -78,8 +87,11 @@ private Q_SLOTS: private: rclcpp::Clock::SharedPtr clock_; - rclcpp::Publisher::SharedPtr dummy_object_info_pub_; + rclcpp::Publisher::SharedPtr dummy_object_info_pub_; + rviz_default_plugins::tools::MoveTool move_tool_; + + rviz_common::properties::BoolProperty * enable_interactive_property_; rviz_common::properties::StringProperty * topic_property_; rviz_common::properties::FloatProperty * std_dev_x_; rviz_common::properties::FloatProperty * std_dev_y_; @@ -87,6 +99,11 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * std_dev_theta_; rviz_common::properties::FloatProperty * position_z_; rviz_common::properties::FloatProperty * velocity_; + rviz_common::properties::TfFrameProperty * property_frame_; + + InteractiveObjectCollection objects_; + + void publishObjectMsg(const std::array & uuid, const uint32_t action); }; } // namespace rviz_plugins From be319973fbc4541e5a2fa6bdd42decaa68da01be Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 22 Mar 2022 11:22:59 +0900 Subject: [PATCH 4/5] feat(tier4_perception_rviz_plugin): plugin supports 2D Interactive unknown object Signed-off-by: satoshi-ota --- .../src/tools/unknown_pose.cpp | 134 ++++++++++++++++-- .../src/tools/unknown_pose.hpp | 23 ++- 2 files changed, 146 insertions(+), 11 deletions(-) diff --git a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp index c88e631f361c0..9d8b207c7f8e1 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp @@ -42,6 +42,8 @@ #include "unknown_pose.hpp" +#include "util.hpp" + #include #include #include @@ -61,6 +63,12 @@ UnknownInitialPoseTool::UnknownInitialPoseTool() { shortcut_key_ = 'u'; + 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()), @@ -95,14 +103,19 @@ void UnknownInitialPoseTool::onInitialize() void UnknownInitialPoseTool::updateTopic() { rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - dummy_object_info_pub_ = raw_node->create_publisher( - topic_property_->getStdString(), 1); + dummy_object_info_pub_ = raw_node->create_publisher(topic_property_->getStdString(), 1); clock_ = raw_node->get_clock(); + move_tool_.initialize(context_); + property_frame_->setFrameManager(context_->getFrameManager()); } void UnknownInitialPoseTool::onPoseSet(double x, double y, double theta) { - dummy_perception_publisher::msg::Object output_msg; + if (enable_interactive_property_->getBool()) { + return; + } + + Object output_msg; std::string fixed_frame = context_->getFixedFrame().toStdString(); // header @@ -110,12 +123,11 @@ void UnknownInitialPoseTool::onPoseSet(double x, double y, double theta) output_msg.header.stamp = clock_->now(); // semantic - output_msg.classification.label = - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + output_msg.classification.label = ObjectClassification::UNKNOWN; output_msg.classification.probability = 1.0; // shape - output_msg.shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + output_msg.shape.type = Shape::POLYGON; const double width = 0.8; const double length = 0.8; output_msg.shape.dimensions.x = length; @@ -139,18 +151,18 @@ void UnknownInitialPoseTool::onPoseSet(double x, double y, double theta) quat.setRPY(0.0, 0.0, theta); output_msg.initial_state.pose_covariance.pose.orientation = tf2::toMsg(quat); RCLCPP_INFO( - rclcpp::get_logger("PedestrianInitialPoseTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", - x, y, position_z_->getFloat(), theta, fixed_frame.c_str()); + rclcpp::get_logger("UnknownInitialPoseTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, + y, position_z_->getFloat(), theta, fixed_frame.c_str()); // twist output_msg.initial_state.twist_covariance.twist.linear.x = velocity_->getFloat(); output_msg.initial_state.twist_covariance.twist.linear.y = 0.0; output_msg.initial_state.twist_covariance.twist.linear.z = 0.0; RCLCPP_INFO( - rclcpp::get_logger("PedestrianInitialPoseTool"), "Setting twist: %.3f %.3f %.3f [frame=%s]", + rclcpp::get_logger("UnknownInitialPoseTool"), "Setting twist: %.3f %.3f %.3f [frame=%s]", velocity_->getFloat(), 0.0, 0.0, fixed_frame.c_str()); // action - output_msg.action = dummy_perception_publisher::msg::Object::ADD; + output_msg.action = Object::ADD; // id std::mt19937 gen(std::random_device{}()); @@ -160,6 +172,108 @@ void UnknownInitialPoseTool::onPoseSet(double x, double y, double theta) dummy_object_info_pub_->publish(output_msg); } +void UnknownInitialPoseTool::publishObjectMsg( + const std::array & uuid, const uint32_t action) +{ + Object output_msg; + std::string fixed_frame = context_->getFixedFrame().toStdString(); + + // header + output_msg.header.frame_id = fixed_frame; + output_msg.header.stamp = clock_->now(); + + // action + output_msg.action = action; + + // id + output_msg.id.uuid = uuid; + + if (action == Object::DELETE) { + dummy_object_info_pub_->publish(output_msg); + return; + } + + const auto object_tf = objects_.transform(uuid); + const auto object_twist = objects_.twist(uuid); + + if (!object_tf || !object_twist) { + return; + } + + // semantic + output_msg.classification.label = ObjectClassification::UNKNOWN; + output_msg.classification.probability = 1.0; + + // shape + output_msg.shape.type = Shape::POLYGON; + const double width = 0.8; + const double length = 0.8; + output_msg.shape.dimensions.x = length; + output_msg.shape.dimensions.y = width; + output_msg.shape.dimensions.z = 2.0; + + // initial state + // pose + tf2::toMsg(object_tf.get(), output_msg.initial_state.pose_covariance.pose); + output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[0] = + std_dev_x_->getFloat() * std_dev_x_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[7] = + std_dev_y_->getFloat() * std_dev_y_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[14] = + std_dev_z_->getFloat() * std_dev_z_->getFloat(); + output_msg.initial_state.pose_covariance.covariance[35] = + std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); + // twist + output_msg.initial_state.twist_covariance.twist = object_twist.get(); + + dummy_object_info_pub_->publish(output_msg); +} + +int UnknownInitialPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (!enable_interactive_property_->getBool()) { + return PoseTool::processMouseEvent(event); + } + + if (event.rightDown()) { + const auto point = get_point_from_mouse(event); + if (point) { + if (event.shift()) { + const auto uuid = objects_.create(point.value()); + publishObjectMsg(uuid.get(), Object::ADD); + } else if (event.alt()) { + const auto uuid = objects_.remove(point.value()); + publishObjectMsg(uuid.get(), Object::DELETE); + } else { + objects_.select(point.value()); + } + } + return 0; + } + + if (event.rightUp()) { + objects_.reset(); + return 0; + } + + if (event.right()) { + const auto point = get_point_from_mouse(event); + if (point) { + const auto uuid = objects_.update(point.value()); + publishObjectMsg(uuid.get(), Object::MODIFY); + } + return 0; + } + + return move_tool_.processMouseEvent(event); +} + +int UnknownInitialPoseTool::processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) +{ + PoseTool::processKeyEvent(event, panel); + return move_tool_.processKeyEvent(event, panel); +} } // end namespace rviz_plugins #include diff --git a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp index bdc3d40becbe9..306d29b51ad27 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp @@ -43,11 +43,17 @@ #ifndef TOOLS__UNKNOWN_POSE_HPP_ #define TOOLS__UNKNOWN_POSE_HPP_ +#include "interactive_object.hpp" + #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 #include #include +#include #include #include +#include +#include +#include #include #endif @@ -55,6 +61,11 @@ namespace rviz_plugins { + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::Shape; +using dummy_perception_publisher::msg::Object; + class UnknownInitialPoseTool : public rviz_default_plugins::tools::PoseTool { Q_OBJECT @@ -63,6 +74,8 @@ class UnknownInitialPoseTool : public rviz_default_plugins::tools::PoseTool UnknownInitialPoseTool(); virtual ~UnknownInitialPoseTool() {} virtual void onInitialize(); + int processMouseEvent(rviz_common::ViewportMouseEvent & event) override; + int processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) override; protected: virtual void onPoseSet(double x, double y, double theta); @@ -72,8 +85,11 @@ private Q_SLOTS: private: rclcpp::Clock::SharedPtr clock_; - rclcpp::Publisher::SharedPtr dummy_object_info_pub_; + rclcpp::Publisher::SharedPtr dummy_object_info_pub_; + rviz_default_plugins::tools::MoveTool move_tool_; + + rviz_common::properties::BoolProperty * enable_interactive_property_; rviz_common::properties::StringProperty * topic_property_; rviz_common::properties::FloatProperty * std_dev_x_; rviz_common::properties::FloatProperty * std_dev_y_; @@ -81,6 +97,11 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * std_dev_theta_; rviz_common::properties::FloatProperty * position_z_; rviz_common::properties::FloatProperty * velocity_; + rviz_common::properties::TfFrameProperty * property_frame_; + + InteractiveObjectCollection objects_; + + void publishObjectMsg(const std::array & uuid, const uint32_t action); }; } // namespace rviz_plugins From d1e2eefd18395b7b919a1c63aef8d40d1445dbf0 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 22 Mar 2022 12:51:58 +0900 Subject: [PATCH 5/5] refactor(tier4_perception_rviz_plugin): use common class Signed-off-by: satoshi-ota --- .../src/tools/car_pose.cpp | 175 ++--------------- .../src/tools/car_pose.hpp | 47 +---- .../src/tools/interactive_object.cpp | 110 +++++++++++ .../src/tools/interactive_object.hpp | 44 +++++ .../src/tools/pedestrian_pose.cpp | 171 ++--------------- .../src/tools/pedestrian_pose.hpp | 59 +----- .../src/tools/unknown_pose.cpp | 177 ++---------------- .../src/tools/unknown_pose.hpp | 50 +---- 8 files changed, 215 insertions(+), 618 deletions(-) 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 426fc044f2e0f..9c16651863487 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp @@ -49,10 +49,7 @@ #include "util.hpp" -#include - -#include -#include +#include #include #include @@ -101,179 +98,43 @@ void CarInitialPoseTool::onInitialize() updateTopic(); } -void CarInitialPoseTool::updateTopic() +Object CarInitialPoseTool::createObjectMsg() const { - rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - dummy_object_info_pub_ = raw_node->create_publisher(topic_property_->getStdString(), 1); - clock_ = raw_node->get_clock(); - move_tool_.initialize(context_); - property_frame_->setFrameManager(context_->getFrameManager()); -} - -void CarInitialPoseTool::onPoseSet(double x, double y, double theta) -{ - if (enable_interactive_property_->getBool()) { - return; - } - - Object output_msg; + Object object{}; std::string fixed_frame = context_->getFixedFrame().toStdString(); // header - output_msg.header.frame_id = fixed_frame; - output_msg.header.stamp = clock_->now(); + object.header.frame_id = fixed_frame; + object.header.stamp = clock_->now(); // semantic - output_msg.classification.label = ObjectClassification::CAR; - output_msg.classification.probability = 1.0; + object.classification.label = ObjectClassification::CAR; + object.classification.probability = 1.0; // shape - output_msg.shape.type = Shape::BOUNDING_BOX; + object.shape.type = Shape::BOUNDING_BOX; const double width = 1.8; const double length = 4.0; - output_msg.shape.dimensions.x = length; - output_msg.shape.dimensions.y = width; - output_msg.shape.dimensions.z = 2.0; + object.shape.dimensions.x = length; + object.shape.dimensions.y = width; + object.shape.dimensions.z = 2.0; // initial state - // pose - output_msg.initial_state.pose_covariance.pose.position.x = x; - output_msg.initial_state.pose_covariance.pose.position.y = y; - output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[0] = + 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(); - output_msg.initial_state.pose_covariance.covariance[7] = + object.initial_state.pose_covariance.covariance[7] = std_dev_y_->getFloat() * std_dev_y_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[14] = + object.initial_state.pose_covariance.covariance[14] = std_dev_z_->getFloat() * std_dev_z_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[35] = + object.initial_state.pose_covariance.covariance[35] = std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, theta); - output_msg.initial_state.pose_covariance.pose.orientation = tf2::toMsg(quat); - RCLCPP_INFO( - rclcpp::get_logger("CarInitialPoseTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, y, - position_z_->getFloat(), theta, fixed_frame.c_str()); - // twist - output_msg.initial_state.twist_covariance.twist.linear.x = velocity_->getFloat(); - output_msg.initial_state.twist_covariance.twist.linear.y = 0.0; - output_msg.initial_state.twist_covariance.twist.linear.z = 0.0; - RCLCPP_INFO( - rclcpp::get_logger("CarInitialPoseTool"), "Setting twist: %.3f %.3f %.3f [frame=%s]", - velocity_->getFloat(), 0.0, 0.0, fixed_frame.c_str()); - // action - output_msg.action = Object::ADD; - - // id std::mt19937 gen(std::random_device{}()); std::independent_bits_engine bit_eng(gen); - std::generate(output_msg.id.uuid.begin(), output_msg.id.uuid.end(), bit_eng); - - dummy_object_info_pub_->publish(output_msg); -} - -void CarInitialPoseTool::publishObjectMsg( - const std::array & uuid, const uint32_t action) -{ - Object output_msg; - std::string fixed_frame = context_->getFixedFrame().toStdString(); - - // header - output_msg.header.frame_id = fixed_frame; - output_msg.header.stamp = clock_->now(); - - // action - output_msg.action = action; - - // id - output_msg.id.uuid = uuid; - - if (action == Object::DELETE) { - dummy_object_info_pub_->publish(output_msg); - return; - } - - const auto object_tf = objects_.transform(uuid); - const auto object_twist = objects_.twist(uuid); - - if (!object_tf || !object_twist) { - return; - } - - // semantic - output_msg.classification.label = ObjectClassification::CAR; - output_msg.classification.probability = 1.0; + std::generate(object.id.uuid.begin(), object.id.uuid.end(), bit_eng); - // shape - output_msg.shape.type = Shape::BOUNDING_BOX; - const double width = 1.8; - const double length = 4.0; - output_msg.shape.dimensions.x = length; - output_msg.shape.dimensions.y = width; - output_msg.shape.dimensions.z = 2.0; - - // initial state - // pose - tf2::toMsg(object_tf.get(), output_msg.initial_state.pose_covariance.pose); - output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[0] = - std_dev_x_->getFloat() * std_dev_x_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[7] = - std_dev_y_->getFloat() * std_dev_y_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[14] = - std_dev_z_->getFloat() * std_dev_z_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[35] = - std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); - // twist - output_msg.initial_state.twist_covariance.twist = object_twist.get(); - - dummy_object_info_pub_->publish(output_msg); -} - -int CarInitialPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) -{ - if (!enable_interactive_property_->getBool()) { - return PoseTool::processMouseEvent(event); - } - - if (event.rightDown()) { - const auto point = get_point_from_mouse(event); - if (point) { - if (event.shift()) { - const auto uuid = objects_.create(point.value()); - publishObjectMsg(uuid.get(), Object::ADD); - } else if (event.alt()) { - const auto uuid = objects_.remove(point.value()); - publishObjectMsg(uuid.get(), Object::DELETE); - } else { - objects_.select(point.value()); - } - } - return 0; - } - - if (event.rightUp()) { - objects_.reset(); - return 0; - } - - if (event.right()) { - const auto point = get_point_from_mouse(event); - if (point) { - const auto uuid = objects_.update(point.value()); - publishObjectMsg(uuid.get(), Object::MODIFY); - } - return 0; - } - - return move_tool_.processMouseEvent(event); -} - -int CarInitialPoseTool::processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) -{ - PoseTool::processKeyEvent(event, panel); - return move_tool_.processKeyEvent(event, panel); + return object; } } // end namespace rviz_plugins 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 c4c6d32b4b82d..6dfb06ee87bd1 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp @@ -50,17 +50,6 @@ #include "interactive_object.hpp" -#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 -#include -#include -#include -#include -#include -#include -#endif - -#include - namespace rviz_plugins { @@ -68,42 +57,12 @@ using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; -class CarInitialPoseTool : public rviz_default_plugins::tools::PoseTool +class CarInitialPoseTool : public InteractiveObjectTool { - Q_OBJECT - public: CarInitialPoseTool(); - virtual ~CarInitialPoseTool() {} - virtual void onInitialize(); - int processMouseEvent(rviz_common::ViewportMouseEvent & event) override; - int processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) override; - -protected: - virtual void onPoseSet(double x, double y, double theta); - -private Q_SLOTS: - void updateTopic(); - -private: - rclcpp::Clock::SharedPtr clock_; - rclcpp::Publisher::SharedPtr dummy_object_info_pub_; - - rviz_default_plugins::tools::MoveTool move_tool_; - - rviz_common::properties::BoolProperty * enable_interactive_property_; - rviz_common::properties::StringProperty * topic_property_; - 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 * std_dev_theta_; - rviz_common::properties::FloatProperty * position_z_; - rviz_common::properties::FloatProperty * velocity_; - rviz_common::properties::TfFrameProperty * property_frame_; - - InteractiveObjectCollection objects_; - - void publishObjectMsg(const std::array & uuid, const uint32_t action); + void onInitialize(); + Object createObjectMsg() const override; }; } // namespace rviz_plugins diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp index 4f951928ea051..351a804d01c8a 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp @@ -49,6 +49,8 @@ #include "util.hpp" +#include + #include #include #include @@ -215,4 +217,112 @@ size_t InteractiveObjectCollection::nearest(const Ogre::Vector3 & point) const size_t index = indices[0]; return distances[index] < 2.0 ? index : npos; } + +void InteractiveObjectTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D Dummy Object"); + updateTopic(); +} + +void InteractiveObjectTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + dummy_object_info_pub_ = raw_node->create_publisher(topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); + move_tool_.initialize(context_); + property_frame_->setFrameManager(context_->getFrameManager()); +} + +void InteractiveObjectTool::onPoseSet(double x, double y, double theta) +{ + if (enable_interactive_property_->getBool()) { + return; + } + + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + + auto output_msg = createObjectMsg(); + output_msg.initial_state.pose_covariance.pose.position.x = x; + output_msg.initial_state.pose_covariance.pose.position.y = y; + output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); + output_msg.initial_state.pose_covariance.pose.orientation = tf2::toMsg(quat); + output_msg.initial_state.twist_covariance.twist.linear.x = velocity_->getFloat(); + output_msg.initial_state.twist_covariance.twist.linear.y = 0.0; + output_msg.initial_state.twist_covariance.twist.linear.z = 0.0; + output_msg.action = Object::ADD; + + dummy_object_info_pub_->publish(output_msg); +} + +void InteractiveObjectTool::publishObjectMsg( + const std::array & uuid, const uint32_t action) +{ + auto output_msg = createObjectMsg(); + output_msg.action = action; + output_msg.id.uuid = uuid; + + if (action == Object::DELETE) { + dummy_object_info_pub_->publish(output_msg); + return; + } + + const auto object_tf = objects_.transform(uuid); + const auto object_twist = objects_.twist(uuid); + + if (!object_tf || !object_twist) { + return; + } + + tf2::toMsg(object_tf.get(), output_msg.initial_state.pose_covariance.pose); + output_msg.initial_state.twist_covariance.twist = object_twist.get(); + + dummy_object_info_pub_->publish(output_msg); +} + +int InteractiveObjectTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (!enable_interactive_property_->getBool()) { + return PoseTool::processMouseEvent(event); + } + + if (event.rightDown()) { + const auto point = get_point_from_mouse(event); + if (point) { + if (event.shift()) { + const auto uuid = objects_.create(point.value()); + publishObjectMsg(uuid.get(), Object::ADD); + } else if (event.alt()) { + const auto uuid = objects_.remove(point.value()); + publishObjectMsg(uuid.get(), Object::DELETE); + } else { + objects_.select(point.value()); + } + } + return 0; + } + + if (event.rightUp()) { + objects_.reset(); + return 0; + } + + if (event.right()) { + const auto point = get_point_from_mouse(event); + if (point) { + const auto uuid = objects_.update(point.value()); + publishObjectMsg(uuid.get(), Object::MODIFY); + } + return 0; + } + + return move_tool_.processMouseEvent(event); +} + +int InteractiveObjectTool::processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) +{ + PoseTool::processKeyEvent(event, panel); + return move_tool_.processKeyEvent(event, panel); +} } // end namespace rviz_plugins 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 a669028c802be..d33a7ea8b4e2c 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp @@ -60,6 +60,8 @@ #include #endif +#include + #include #include @@ -74,6 +76,10 @@ namespace rviz_plugins { +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::Shape; +using dummy_perception_publisher::msg::Object; + class InteractiveObject { public: @@ -112,5 +118,43 @@ class InteractiveObjectCollection InteractiveObject * target_; std::vector> objects_; }; + +class InteractiveObjectTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + virtual ~InteractiveObjectTool() = default; + virtual void onInitialize(); + int processMouseEvent(rviz_common::ViewportMouseEvent & event) override; + int processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) override; + + virtual Object createObjectMsg() const = 0; + +protected Q_SLOTS: + virtual void updateTopic(); + +protected: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr dummy_object_info_pub_; + + rviz_default_plugins::tools::MoveTool move_tool_; + + rviz_common::properties::BoolProperty * enable_interactive_property_; + rviz_common::properties::StringProperty * topic_property_; + 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 * std_dev_theta_; + rviz_common::properties::FloatProperty * position_z_; + rviz_common::properties::FloatProperty * velocity_; + rviz_common::properties::TfFrameProperty * property_frame_; + +private: + void publishObjectMsg(const std::array & uuid, const uint32_t action); + void onPoseSet(double x, double y, double theta); + + InteractiveObjectCollection objects_; +}; } // namespace rviz_plugins #endif // TOOLS__INTERACTIVE_OBJECT_HPP_ diff --git a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp index d19166174d335..a6e0d34c3aa37 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp @@ -52,7 +52,6 @@ #include #include -#include #include #include @@ -99,179 +98,43 @@ void PedestrianInitialPoseTool::onInitialize() updateTopic(); } -void PedestrianInitialPoseTool::updateTopic() +Object PedestrianInitialPoseTool::createObjectMsg() const { - rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - dummy_object_info_pub_ = raw_node->create_publisher(topic_property_->getStdString(), 1); - clock_ = raw_node->get_clock(); - move_tool_.initialize(context_); - property_frame_->setFrameManager(context_->getFrameManager()); -} - -void PedestrianInitialPoseTool::onPoseSet(double x, double y, double theta) -{ - if (enable_interactive_property_->getBool()) { - return; - } - - Object output_msg; + Object object{}; std::string fixed_frame = context_->getFixedFrame().toStdString(); // header - output_msg.header.frame_id = fixed_frame; - output_msg.header.stamp = clock_->now(); + object.header.frame_id = fixed_frame; + object.header.stamp = clock_->now(); // semantic - output_msg.classification.label = ObjectClassification::PEDESTRIAN; - output_msg.classification.probability = 1.0; + object.classification.label = ObjectClassification::PEDESTRIAN; + object.classification.probability = 1.0; // shape - output_msg.shape.type = Shape::CYLINDER; + object.shape.type = Shape::CYLINDER; const double width = 0.8; const double length = 0.8; - output_msg.shape.dimensions.x = length; - output_msg.shape.dimensions.y = width; - output_msg.shape.dimensions.z = 2.0; + object.shape.dimensions.x = length; + object.shape.dimensions.y = width; + object.shape.dimensions.z = 2.0; // initial state - // pose - output_msg.initial_state.pose_covariance.pose.position.x = x; - output_msg.initial_state.pose_covariance.pose.position.y = y; - output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[0] = + 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(); - output_msg.initial_state.pose_covariance.covariance[7] = + object.initial_state.pose_covariance.covariance[7] = std_dev_y_->getFloat() * std_dev_y_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[14] = + object.initial_state.pose_covariance.covariance[14] = std_dev_z_->getFloat() * std_dev_z_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[35] = + object.initial_state.pose_covariance.covariance[35] = std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, theta); - output_msg.initial_state.pose_covariance.pose.orientation = tf2::toMsg(quat); - RCLCPP_INFO( - rclcpp::get_logger("PedestrianInitialPoseTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", - x, y, position_z_->getFloat(), theta, fixed_frame.c_str()); - // twist - output_msg.initial_state.twist_covariance.twist.linear.x = velocity_->getFloat(); - output_msg.initial_state.twist_covariance.twist.linear.y = 0.0; - output_msg.initial_state.twist_covariance.twist.linear.z = 0.0; - RCLCPP_INFO( - rclcpp::get_logger("PedestrianInitialPoseTool"), "Setting twist: %.3f %.3f %.3f [frame=%s]", - velocity_->getFloat(), 0.0, 0.0, fixed_frame.c_str()); - - // action - output_msg.action = Object::ADD; - // id std::mt19937 gen(std::random_device{}()); std::independent_bits_engine bit_eng(gen); - std::generate(output_msg.id.uuid.begin(), output_msg.id.uuid.end(), bit_eng); - - dummy_object_info_pub_->publish(output_msg); -} - -void PedestrianInitialPoseTool::publishObjectMsg( - const std::array & uuid, const uint32_t action) -{ - Object output_msg; - std::string fixed_frame = context_->getFixedFrame().toStdString(); + std::generate(object.id.uuid.begin(), object.id.uuid.end(), bit_eng); - // header - output_msg.header.frame_id = fixed_frame; - output_msg.header.stamp = clock_->now(); - - // action - output_msg.action = action; - - // id - output_msg.id.uuid = uuid; - - if (action == Object::DELETE) { - dummy_object_info_pub_->publish(output_msg); - return; - } - - const auto object_tf = objects_.transform(uuid); - const auto object_twist = objects_.twist(uuid); - - if (!object_tf || !object_twist) { - return; - } - - // semantic - output_msg.classification.label = ObjectClassification::PEDESTRIAN; - output_msg.classification.probability = 1.0; - - // shape - output_msg.shape.type = Shape::CYLINDER; - const double width = 0.8; - const double length = 0.8; - output_msg.shape.dimensions.x = length; - output_msg.shape.dimensions.y = width; - output_msg.shape.dimensions.z = 2.0; - - // initial state - // pose - tf2::toMsg(object_tf.get(), output_msg.initial_state.pose_covariance.pose); - output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[0] = - std_dev_x_->getFloat() * std_dev_x_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[7] = - std_dev_y_->getFloat() * std_dev_y_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[14] = - std_dev_z_->getFloat() * std_dev_z_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[35] = - std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); - // twist - output_msg.initial_state.twist_covariance.twist = object_twist.get(); - - dummy_object_info_pub_->publish(output_msg); -} - -int PedestrianInitialPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) -{ - if (!enable_interactive_property_->getBool()) { - return PoseTool::processMouseEvent(event); - } - - if (event.rightDown()) { - const auto point = get_point_from_mouse(event); - if (point) { - if (event.shift()) { - const auto uuid = objects_.create(point.value()); - publishObjectMsg(uuid.get(), Object::ADD); - } else if (event.alt()) { - const auto uuid = objects_.remove(point.value()); - publishObjectMsg(uuid.get(), Object::DELETE); - } else { - objects_.select(point.value()); - } - } - return 0; - } - - if (event.rightUp()) { - objects_.reset(); - return 0; - } - - if (event.right()) { - const auto point = get_point_from_mouse(event); - if (point) { - const auto uuid = objects_.update(point.value()); - publishObjectMsg(uuid.get(), Object::MODIFY); - } - return 0; - } - - return move_tool_.processMouseEvent(event); -} - -int PedestrianInitialPoseTool::processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) -{ - PoseTool::processKeyEvent(event, panel); - return move_tool_.processKeyEvent(event, panel); + return object; } } // end namespace rviz_plugins diff --git a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp index 61339c1e4d175..b19aacc91c2c0 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp @@ -50,29 +50,6 @@ #include "interactive_object.hpp" -#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#endif - -#include - -#include - -#include -#include - -#include -#include -#include - namespace rviz_plugins { @@ -80,42 +57,12 @@ using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; -class PedestrianInitialPoseTool : public rviz_default_plugins::tools::PoseTool +class PedestrianInitialPoseTool : public InteractiveObjectTool { - Q_OBJECT - public: PedestrianInitialPoseTool(); - virtual ~PedestrianInitialPoseTool() {} - virtual void onInitialize(); - int processMouseEvent(rviz_common::ViewportMouseEvent & event) override; - int processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) override; - -protected: - virtual void onPoseSet(double x, double y, double theta); - -private Q_SLOTS: - void updateTopic(); - -private: - rclcpp::Clock::SharedPtr clock_; - rclcpp::Publisher::SharedPtr dummy_object_info_pub_; - - rviz_default_plugins::tools::MoveTool move_tool_; - - rviz_common::properties::BoolProperty * enable_interactive_property_; - rviz_common::properties::StringProperty * topic_property_; - 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 * std_dev_theta_; - rviz_common::properties::FloatProperty * position_z_; - rviz_common::properties::FloatProperty * velocity_; - rviz_common::properties::TfFrameProperty * property_frame_; - - InteractiveObjectCollection objects_; - - void publishObjectMsg(const std::array & uuid, const uint32_t action); + void onInitialize(); + Object createObjectMsg() const override; }; } // namespace rviz_plugins diff --git a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp index 9d8b207c7f8e1..5fa7ff773e77a 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp @@ -45,13 +45,6 @@ #include "util.hpp" #include -#include -#include - -#include - -#include -#include #include #include @@ -100,179 +93,43 @@ void UnknownInitialPoseTool::onInitialize() updateTopic(); } -void UnknownInitialPoseTool::updateTopic() -{ - rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); - dummy_object_info_pub_ = raw_node->create_publisher(topic_property_->getStdString(), 1); - clock_ = raw_node->get_clock(); - move_tool_.initialize(context_); - property_frame_->setFrameManager(context_->getFrameManager()); -} - -void UnknownInitialPoseTool::onPoseSet(double x, double y, double theta) +Object UnknownInitialPoseTool::createObjectMsg() const { - if (enable_interactive_property_->getBool()) { - return; - } - - Object output_msg; + Object object{}; std::string fixed_frame = context_->getFixedFrame().toStdString(); // header - output_msg.header.frame_id = fixed_frame; - output_msg.header.stamp = clock_->now(); + object.header.frame_id = fixed_frame; + object.header.stamp = clock_->now(); // semantic - output_msg.classification.label = ObjectClassification::UNKNOWN; - output_msg.classification.probability = 1.0; + object.classification.label = ObjectClassification::UNKNOWN; + object.classification.probability = 1.0; // shape - output_msg.shape.type = Shape::POLYGON; + object.shape.type = Shape::POLYGON; const double width = 0.8; const double length = 0.8; - output_msg.shape.dimensions.x = length; - output_msg.shape.dimensions.y = width; - output_msg.shape.dimensions.z = 2.0; + object.shape.dimensions.x = length; + object.shape.dimensions.y = width; + object.shape.dimensions.z = 2.0; // initial state - // pose - output_msg.initial_state.pose_covariance.pose.position.x = x; - output_msg.initial_state.pose_covariance.pose.position.y = y; - output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[0] = + 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(); - output_msg.initial_state.pose_covariance.covariance[7] = + object.initial_state.pose_covariance.covariance[7] = std_dev_y_->getFloat() * std_dev_y_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[14] = + object.initial_state.pose_covariance.covariance[14] = std_dev_z_->getFloat() * std_dev_z_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[35] = + object.initial_state.pose_covariance.covariance[35] = std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, theta); - output_msg.initial_state.pose_covariance.pose.orientation = tf2::toMsg(quat); - RCLCPP_INFO( - rclcpp::get_logger("UnknownInitialPoseTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, - y, position_z_->getFloat(), theta, fixed_frame.c_str()); - // twist - output_msg.initial_state.twist_covariance.twist.linear.x = velocity_->getFloat(); - output_msg.initial_state.twist_covariance.twist.linear.y = 0.0; - output_msg.initial_state.twist_covariance.twist.linear.z = 0.0; - RCLCPP_INFO( - rclcpp::get_logger("UnknownInitialPoseTool"), "Setting twist: %.3f %.3f %.3f [frame=%s]", - velocity_->getFloat(), 0.0, 0.0, fixed_frame.c_str()); - // action - output_msg.action = Object::ADD; - - // id std::mt19937 gen(std::random_device{}()); std::independent_bits_engine bit_eng(gen); - std::generate(output_msg.id.uuid.begin(), output_msg.id.uuid.end(), bit_eng); - - dummy_object_info_pub_->publish(output_msg); -} - -void UnknownInitialPoseTool::publishObjectMsg( - const std::array & uuid, const uint32_t action) -{ - Object output_msg; - std::string fixed_frame = context_->getFixedFrame().toStdString(); - - // header - output_msg.header.frame_id = fixed_frame; - output_msg.header.stamp = clock_->now(); - - // action - output_msg.action = action; - - // id - output_msg.id.uuid = uuid; - - if (action == Object::DELETE) { - dummy_object_info_pub_->publish(output_msg); - return; - } - - const auto object_tf = objects_.transform(uuid); - const auto object_twist = objects_.twist(uuid); - - if (!object_tf || !object_twist) { - return; - } - - // semantic - output_msg.classification.label = ObjectClassification::UNKNOWN; - output_msg.classification.probability = 1.0; + std::generate(object.id.uuid.begin(), object.id.uuid.end(), bit_eng); - // shape - output_msg.shape.type = Shape::POLYGON; - const double width = 0.8; - const double length = 0.8; - output_msg.shape.dimensions.x = length; - output_msg.shape.dimensions.y = width; - output_msg.shape.dimensions.z = 2.0; - - // initial state - // pose - tf2::toMsg(object_tf.get(), output_msg.initial_state.pose_covariance.pose); - output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[0] = - std_dev_x_->getFloat() * std_dev_x_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[7] = - std_dev_y_->getFloat() * std_dev_y_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[14] = - std_dev_z_->getFloat() * std_dev_z_->getFloat(); - output_msg.initial_state.pose_covariance.covariance[35] = - std_dev_theta_->getFloat() * std_dev_theta_->getFloat(); - // twist - output_msg.initial_state.twist_covariance.twist = object_twist.get(); - - dummy_object_info_pub_->publish(output_msg); -} - -int UnknownInitialPoseTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) -{ - if (!enable_interactive_property_->getBool()) { - return PoseTool::processMouseEvent(event); - } - - if (event.rightDown()) { - const auto point = get_point_from_mouse(event); - if (point) { - if (event.shift()) { - const auto uuid = objects_.create(point.value()); - publishObjectMsg(uuid.get(), Object::ADD); - } else if (event.alt()) { - const auto uuid = objects_.remove(point.value()); - publishObjectMsg(uuid.get(), Object::DELETE); - } else { - objects_.select(point.value()); - } - } - return 0; - } - - if (event.rightUp()) { - objects_.reset(); - return 0; - } - - if (event.right()) { - const auto point = get_point_from_mouse(event); - if (point) { - const auto uuid = objects_.update(point.value()); - publishObjectMsg(uuid.get(), Object::MODIFY); - } - return 0; - } - - return move_tool_.processMouseEvent(event); -} - -int UnknownInitialPoseTool::processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) -{ - PoseTool::processKeyEvent(event, panel); - return move_tool_.processKeyEvent(event, panel); + return object; } } // end namespace rviz_plugins diff --git a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp index 306d29b51ad27..04556805b3ff0 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp @@ -45,20 +45,6 @@ #include "interactive_object.hpp" -#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#endif - -#include - namespace rviz_plugins { @@ -66,42 +52,12 @@ using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::Shape; using dummy_perception_publisher::msg::Object; -class UnknownInitialPoseTool : public rviz_default_plugins::tools::PoseTool +class UnknownInitialPoseTool : public InteractiveObjectTool { - Q_OBJECT - public: UnknownInitialPoseTool(); - virtual ~UnknownInitialPoseTool() {} - virtual void onInitialize(); - int processMouseEvent(rviz_common::ViewportMouseEvent & event) override; - int processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) override; - -protected: - virtual void onPoseSet(double x, double y, double theta); - -private Q_SLOTS: - void updateTopic(); - -private: - rclcpp::Clock::SharedPtr clock_; - rclcpp::Publisher::SharedPtr dummy_object_info_pub_; - - rviz_default_plugins::tools::MoveTool move_tool_; - - rviz_common::properties::BoolProperty * enable_interactive_property_; - rviz_common::properties::StringProperty * topic_property_; - 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 * std_dev_theta_; - rviz_common::properties::FloatProperty * position_z_; - rviz_common::properties::FloatProperty * velocity_; - rviz_common::properties::TfFrameProperty * property_frame_; - - InteractiveObjectCollection objects_; - - void publishObjectMsg(const std::array & uuid, const uint32_t action); + void onInitialize(); + Object createObjectMsg() const override; }; } // namespace rviz_plugins