forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 31
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #244 from tier4/sync-upstream
chore: sync upstream
- Loading branch information
Showing
93 changed files
with
3,344 additions
and
437 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Binary file added
BIN
+18.4 KB
common/tier4_planning_rviz_plugin/icons/classes/PoseWithUuidStamped.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
84 changes: 84 additions & 0 deletions
84
common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,84 @@ | ||
| // Copyright 2023 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. | ||
|
|
||
| #ifndef POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ | ||
| #define POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ | ||
|
|
||
| #include <rviz_common/message_filter_display.hpp> | ||
|
|
||
| #include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp> | ||
|
|
||
| #include <deque> | ||
| #include <memory> | ||
| #include <string> | ||
|
|
||
| namespace rviz_rendering | ||
| { | ||
| class Axes; | ||
| class MovableText; | ||
| } // namespace rviz_rendering | ||
| namespace rviz_common::properties | ||
| { | ||
| class FloatProperty; | ||
| class TfFrameProperty; | ||
| } // namespace rviz_common::properties | ||
|
|
||
| namespace rviz_plugins | ||
| { | ||
| class AutowarePoseWithUuidStampedDisplay | ||
| : public rviz_common::MessageFilterDisplay<autoware_planning_msgs::msg::PoseWithUuidStamped> | ||
| { | ||
| Q_OBJECT | ||
|
|
||
| public: | ||
| AutowarePoseWithUuidStampedDisplay(); | ||
| ~AutowarePoseWithUuidStampedDisplay() override; | ||
| AutowarePoseWithUuidStampedDisplay(const AutowarePoseWithUuidStampedDisplay &) = delete; | ||
| AutowarePoseWithUuidStampedDisplay(const AutowarePoseWithUuidStampedDisplay &&) = delete; | ||
| AutowarePoseWithUuidStampedDisplay & operator=(const AutowarePoseWithUuidStampedDisplay &) = | ||
| delete; | ||
| AutowarePoseWithUuidStampedDisplay & operator=(const AutowarePoseWithUuidStampedDisplay &&) = | ||
| delete; | ||
|
|
||
| protected: | ||
| void onInitialize() override; | ||
| void onEnable() override; | ||
| void onDisable() override; | ||
|
|
||
| private Q_SLOTS: | ||
| void updateVisualization(); | ||
|
|
||
| private: | ||
| void subscribe() override; | ||
| void unsubscribe() override; | ||
| void processMessage( | ||
| const autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr meg_ptr) override; | ||
|
|
||
| std::unique_ptr<rviz_rendering::Axes> axes_; | ||
| std::unique_ptr<Ogre::SceneNode> uuid_node_; | ||
| rviz_rendering::MovableText * uuid_; | ||
|
|
||
| rviz_common::properties::FloatProperty * length_property_; | ||
| rviz_common::properties::FloatProperty * radius_property_; | ||
| rviz_common::properties::TfFrameProperty * frame_property_; | ||
|
|
||
| rviz_common::properties::BoolProperty * uuid_text_view_property_; | ||
| rviz_common::properties::FloatProperty * uuid_text_scale_property_; | ||
|
|
||
| autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr last_msg_ptr_; | ||
| }; | ||
|
|
||
| } // namespace rviz_plugins | ||
|
|
||
| #endif // POSE_WITH_UUID_STAMPED__DISPLAY_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
146 changes: 146 additions & 0 deletions
146
common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,146 @@ | ||
| // Copyright 2023 TIER IV, Inc. All rights reserved. | ||
| // | ||
| // 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. | ||
|
|
||
| #include "rviz_common/properties/tf_frame_property.hpp" | ||
|
|
||
| #include <pose_with_uuid_stamped/display.hpp> | ||
| #include <rviz_common/properties/float_property.hpp> | ||
| #include <rviz_common/validate_floats.hpp> | ||
| #include <rviz_rendering/objects/axes.hpp> | ||
| #include <rviz_rendering/objects/movable_text.hpp> | ||
|
|
||
| namespace | ||
| { | ||
| std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) | ||
| { | ||
| std::stringstream ss; | ||
| for (auto i = 0; i < 16; ++i) { | ||
| ss << std::hex << std::setfill('0') << std::setw(2) << +u.uuid[i]; | ||
| } | ||
| return ss.str(); | ||
| } | ||
| } // namespace | ||
|
|
||
| namespace rviz_plugins | ||
| { | ||
| AutowarePoseWithUuidStampedDisplay::AutowarePoseWithUuidStampedDisplay() | ||
| { | ||
| length_property_ = new rviz_common::properties::FloatProperty( | ||
| "Length", 1.5f, "Length of each axis, in meters.", this, SLOT(updateVisualization())); | ||
| length_property_->setMin(0.0001f); | ||
|
|
||
| radius_property_ = new rviz_common::properties::FloatProperty( | ||
| "Radius", 0.5f, "Radius of each axis, in meters.", this, SLOT(updateVisualization())); | ||
| radius_property_->setMin(0.0001f); | ||
|
|
||
| uuid_text_view_property_ = new rviz_common::properties::BoolProperty( | ||
| "UUID", false, "flag of visualizing uuid text", this, SLOT(updateVisualization()), this); | ||
| uuid_text_scale_property_ = new rviz_common::properties::FloatProperty( | ||
| "Scale", 0.3f, "Scale of uuid text", uuid_text_view_property_, SLOT(updateVisualization()), | ||
| this); | ||
| } | ||
|
|
||
| AutowarePoseWithUuidStampedDisplay::~AutowarePoseWithUuidStampedDisplay() = default; | ||
|
|
||
| void AutowarePoseWithUuidStampedDisplay::onInitialize() | ||
| { | ||
| MFDClass::onInitialize(); | ||
| axes_ = std::make_unique<rviz_rendering::Axes>( | ||
| scene_manager_, scene_node_, length_property_->getFloat(), radius_property_->getFloat()); | ||
| axes_->getSceneNode()->setVisible(isEnabled()); | ||
|
|
||
| uuid_node_.reset(scene_node_->createChildSceneNode()); | ||
| uuid_ = new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); | ||
| } | ||
|
|
||
| void AutowarePoseWithUuidStampedDisplay::onEnable() | ||
| { | ||
| subscribe(); | ||
| axes_->getSceneNode()->setVisible(true); | ||
| } | ||
|
|
||
| void AutowarePoseWithUuidStampedDisplay::onDisable() | ||
| { | ||
| unsubscribe(); | ||
| axes_->getSceneNode()->setVisible(false); | ||
| } | ||
|
|
||
| void AutowarePoseWithUuidStampedDisplay::subscribe() { MFDClass::subscribe(); } | ||
|
|
||
| void AutowarePoseWithUuidStampedDisplay::unsubscribe() { MFDClass::unsubscribe(); } | ||
|
|
||
| void AutowarePoseWithUuidStampedDisplay::updateVisualization() | ||
| { | ||
| if (last_msg_ptr_ != nullptr) { | ||
| processMessage(last_msg_ptr_); | ||
| } | ||
| } | ||
|
|
||
| void AutowarePoseWithUuidStampedDisplay::processMessage( | ||
| const autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr msg_ptr) | ||
| { | ||
| uuid_node_->detachAllObjects(); | ||
|
|
||
| { | ||
| Ogre::Vector3 position; | ||
| Ogre::Quaternion orientation; | ||
|
|
||
| if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { | ||
| const auto frame = msg_ptr->header.frame_id.c_str(); | ||
| RCLCPP_DEBUG( | ||
| rclcpp::get_logger("AutowarePoseWithUuidStampedDisplay"), | ||
| "Error transforming from frame '%s' to frame '%s'", frame, qPrintable(fixed_frame_)); | ||
| axes_->getSceneNode()->setVisible(false); | ||
| uuid_->setVisible(false); | ||
| setMissingTransformToFixedFrame(frame); | ||
| } else { | ||
| setTransformOk(); | ||
| axes_->getSceneNode()->setVisible(true); | ||
| uuid_->setVisible(true); | ||
| scene_node_->setPosition(position); | ||
| scene_node_->setOrientation(orientation); | ||
| } | ||
| } | ||
|
|
||
| { | ||
| Ogre::Vector3 position; | ||
| position.x = msg_ptr->pose.position.x; | ||
| position.y = msg_ptr->pose.position.y; | ||
| position.z = msg_ptr->pose.position.z; | ||
|
|
||
| Ogre::Quaternion orientation; | ||
| orientation.x = msg_ptr->pose.orientation.x; | ||
| orientation.y = msg_ptr->pose.orientation.y; | ||
| orientation.z = msg_ptr->pose.orientation.z; | ||
| orientation.w = msg_ptr->pose.orientation.w; | ||
| axes_->setPosition(position); | ||
| axes_->setOrientation(orientation); | ||
| axes_->set(length_property_->getFloat(), radius_property_->getFloat()); | ||
|
|
||
| if (uuid_text_view_property_->getBool()) { | ||
| uuid_node_->setPosition(position); | ||
| uuid_->setTextAlignment( | ||
| rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); | ||
| uuid_->setCaption(uuid_to_string(msg_ptr->uuid)); | ||
| uuid_->setCharacterHeight(uuid_text_scale_property_->getFloat()); | ||
| uuid_->setVisible(true); | ||
| uuid_node_->attachObject(uuid_); | ||
| } | ||
| } | ||
| last_msg_ptr_ = msg_ptr; | ||
| } | ||
| } // namespace rviz_plugins | ||
|
|
||
| #include <pluginlib/class_list_macros.hpp> | ||
| PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePoseWithUuidStampedDisplay, rviz_common::Display) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.