diff --git a/common/tier4_camera_view_rviz_plugin/CMakeLists.txt b/common/tier4_camera_view_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..8ae8efef1e4b9 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_camera_view_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wno-unused-parameter) +endif() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/third_person_view_controller.cpp + src/third_person_view_tool.cpp + src/bird_eye_view_tool.cpp + src/bird_eye_view_controller.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package(INSTALL_TO_SHARE icons) diff --git a/common/tier4_camera_view_rviz_plugin/README.md b/common/tier4_camera_view_rviz_plugin/README.md new file mode 100644 index 0000000000000..99480d5a92e1c --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/README.md @@ -0,0 +1,9 @@ +# tier4_camera_view_rviz_plugin + +## ThirdPersonView Tool + +Add the `tier4_camera_view_rviz_plugin/ThirdPersonViewTool` tool to the RViz. Push the button, the camera will focus on the vehicle and set the target frame to `base_link`. Short cut key 'o'. + +## BirdEyeView Tool + +Add the `tier4_camera_view_rviz_plugin/BirdEyeViewTool` tool to the RViz. Push the button, the camera will turn to the BEV view, the target frame is consistent with the latest frame. Short cut key 'r'. diff --git a/common/tier4_camera_view_rviz_plugin/icons/classes/BirdEyeViewTool.png b/common/tier4_camera_view_rviz_plugin/icons/classes/BirdEyeViewTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_camera_view_rviz_plugin/icons/classes/BirdEyeViewTool.png differ diff --git a/common/tier4_camera_view_rviz_plugin/icons/classes/ThirdPersonViewTool.png b/common/tier4_camera_view_rviz_plugin/icons/classes/ThirdPersonViewTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_camera_view_rviz_plugin/icons/classes/ThirdPersonViewTool.png differ diff --git a/common/tier4_camera_view_rviz_plugin/package.xml b/common/tier4_camera_view_rviz_plugin/package.xml new file mode 100644 index 0000000000000..0ede5e37b8287 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/package.xml @@ -0,0 +1,31 @@ + + + + tier4_camera_view_rviz_plugin + 0.0.0 + The autoware camera view rviz plugin package + Yuxuan Liu + Makoto Yabuta + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_ad_api_specs + component_interface_utils + geometry_msgs + libqt5-core + libqt5-gui + libqt5-widgets + rclcpp + rviz_common + rviz_default_plugins + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_camera_view_rviz_plugin/plugins/plugin_description.xml b/common/tier4_camera_view_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..ab522b80de26a --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,24 @@ + + + + + Control the camera for bird-eye view. + + + + + Bird-eye-view Tool. This tool requires the corresponding BirdEyeViewController. + + + + + Control the camera for third-person view. + + + + + Third-person-view Tool. This tool requires the corresponding ThirdPersonViewController. + + + + diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp new file mode 100644 index 0000000000000..bd5c3349d3c35 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp @@ -0,0 +1,242 @@ +// Copyright 2023 Autoware Foundation +// +// 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. +/* + * Copyright (c) 2009, 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 the 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 "bird_eye_view_controller.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/viewport_mouse_event.hpp" +#include "rviz_rendering/objects/shape.hpp" +#include "rviz_rendering/orthographic.hpp" + +#include +#include +#include +#include +#include +#include + +namespace tier4_camera_view_rviz_plugin +{ +static const Ogre::Quaternion ROBOT_TO_CAMERA_ROTATION = + Ogre::Quaternion(Ogre::Radian(-Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Y) * + Ogre::Quaternion(Ogre::Radian(-Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Z); + +static const float PITCH_LIMIT_LOW = -Ogre::Math::HALF_PI + 0.001; +static const float PITCH_LIMIT_HIGH = Ogre::Math::HALF_PI - 0.001; + +BirdEyeViewController::BirdEyeViewController() : dragging_(false) +{ + scale_property_ = new rviz_common::properties::FloatProperty( + "Scale", 10, "How much to scale up the size of things in the scene.", this); + angle_property_ = new rviz_common::properties::FloatProperty( + "Angle", 0, "Angle around the Z axis to rotate.", this); + x_property_ = + new rviz_common::properties::FloatProperty("X", 0, "X component of camera position.", this); + y_property_ = + new rviz_common::properties::FloatProperty("Y", 0, "Y component of camera position.", this); +} + +BirdEyeViewController::~BirdEyeViewController() +{ +} + +void BirdEyeViewController::onInitialize() +{ + FramePositionTrackingViewController::onInitialize(); + + camera_->setProjectionType(Ogre::PT_ORTHOGRAPHIC); + auto camera_parent = getCameraParent(camera_); + camera_parent->setFixedYawAxis(false); + invert_z_->hide(); +} + +void BirdEyeViewController::reset() +{ + scale_property_->setFloat(10); + angle_property_->setFloat(0); + x_property_->setFloat(0); + y_property_->setFloat(0); +} + +void BirdEyeViewController::handleMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (event.shift()) { + setStatus("Left-Click: Move X/Y."); + } else { + setStatus( + "Left-Click: Rotate. Middle-Click: Move X/Y. Right-Click:: Zoom. " + "Shift: More options."); + } + + bool moved = false; + + int32_t diff_x = 0; + int32_t diff_y = 0; + + if (event.type == QEvent::MouseButtonPress) { + dragging_ = true; + } else if (event.type == QEvent::MouseButtonRelease) { + dragging_ = false; + } else if (dragging_ && event.type == QEvent::MouseMove) { + diff_x = event.x - event.last_x; + diff_y = event.y - event.last_y; + moved = true; + } + + if (event.left() && !event.shift()) { + setCursor(Rotate2D); + angle_property_->add(diff_x * 0.005); + orientCamera(); + } else if (event.middle() || (event.shift() && event.left())) { + setCursor(MoveXY); + float scale = scale_property_->getFloat(); + move_camera(-diff_x / scale, diff_y / scale); + } else if (event.right()) { + setCursor(Zoom); + scale_property_->multiply(1.0 - diff_y * 0.01); + } else { + setCursor(event.shift() ? MoveXY : Rotate2D); + } + + if (event.wheel_delta != 0) { + int diff = event.wheel_delta; + scale_property_->multiply(1.0 - (-diff) * 0.001); + + moved = true; + } + + if (moved) { + context_->queueRender(); + emitConfigChanged(); + } +} + +void BirdEyeViewController::orientCamera() +{ + camera_->setOrientation( + Ogre::Quaternion(Ogre::Radian(angle_property_->getFloat()), Ogre::Vector3::UNIT_Z)); +} + +void BirdEyeViewController::mimic(rviz_common::ViewController * source_view) +{ + FramePositionTrackingViewController::mimic(source_view); + + if (BirdEyeViewController * source_ortho = qobject_cast(source_view)) { + scale_property_->setFloat(source_ortho->scale_property_->getFloat()); + angle_property_->setFloat(source_ortho->angle_property_->getFloat()); + x_property_->setFloat(source_ortho->x_property_->getFloat()); + y_property_->setFloat(source_ortho->y_property_->getFloat()); + } else { + auto source_camera_parent = getCameraParent(source_view->getCamera()); + setPosition(source_camera_parent->getPosition()); + } +} +void BirdEyeViewController::update(float dt, float ros_dt) +{ + FramePositionTrackingViewController::update(dt, ros_dt); + updateCamera(); +} + +void BirdEyeViewController::lookAt(const Ogre::Vector3 & point) +{ + setPosition(point - target_scene_node_->getPosition()); +} + +void BirdEyeViewController::onTargetFrameChanged( + const Ogre::Vector3 & old_reference_position, + const Ogre::Quaternion & /*old_reference_orientation*/) +{ + move_camera( + old_reference_position.x - reference_position_.x, + old_reference_position.y - reference_position_.y); +} + +void BirdEyeViewController::updateCamera() +{ + orientCamera(); + + float width = camera_->getViewport()->getActualWidth(); + float height = camera_->getViewport()->getActualHeight(); + + float scale = scale_property_->getFloat(); + Ogre::Matrix4 proj = rviz_rendering::buildScaledOrthoMatrix( + -width / scale / 2, width / scale / 2, -height / scale / 2, height / scale / 2, + camera_->getNearClipDistance(), camera_->getFarClipDistance()); + camera_->setCustomProjectionMatrix(true, proj); + + // For Z, we use half of the far-clip distance set in + // selection_context.cpp, so that the shader program which computes + // depth can see equal distances above and below the Z=0 plane. + auto camera_parent = getCameraParent(camera_); + camera_parent->setPosition(x_property_->getFloat(), y_property_->getFloat(), 500); +} + +Ogre::SceneNode * BirdEyeViewController::getCameraParent(Ogre::Camera * camera) +{ + auto camera_parent = camera->getParentSceneNode(); + + if (!camera_parent) { + throw std::runtime_error("camera's parent scene node pointer unexpectedly nullptr"); + } + return camera_parent; +} + +void BirdEyeViewController::setPosition(const Ogre::Vector3 & pos_rel_target) +{ + x_property_->setFloat(pos_rel_target.x); + y_property_->setFloat(pos_rel_target.y); +} + +void BirdEyeViewController::move_camera(float dx, float dy) +{ + float angle = angle_property_->getFloat(); + x_property_->add(dx * cos(angle) - dy * sin(angle)); + y_property_->add(dx * sin(angle) + dy * cos(angle)); +} + +} // namespace tier4_camera_view_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS( + tier4_camera_view_rviz_plugin::BirdEyeViewController, rviz_common::ViewController) diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.hpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.hpp new file mode 100644 index 0000000000000..3c0091740bd59 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.hpp @@ -0,0 +1,112 @@ +// Copyright 2023 Autoware Foundation +// +// 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. +/* + * 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 the 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 BIRD_EYE_VIEW_CONTROLLER_HPP_ +#define BIRD_EYE_VIEW_CONTROLLER_HPP_ + +#include "rviz_common/frame_position_tracking_view_controller.hpp" + +#include +#include + +namespace rviz_common +{ +namespace properties +{ +class FloatProperty; +class Shape; +class VectorProperty; +} // namespace properties +} // namespace rviz_common + +namespace tier4_camera_view_rviz_plugin +{ +/** @brief A first-person camera, controlled by yaw, pitch, and position. */ +class BirdEyeViewController : public rviz_common::FramePositionTrackingViewController +{ + Q_OBJECT + +public: + BirdEyeViewController(); + + ~BirdEyeViewController() override; + + void onInitialize() override; + + void handleMouseEvent(rviz_common::ViewportMouseEvent & evt) override; + + void lookAt(const Ogre::Vector3 & point) override; + + void reset() override; + + /** @brief Configure the settings of this view controller to give, + * as much as possible, a similar view as that given by the + * @param source_view. + * + * @param source_view must return a valid @c Ogre::Camera* from getCamera(). */ + void mimic(rviz_common::ViewController * source_view) override; + + void update(float dt, float ros_dt) override; + +protected: + void onTargetFrameChanged( + const Ogre::Vector3 & old_reference_position, + const Ogre::Quaternion & old_reference_orientation) override; + + /** Set the camera orientation based on angle_. */ + void orientCamera(); + + void setPosition(const Ogre::Vector3 & pos_rel_target); + void move_camera(float x, float y); + void updateCamera(); + Ogre::SceneNode * getCameraParent(Ogre::Camera * camera); + + rviz_common::properties::FloatProperty * scale_property_; + rviz_common::properties::FloatProperty * angle_property_; + rviz_common::properties::FloatProperty * x_property_; + rviz_common::properties::FloatProperty * y_property_; + bool dragging_; +}; + +} // namespace tier4_camera_view_rviz_plugin + +#endif // BIRD_EYE_VIEW_CONTROLLER_HPP_ diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_tool.cpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_tool.cpp new file mode 100644 index 0000000000000..0c50f1ae1a0f4 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_tool.cpp @@ -0,0 +1,167 @@ +// Copyright 2023 Autoware Foundation +// +// 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. +/* + * UOS-ROS packages - Robot Operating System code by the University of Osnabrück + * Copyright (C) 2014 University of Osnabrück + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. 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. + * + * 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 HOLDER 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. + * + * route_set_tool.cpp + * + * Author: Henning Deeken {hdeeken@uos.de} + * + */ + +#include "bird_eye_view_tool.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/view_manager.hpp" + +#include + +namespace tier4_camera_view_rviz_plugin +{ +BirdEyeViewTool::BirdEyeViewTool() +{ + shortcut_key_ = 'r'; +} + +BirdEyeViewTool::~BirdEyeViewTool() +{ +} + +void BirdEyeViewTool::onInitialize() +{ + setName("Bird Eye View"); + + step_length_property_ = new rviz_common::properties::FloatProperty( + "Step Length", 0.1, "The length by with the position is updated on each step.", + getPropertyContainer(), SLOT(setOffset()), this); + + boost_property_ = new rviz_common::properties::FloatProperty( + "Boost Property", 0.5, "Gives the boost factor which is applied if pressing shift.", + getPropertyContainer(), SLOT(setBoost()), this); + + fly_property_ = new rviz_common::properties::BoolProperty( + "Fly Mode", false, "In fly mode it is possible to move along the z axis as well.", + getPropertyContainer(), SLOT(setFlyMode()), this); + + left_hand_property_ = new rviz_common::properties::BoolProperty( + "Left Hand Mode", false, "In left hand mode one uses the arrows to move around", + getPropertyContainer(), SLOT(setLeftHandMode()), this); + + fallback_view_controller_property_ = new rviz_common::properties::EnumProperty( + "Fallback ViewController", QString("tier4_camera_view_rviz_plugin/BirdEyeView"), + "Determines to which view controller the control switches, if the tool is deactivated.", + getPropertyContainer(), SLOT(setFallbackViewController()), this); + + m_pos_offset = 0.1; + m_boost = 0.5; + m_fly_mode = false; + m_left_hand_mode = false; + + // temporarily disabled + // setFallbackToolProperty(); + setFallbackViewControllerProperty(); +} + +void BirdEyeViewTool::setFallbackViewControllerProperty() +{ + fallback_view_controller_property_->clearOptions(); + m_view_controller_classes.clear(); + + m_view_controller_classes = context_->getViewManager()->getDeclaredClassIdsFromFactory(); + + for (int i = 0; i < m_view_controller_classes.size(); i++) { + if (m_view_controller_classes[i] != QString("tier4_camera_view_rviz_plugin/BirdEyeView")) { + fallback_view_controller_property_->addOption(m_view_controller_classes[i], i); + m_view_controller.push_back(context_->getViewManager()->getViewAt(i)); + } + } + + fallback_view_controller_property_->show(); + setFallbackViewController(); +} + +// temporarily disabled +// void BirdEyeViewTool::setFallbackToolProperty() +// { +// fallback_tool_property_->clearOptions(); +// m_tools.clear(); + +// m_tool_classes = context_->getToolManager()->getToolClasses(); + +// for (int i = 0; i < m_tool_classes.size(); i++) { +// if (m_tool_classes[i] != getClassId()) { +// fallback_tool_property_->addOption(m_tool_classes[i], i); +// m_tools.push_back(context_->getToolManager()->getTool(i)); +// } +// } + +// fallback_tool_property_->show(); +// setFallbackTool(); +// } + +void BirdEyeViewTool::activate() +{ + context_->getViewManager()->setCurrentViewControllerType( + QString("tier4_camera_view_rviz_plugin/BirdEyeView")); + context_->getViewManager()->getCurrent()->reset(); + + // temporarily disabled + // setFallbackToolProperty(); + setFallbackViewControllerProperty(); +} + +void BirdEyeViewTool::deactivate() +{ +} + +int BirdEyeViewTool::processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) +{ + return 0; +} + +int BirdEyeViewTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (event.panel->getViewController()) { + event.panel->getViewController()->handleMouseEvent(event); + setCursor(event.panel->getViewController()->getCursor()); + } + return 0; +} + +} // namespace tier4_camera_view_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(tier4_camera_view_rviz_plugin::BirdEyeViewTool, rviz_common::Tool) diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_tool.hpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_tool.hpp new file mode 100644 index 0000000000000..212cdd49c13bd --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_tool.hpp @@ -0,0 +1,144 @@ +// Copyright 2023 Autoware Foundation +// +// 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. +/* + * UOS-ROS packages - Robot Operating System code by the University of Osnabrück + * Copyright (C) 2014 University of Osnabrück + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. 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. + * + * 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 HOLDER 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. + * + * route_set_tool.h + * + * Author: Henning Deeken + * + */ + +#ifndef BIRD_EYE_VIEW_TOOL_HPP_ +#define BIRD_EYE_VIEW_TOOL_HPP_ + +#include "bird_eye_view_controller.hpp" +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/render_panel.hpp" +#include "rviz_common/tool.hpp" +#include "rviz_common/viewport_mouse_event.hpp" + +#include +#include +#include + +#include +#include + +/** + * + *@class BirdEyeViewTool + * + *@brief Implements a rviz tool that allows to navigate in a ego-shooter mode. + */ + +namespace tier4_camera_view_rviz_plugin +{ + +class FPSMotionConfigWidget; +class BirdEyeViewTool : public rviz_common::Tool +{ + Q_OBJECT + +public: + BirdEyeViewTool(); + ~BirdEyeViewTool(); + virtual void onInitialize(); + virtual void activate(); + virtual void deactivate(); + + virtual int processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel); + virtual int processMouseEvent(rviz_common::ViewportMouseEvent & event); + +private Q_SLOTS: + void setOffset() { m_pos_offset = static_cast(step_length_property_->getFloat()); } + void setBoost() + { + if (boost_property_->getFloat() < 0.0) { + m_boost = 0.0; + } else if (boost_property_->getFloat() > 1.0) { + m_boost = 1.0; + } else { + m_boost = static_cast(boost_property_->getFloat()); + } + } + + void setFlyMode() { m_fly_mode = fly_property_->getBool(); } + void setLeftHandMode() { m_left_hand_mode = left_hand_property_->getBool(); } + // temporarily disabled + // void setFallbackTool() { m_fallback_tool = m_tools.at(fallback_tool_property_->getOptionInt()); + // } + void setFallbackViewController() + { + m_fallback_view_controller = + m_view_controller_classes.at(fallback_view_controller_property_->getOptionInt()); + } + +private: + Ogre::SceneNode * m_sceneNode; + + bool m_fly_mode; + bool m_left_hand_mode; + bool m_removed_select; + + double m_pos_offset; + double m_boost; + + QStringList m_tool_classes; + // temporarily disabled + // std::vector m_tools; + // rviz_common::Tool *m_fallback_tool; + + QStringList m_view_controller_classes; + QString m_fallback_view_controller; + std::vector m_view_controller; + + rviz_common::properties::FloatProperty * step_length_property_; + rviz_common::properties::FloatProperty * boost_property_; + rviz_common::properties::BoolProperty * fly_property_; + rviz_common::properties::BoolProperty * left_hand_property_; + // temporarily disabled + // rviz_common::properties::EnumProperty *fallback_tool_property_; + rviz_common::properties::EnumProperty * fallback_view_controller_property_; + + // temporarily disabled + // void setFallbackToolProperty(); + void setFallbackViewControllerProperty(); +}; +} // namespace tier4_camera_view_rviz_plugin +#endif // BIRD_EYE_VIEW_TOOL_HPP_ diff --git a/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp new file mode 100644 index 0000000000000..9f4306a9d982a --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp @@ -0,0 +1,258 @@ +// Copyright 2023 Autoware Foundation +// +// 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. +/* + * Copyright (c) 2009, 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 the 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 "third_person_view_controller.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/tf_frame_property.hpp" +#include "rviz_common/properties/vector_property.hpp" +#include "rviz_common/viewport_mouse_event.hpp" +#include "rviz_rendering/objects/shape.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace tier4_camera_view_rviz_plugin +{ +static const float PITCH_START = Ogre::Math::HALF_PI / 3; +static const float YAW_START = Ogre::Math::PI; +static const float DISTANCE_START = 30; +static const float FOCAL_SHAPE_SIZE_START = 0.05; +static const bool FOCAL_SHAPE_FIXED_SIZE = true; +static const char * TARGET_FRAME_START = "base_link"; + +// move camera up so the focal point appears in the lower image half +static const float CAMERA_OFFSET = 0.2; + +void ThirdPersonViewController::onInitialize() +{ + OrbitViewController::onInitialize(); + focal_shape_->setColor(0.0f, 1.0f, 1.0f, 0.5f); +} + +void ThirdPersonViewController::reset() +{ + yaw_property_->setFloat(YAW_START); + pitch_property_->setFloat(PITCH_START); + distance_property_->setFloat(DISTANCE_START); + focal_shape_size_property_->setFloat(FOCAL_SHAPE_SIZE_START); + focal_shape_fixed_size_property_->setBool(false); + updateFocalShapeSize(); + focal_point_property_->setVector(Ogre::Vector3::ZERO); +} + +std::pair ThirdPersonViewController::intersectGroundPlane(Ogre::Ray mouse_ray) +{ + // convert rays into reference frame + mouse_ray.setOrigin(target_scene_node_->convertWorldToLocalPosition(mouse_ray.getOrigin())); + mouse_ray.setDirection( + target_scene_node_->convertWorldToLocalOrientation(Ogre::Quaternion::IDENTITY) * + mouse_ray.getDirection()); + + Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0); + + std::pair intersection = mouse_ray.intersects(ground_plane); + return std::make_pair(intersection.first, mouse_ray.getPoint(intersection.second)); +} + +void ThirdPersonViewController::handleMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (event.shift()) { + setStatus("Left-Click: Move X/Y. Right-Click:: Zoom."); + } else { + setStatus( + "Left-Click: Rotate. Middle-Click: Move X/Y. Right-Click:: Move Z. " + " Shift: More options."); + } + + int32_t diff_x = 0; + int32_t diff_y = 0; + + bool moved = false; + if (event.type == QEvent::MouseButtonPress) { + focal_shape_->getRootNode()->setVisible(true); + moved = true; + } else if (event.type == QEvent::MouseButtonRelease) { + focal_shape_->getRootNode()->setVisible(false); + moved = true; + } else if (event.type == QEvent::MouseMove) { + diff_x = event.x - event.last_x; + diff_y = event.y - event.last_y; + moved = true; + } + + if (event.left() && !event.shift()) { + setCursor(Rotate3D); + yaw(diff_x * 0.005); + pitch(-diff_y * 0.005); + } else if (event.middle() || (event.left() && event.shift())) { + setCursor(MoveXY); + // handle mouse movement + int width = camera_->getViewport()->getActualWidth(); + int height = camera_->getViewport()->getActualHeight(); + + Ogre::Ray mouse_ray = camera_->getCameraToViewportRay( + event.x / static_cast(width), event.y / static_cast(height)); + + Ogre::Ray last_mouse_ray = camera_->getCameraToViewportRay( + event.last_x / static_cast(width), event.last_y / static_cast(height)); + + auto last_intersect_pair = intersectGroundPlane(last_mouse_ray); + auto intersect_pair = intersectGroundPlane(mouse_ray); + + if (last_intersect_pair.first && intersect_pair.first) { + Ogre::Vector3 motion = intersect_pair.second - last_intersect_pair.second; + + // When dragging near the horizon, the motion can get out of + // control. This throttles it to an arbitrary limit per mouse + // event. + float motion_distance_limit = 1; /*meter*/ + if (motion.length() > motion_distance_limit) { + motion.normalise(); + motion *= motion_distance_limit; + } + + focal_point_property_->add(motion); + emitConfigChanged(); + } + } else if (event.right()) { + setCursor(Zoom); + zoom(-diff_y * 0.1 * (distance_property_->getFloat() / 10.0f)); + } else { + setCursor(event.shift() ? MoveXY : Rotate3D); + } + + if (event.wheel_delta != 0) { + int diff = event.wheel_delta; + zoom(diff * 0.001 * distance_property_->getFloat()); + moved = true; + } + + if (moved) { + context_->queueRender(); + } +} + +void ThirdPersonViewController::mimic(rviz_common::ViewController * source_view) +{ + FramePositionTrackingViewController::mimic(source_view); + + target_frame_property_->setValue(TARGET_FRAME_START); + getNewTransform(); + + Ogre::Camera * source_camera = source_view->getCamera(); + + Ogre::Ray camera_dir_ray(source_camera->getRealPosition(), source_camera->getRealDirection()); + Ogre::Ray camera_down_ray(source_camera->getRealPosition(), -1.0f * source_camera->getRealUp()); + + auto camera_intersection = intersectGroundPlane(camera_dir_ray); + auto camera_down_intersection = intersectGroundPlane(camera_down_ray); + + if (camera_intersection.first && camera_down_intersection.first) { + // Set a focal point by intersecting with the ground plane from above. This will be possible + // if some part of the ground plane is visible in the view and the camera is above the z-plane. + float l_b = source_camera->getRealPosition().distance(camera_intersection.second); + float l_a = source_camera->getRealPosition().distance(camera_down_intersection.second); + + distance_property_->setFloat((l_b * l_a) / (CAMERA_OFFSET * l_b + l_a)); + Ogre::Vector3 position_offset = + source_camera->getRealUp() * distance_property_->getFloat() * CAMERA_OFFSET; + + camera_dir_ray.setOrigin(source_camera->getRealPosition() - position_offset); + auto new_focal_point = intersectGroundPlane(camera_dir_ray); + focal_point_property_->setVector(new_focal_point.second - reference_position_); + + calculatePitchYawFromPosition( + source_camera->getParentSceneNode()->getPosition() - position_offset); + } +} + +void ThirdPersonViewController::updateCamera() +{ + OrbitViewController::updateCamera(); + camera_->getParentSceneNode()->setPosition( + camera_->getParentSceneNode()->getPosition() + + camera_->getParentSceneNode()->getLocalAxes() * Ogre::Vector3::UNIT_Y * + distance_property_->getFloat() * CAMERA_OFFSET); +} + +void ThirdPersonViewController::updateTargetSceneNode() +{ + if (FramePositionTrackingViewController::getNewTransform()) { + target_scene_node_->setPosition(reference_position_); + Ogre::Radian ref_yaw = reference_orientation_.getRoll( + false); // OGRE camera frame looks along -Z, so they call rotation around Z "roll". + Ogre::Quaternion ref_yaw_quat(Ogre::Math::Cos(ref_yaw / 2), 0, 0, Ogre::Math::Sin(ref_yaw / 2)); + target_scene_node_->setOrientation(ref_yaw_quat); + + context_->queueRender(); + } +} + +void ThirdPersonViewController::lookAt(const Ogre::Vector3 & point) +{ + Ogre::Vector3 camera_position = camera_->getParentSceneNode()->getPosition(); + Ogre::Vector3 new_focal_point = + target_scene_node_->getOrientation().Inverse() * (point - target_scene_node_->getPosition()); + new_focal_point.z = 0; + distance_property_->setFloat(new_focal_point.distance(camera_position)); + focal_point_property_->setVector(new_focal_point); + + calculatePitchYawFromPosition(camera_position); +} + +} // namespace tier4_camera_view_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS( + tier4_camera_view_rviz_plugin::ThirdPersonViewController, rviz_common::ViewController) diff --git a/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.hpp b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.hpp new file mode 100644 index 0000000000000..fef036ceccda3 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.hpp @@ -0,0 +1,94 @@ +// Copyright 2023 Autoware Foundation +// +// 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. +/* + * 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 the 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 THIRD_PERSON_VIEW_CONTROLLER_HPP_ +#define THIRD_PERSON_VIEW_CONTROLLER_HPP_ + +#include "rviz_default_plugins/view_controllers/orbit/orbit_view_controller.hpp" + +#include + +#include + +namespace Ogre +{ +class SceneNode; +} + +namespace tier4_camera_view_rviz_plugin +{ +class TfFrameProperty; + +/** + * \brief Like the orbit view controller, but focal point moves only in the x-y plane. + */ +class ThirdPersonViewController : public rviz_default_plugins::view_controllers::OrbitViewController +{ + Q_OBJECT + +public: + void onInitialize() override; + + void handleMouseEvent(rviz_common::ViewportMouseEvent & evt) override; + + void lookAt(const Ogre::Vector3 & point) override; + + void reset() override; + + /** @brief Configure the settings of this view controller to give, + * as much as possible, a similar view as that given by the + * @a source_view. + * + * @a source_view must return a valid @c Ogre::Camera* from getCamera(). */ + void mimic(rviz_common::ViewController * source_view) override; + +protected: + void updateCamera() override; + + void updateTargetSceneNode() override; + + std::pair intersectGroundPlane(Ogre::Ray mouse_ray); +}; + +} // namespace tier4_camera_view_rviz_plugin + +#endif // THIRD_PERSON_VIEW_CONTROLLER_HPP_ diff --git a/common/tier4_camera_view_rviz_plugin/src/third_person_view_tool.cpp b/common/tier4_camera_view_rviz_plugin/src/third_person_view_tool.cpp new file mode 100644 index 0000000000000..09c7fe0c2677a --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/third_person_view_tool.cpp @@ -0,0 +1,173 @@ +// Copyright 2023-2024 Autoware Foundation +// +// 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. +/* + * UOS-ROS packages - Robot Operating System code by the University of Osnabrück + * Copyright (C) 2014 University of Osnabrück + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. 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. + * + * 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 HOLDER 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. + * + * route_set_tool.cpp + * + * Author: Henning Deeken {hdeeken@uos.de} + * + * + */ + +#include "third_person_view_tool.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/view_manager.hpp" + +#include + +namespace tier4_camera_view_rviz_plugin +{ +ThirdPersonViewTool::ThirdPersonViewTool() +{ + shortcut_key_ = 'o'; +} + +ThirdPersonViewTool::~ThirdPersonViewTool() +{ +} + +void ThirdPersonViewTool::onInitialize() +{ + setName("Third Person View"); + + step_length_property_ = new rviz_common::properties::FloatProperty( + "Step Length", 0.1, "The length by with the position is updated on each step.", + getPropertyContainer(), SLOT(setOffset()), this); + + boost_property_ = new rviz_common::properties::FloatProperty( + "Boost Property", 0.5, "Gives the boost factor which is applied if pressing shift.", + getPropertyContainer(), SLOT(setBoost()), this); + + fly_property_ = new rviz_common::properties::BoolProperty( + "Fly Mode", false, "In fly mode it is possible to move along the z axis as well.", + getPropertyContainer(), SLOT(setFlyMode()), this); + + left_hand_property_ = new rviz_common::properties::BoolProperty( + "Left Hand Mode", false, "In left hand mode one uses the arrows to move around", + getPropertyContainer(), SLOT(setLeftHandMode()), this); + + // temporarily disabled + // fallback_tool_property_ = new rviz_common::properties::EnumProperty( + // "Fallback Tool", QString("tier4_camera_view_rviz_plugin/ThirdPersonViewTool"), + // "Determines to which tool the control switches, if the tool is deactivated.", + // getPropertyContainer(), SLOT(setFallbackTool()), this); + fallback_view_controller_property_ = new rviz_common::properties::EnumProperty( + "Fallback ViewController", QString("tier4_camera_view_rviz_plugin/ThirdPersonView"), + "Determines to which view controller the control switches, if the tool is deactivated.", + getPropertyContainer(), SLOT(setFallbackViewController()), this); + + m_pos_offset = 0.1; + m_boost = 0.5; + m_fly_mode = false; + m_left_hand_mode = false; + + // temporarily disabled + // setFallbackToolProperty(); + setFallbackViewControllerProperty(); +} + +void ThirdPersonViewTool::setFallbackViewControllerProperty() +{ + fallback_view_controller_property_->clearOptions(); + m_view_controller_classes.clear(); + + m_view_controller_classes = context_->getViewManager()->getDeclaredClassIdsFromFactory(); + + for (int i = 0; i < m_view_controller_classes.size(); i++) { + if (m_view_controller_classes[i] != QString("tier4_camera_view_rviz_plugin/ThirdPersonView")) { + fallback_view_controller_property_->addOption(m_view_controller_classes[i], i); + m_view_controller.push_back(context_->getViewManager()->getViewAt(i)); + } + } + + fallback_view_controller_property_->show(); + setFallbackViewController(); +} + +// temporarily disabled +// void ThirdPersonViewTool::setFallbackToolProperty() +// { +// fallback_tool_property_->clearOptions(); +// m_tools.clear(); + +// m_tool_classes = context_->getToolManager()->getToolClasses(); + +// for (int i = 0; i < m_tool_classes.size(); i++) { +// if (m_tool_classes[i] != getClassId()) { +// fallback_tool_property_->addOption(m_tool_classes[i], i); +// m_tools.push_back(context_->getToolManager()->getTool(i)); +// } +// } + +// fallback_tool_property_->show(); +// setFallbackTool(); +// } + +void ThirdPersonViewTool::activate() +{ + context_->getViewManager()->setCurrentViewControllerType( + QString("tier4_camera_view_rviz_plugin/ThirdPersonView")); + context_->getViewManager()->getCurrent()->reset(); + + // temporarily disabled + // setFallbackToolProperty(); + setFallbackViewControllerProperty(); +} + +void ThirdPersonViewTool::deactivate() +{ +} + +int ThirdPersonViewTool::processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) +{ + return 0; +} + +int ThirdPersonViewTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (event.panel->getViewController()) { + event.panel->getViewController()->handleMouseEvent(event); + setCursor(event.panel->getViewController()->getCursor()); + } + return 0; +} + +} // namespace tier4_camera_view_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(tier4_camera_view_rviz_plugin::ThirdPersonViewTool, rviz_common::Tool) diff --git a/common/tier4_camera_view_rviz_plugin/src/third_person_view_tool.hpp b/common/tier4_camera_view_rviz_plugin/src/third_person_view_tool.hpp new file mode 100644 index 0000000000000..37d0db13b4629 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/third_person_view_tool.hpp @@ -0,0 +1,144 @@ +// Copyright 2023 Autoware Foundation +// +// 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. +/* + * UOS-ROS packages - Robot Operating System code by the University of Osnabrück + * Copyright (C) 2014 University of Osnabrück + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. 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. + * + * 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 HOLDER 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. + * + * route_set_tool.h + * + * Author: Henning Deeken + * + */ + +#ifndef THIRD_PERSON_VIEW_TOOL_HPP_ +#define THIRD_PERSON_VIEW_TOOL_HPP_ + +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/render_panel.hpp" +#include "rviz_common/tool.hpp" +#include "rviz_common/viewport_mouse_event.hpp" +#include "third_person_view_controller.hpp" + +#include +#include +#include + +#include +#include + +/** + * + *@class ThirdPersonViewTool + * + *@brief Implements a rviz tool that allows to navigate in a ego-shooter mode. + */ + +namespace tier4_camera_view_rviz_plugin +{ + +class FPSMotionConfigWidget; +class ThirdPersonViewTool : public rviz_common::Tool +{ + Q_OBJECT + +public: + ThirdPersonViewTool(); + ~ThirdPersonViewTool(); + virtual void onInitialize(); + virtual void activate(); + virtual void deactivate(); + + virtual int processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel); + virtual int processMouseEvent(rviz_common::ViewportMouseEvent & event); + +private Q_SLOTS: + void setOffset() { m_pos_offset = static_cast(step_length_property_->getFloat()); } + void setBoost() + { + if (boost_property_->getFloat() < 0.0) { + m_boost = 0.0; + } else if (boost_property_->getFloat() > 1.0) { + m_boost = 1.0; + } else { + m_boost = static_cast(boost_property_->getFloat()); + } + } + + void setFlyMode() { m_fly_mode = fly_property_->getBool(); } + void setLeftHandMode() { m_left_hand_mode = left_hand_property_->getBool(); } + // temporarily disabled + // void setFallbackTool() { m_fallback_tool = m_tools.at(fallback_tool_property_->getOptionInt()); + // } + void setFallbackViewController() + { + m_fallback_view_controller = + m_view_controller_classes.at(fallback_view_controller_property_->getOptionInt()); + } + +private: + Ogre::SceneNode * m_sceneNode; + + bool m_fly_mode; + bool m_left_hand_mode; + bool m_removed_select; + + double m_pos_offset; + double m_boost; + + QStringList m_tool_classes; + // temporarily disabled + // std::vector m_tools; + // rviz_common::Tool *m_fallback_tool; + + QStringList m_view_controller_classes; + QString m_fallback_view_controller; + std::vector m_view_controller; + + rviz_common::properties::FloatProperty * step_length_property_; + rviz_common::properties::FloatProperty * boost_property_; + rviz_common::properties::BoolProperty * fly_property_; + rviz_common::properties::BoolProperty * left_hand_property_; + // temporarily disabled + // rviz_common::properties::EnumProperty *fallback_tool_property_; + rviz_common::properties::EnumProperty * fallback_view_controller_property_; + + // temporarily disabled + // void setFallbackToolProperty(); + void setFallbackViewControllerProperty(); +}; +} // namespace tier4_camera_view_rviz_plugin +#endif // THIRD_PERSON_VIEW_TOOL_HPP_