diff --git a/examples/visualization_demo/Main.cc b/examples/visualization_demo/Main.cc index e36d550ec..b4cfc2f4f 100644 --- a/examples/visualization_demo/Main.cc +++ b/examples/visualization_demo/Main.cc @@ -118,11 +118,21 @@ void buildScene(ScenePtr _scene) blue->SetTransparency(0.5); blue->SetDepthWriteEnabled(false); + // create gray material + MaterialPtr gray = _scene->CreateMaterial(); + gray->SetAmbient(0.7, 0.7, 0.7); + gray->SetDiffuse(0.7, 0.7, 0.7); + gray->SetSpecular(0.7, 0.7, 0.7); + gray->SetShininess(50); + gray->SetReflectivity(0); + gray->SetTransparency(0.75); + gray->SetDepthWriteEnabled(false); + // create box visual VisualPtr box = _scene->CreateVisual("parent_box"); box->AddGeometry(_scene->CreateBox()); box->SetOrigin(0.0, 0.0, 0.0); - box->SetLocalPosition(4.5, 0.0, 0.0); + box->SetLocalPosition(4.5, -1.0, 0.0); box->SetLocalRotation(0, 0, 0); box->SetMaterial(blue); root->AddChild(box); @@ -149,7 +159,7 @@ void buildScene(ScenePtr _scene) ignition::math::Pose3d p(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); ignition::math::Inertiald inertial{massMatrix, p}; inertiaVisual->SetInertial(inertial); - inertiaVisual->SetLocalPosition(1, 0, 0); + inertiaVisual->SetLocalPosition(1.5, -1.0, 0); root->AddChild(inertiaVisual); // create CoM visual @@ -162,6 +172,34 @@ void buildScene(ScenePtr _scene) comVisual->SetInertial(comVisualInertial); box->AddChild(comVisual); + // create joint child visual + VisualPtr jointChildBox = _scene->CreateVisual("joint_child"); + jointChildBox->AddGeometry(_scene->CreateBox()); + jointChildBox->SetOrigin(0.0, 0.0, 0.0); + jointChildBox->SetLocalPosition(3.5, 0.5, 0.0); + jointChildBox->SetLocalRotation(0, 0, 0); + jointChildBox->SetMaterial(blue); + root->AddChild(jointChildBox); + + // create joint parent visual + VisualPtr jointParentBox = _scene->CreateVisual("joint_parent"); + jointParentBox->AddGeometry(_scene->CreateBox()); + jointParentBox->SetOrigin(0.0, 0.0, 0.0); + jointParentBox->SetLocalPosition(2.0, 0.5, 0.0); + jointParentBox->SetLocalRotation(1.5, -1.0, 0); + jointParentBox->SetMaterial(gray); + root->AddChild(jointParentBox); + + // create joint visual + JointVisualPtr jointVisual = _scene->CreateJointVisual(); + jointChildBox->AddChild(jointVisual); + jointVisual->SetType(JointVisualType::JVT_REVOLUTE2); + ignition::math::Vector3d axis2(1.0, 1.0, 1.0); + jointVisual->SetAxis(axis2); + + ignition::math::Vector3d axis1(1.0, 0.0, 0.0); + jointVisual->SetParentAxis(axis1, jointParentBox->Name(), true); + // create camera CameraPtr camera = _scene->CreateCamera("camera"); camera->SetLocalPosition(0.0, 0.0, 0.0); diff --git a/include/ignition/rendering/ArrowVisual.hh b/include/ignition/rendering/ArrowVisual.hh index 38c3ca3dd..2f4696be8 100644 --- a/include/ignition/rendering/ArrowVisual.hh +++ b/include/ignition/rendering/ArrowVisual.hh @@ -42,9 +42,21 @@ namespace ignition /// \return The arrow-shaft visual public: virtual VisualPtr Shaft() const = 0; + /// \brief Get arrow-rotation visual + /// \return The arrow-rotation visual + public: virtual VisualPtr Rotation() const = 0; + /// \brief set true to show the arrow head, false otherwise /// \param[in] _b true to show the arrow head, false otherwise public: virtual void ShowArrowHead(bool _b) = 0; + + /// \brief set true to show the arrow shaft, false otherwise + /// \param[in] _b true to show the arrow shaft, false otherwise + public: virtual void ShowArrowShaft(bool _b) = 0; + + /// \brief Set true to show the rotation of the arrow, false otherwise + /// \param[in] _b True to show the arrow rotation. + public: virtual void ShowArrowRotation(bool _b) = 0; }; } } diff --git a/include/ignition/rendering/AxisVisual.hh b/include/ignition/rendering/AxisVisual.hh index d94db465d..57fe72853 100644 --- a/include/ignition/rendering/AxisVisual.hh +++ b/include/ignition/rendering/AxisVisual.hh @@ -37,6 +37,11 @@ namespace ignition /// \brief set true to show the axis heads, false otherwise /// \param[in] _b true to show the axis heads, false otherwise public: virtual void ShowAxisHead(bool _b) = 0; + + /// \brief set true to show the specified axis head, false otherwise + /// \param[in] _axis Axis index. 0: x, 1: y, 2: z + /// \param[in] _b true to show the specified axis head, false otherwise + public: virtual void ShowAxisHead(unsigned int _axis, bool _b) = 0; }; } } diff --git a/include/ignition/rendering/JointVisual.hh b/include/ignition/rendering/JointVisual.hh new file mode 100644 index 000000000..d0cb89664 --- /dev/null +++ b/include/ignition/rendering/JointVisual.hh @@ -0,0 +1,138 @@ +/* + * Copyright (C) 2021 Open Source Robotics 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. + * + */ +#ifndef IGNITION_RENDERING_JOINTVISUAL_HH_ +#define IGNITION_RENDERING_JOINTVISUAL_HH_ + +#include + +#include + +#include "ignition/rendering/config.hh" +#include "ignition/rendering/Object.hh" +#include "ignition/rendering/RenderTypes.hh" +#include "ignition/rendering/Visual.hh" + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + // + /// \brief Enum for JointVisual types + enum IGNITION_RENDERING_VISIBLE JointVisualType + { + /// \brief No type + JVT_NONE = 0, + + /// \brief Revolute joint type + JVT_REVOLUTE = 1, + + /// \brief Revolute2 joint type + JVT_REVOLUTE2 = 2, + + /// \brief Prismatic joint type + JVT_PRISMATIC = 3, + + /// \brief Universal joint type + JVT_UNIVERSAL = 4, + + /// \brief Ball joint type + JVT_BALL = 5, + + /// \brief Screw joint type + JVT_SCREW = 6, + + /// \brief Gearbox joint type + JVT_GEARBOX = 7, + + /// \brief Fixed joint type + JVT_FIXED = 8 + }; + + /// \class JointVisual JointVisual.hh + /// ignition/rendering/JointVisual.hh + /// \brief Represents a joint visual + class IGNITION_RENDERING_VISIBLE JointVisual : + public virtual Visual + { + /// \brief Destructor + public: virtual ~JointVisual() {} + + /// \brief Create an axis and attach it to the joint visual. + /// \param[in] _axis Axis vector. + /// \param[in] _useParentFrame True if axis vector is expressed in + /// parent frame. + public: virtual void SetAxis(const ignition::math::Vector3d &_axis, + bool _useParentFrame = false) = 0; + + /// \brief Get axis vector. + /// \return The axis vector. + public: virtual ignition::math::Vector3d Axis() const = 0; + + /// \brief Create a parent axis for hinge2 and universal joint types + /// and attach it to the joint visual. + /// \param[in] _axis Axis vector. + /// \param[in] _parentName Joint parent name. + /// \param[in] _useParentFrame True if axis vector is expressed in + /// parent frame. + public: virtual void SetParentAxis( + const ignition::math::Vector3d &_axis, + const std::string &_parentName, + bool _useParentFrame = false) = 0; + + /// \brief Get parent axis vector. + /// \return The parent axis vector. + public: virtual ignition::math::Vector3d ParentAxis() const = 0; + + /// \brief Update an axis' arrow visual. + /// \param[in] _axis Axis vector. + /// \param[in] _useParentFrame True if axis vector is expressed in + /// parent frame. + /// \return True if axis was updated else false. + public: virtual bool UpdateAxis(const ignition::math::Vector3d &_axis, + bool _useParentFrame = false) = 0; + + /// \brief Update the parent axis' arrow visual if it exists. + /// \param[in] _axis Axis vector. + /// \param[in] _useParentFrame True if axis vector is expressed in + /// parent frame. + /// \return True if parent axis was updated else false. + public: virtual bool UpdateParentAxis( + const ignition::math::Vector3d &_axis, + bool _useParentFrame = false) = 0; + + /// \brief Set type for joint visual. + /// \param[in] _type The type of visualisation for joint. + public: virtual void SetType(const JointVisualType _type) = 0; + + /// \brief Get joint visual type. + /// \return The joint visual type. + public: virtual JointVisualType Type() const = 0; + + /// \brief Get the JointVisual which is attached to the parent. + /// \return Parent axis visual. + public: virtual JointVisualPtr ParentAxisVisual() const = 0; + + /// \brief Get the arrow visual which represents the axis attached to the + /// child. + /// \return Arrow visual. + public: virtual ArrowVisualPtr ArrowVisual() const = 0; + }; + } + } +} +#endif diff --git a/include/ignition/rendering/Node.hh b/include/ignition/rendering/Node.hh index 096fb281a..d04352bb7 100644 --- a/include/ignition/rendering/Node.hh +++ b/include/ignition/rendering/Node.hh @@ -70,6 +70,10 @@ namespace ignition /// \return The local pose public: virtual math::Pose3d LocalPose() const = 0; + /// \brief Get the initial local pose + /// \return The initial local pose + public: virtual math::Pose3d InitialLocalPose() const = 0; + /// \brief Set the local pose /// \param[in] _pose New local pose public: virtual void SetLocalPose(const math::Pose3d &_pose) = 0; diff --git a/include/ignition/rendering/Scene.hh b/include/ignition/rendering/Scene.hh index 169060dc5..d38b36612 100644 --- a/include/ignition/rendering/Scene.hh +++ b/include/ignition/rendering/Scene.hh @@ -881,6 +881,35 @@ namespace ignition public: virtual InertiaVisualPtr CreateInertiaVisual( unsigned int _id, const std::string &_name) = 0; + /// \brief Create new joint visual. A unique ID and name will + /// automatically be assigned to the Joint visual. + /// \return The created Joint visual + public: virtual JointVisualPtr CreateJointVisual() = 0; + + /// \brief Create new joint visual with the given ID. A unique name + /// will automatically be assigned to the visual. If the given ID is + /// already in use, NULL will be returned. + /// \param[in] _id ID of the new Joint visual + /// \return The created Joint visual + public: virtual JointVisualPtr CreateJointVisual( + unsigned int _id) = 0; + + /// \brief Create new joint visual with the given name. A unique ID + /// will automatically be assigned to the visual. If the given name is + /// already in use, NULL will be returned. + /// \param[in] _name Name of the new Joint visual + /// \return The created Joint visual + public: virtual JointVisualPtr CreateJointVisual( + const std::string &_name) = 0; + + /// \brief Create new joint visual with the given name. If either the + /// given ID or name is already in use, NULL will be returned. + /// \param[in] _id ID of the new Joint visual + /// \param[in] _name Name of the new Joint visual + /// \return The created Joint visual + public: virtual JointVisualPtr CreateJointVisual( + unsigned int _id, const std::string &_name) = 0; + /// \brief Create new light visual. A unique ID and name will /// automatically be assigned to the light visual. /// \return The created light visual diff --git a/include/ignition/rendering/base/BaseArrowVisual.hh b/include/ignition/rendering/base/BaseArrowVisual.hh index 6e1c715bc..f4e85aa70 100644 --- a/include/ignition/rendering/base/BaseArrowVisual.hh +++ b/include/ignition/rendering/base/BaseArrowVisual.hh @@ -17,6 +17,10 @@ #ifndef IGNITION_RENDERING_BASE_BASEARROWVISUAL_HH_ #define IGNITION_RENDERING_BASE_BASEARROWVISUAL_HH_ +#include + +#include + #include "ignition/rendering/ArrowVisual.hh" #include "ignition/rendering/Scene.hh" @@ -37,17 +41,35 @@ namespace ignition /// \brief Destructor public: virtual ~BaseArrowVisual(); + // Documentation inherited. + protected: virtual void Destroy() override; + // Documentation inherited. public: virtual VisualPtr Head() const override; // Documentation inherited. public: virtual VisualPtr Shaft() const override; + // Documentation inherited. + public: virtual VisualPtr Rotation() const override; + // Documentation inherited public: virtual void ShowArrowHead(bool _b) override; + // Documentation inherited + public: virtual void ShowArrowShaft(bool _b) override; + + // Documentation inherited + public: virtual void ShowArrowRotation(bool _b) override; + + // Documentation inherited + public: virtual void SetVisible(bool _visible) override; + // Documentation inherited. protected: virtual void Init() override; + + /// \brief Flag to indicate whether arrow rotation is visible + protected: bool rotationVisible = false; }; ////////////////////////////////////////////////// @@ -62,16 +84,37 @@ namespace ignition { } + ///////////////////////////////////////////////// + template + void BaseArrowVisual::Destroy() + { + while (this->ChildCount() > 0u) + { + auto visual = std::dynamic_pointer_cast(this->ChildByIndex(0)); + if (visual) + { + visual->Destroy(); + } + } + } + ////////////////////////////////////////////////// template VisualPtr BaseArrowVisual::Head() const { - return std::dynamic_pointer_cast(this->ChildByIndex(1)); + return std::dynamic_pointer_cast(this->ChildByIndex(2)); } ////////////////////////////////////////////////// template VisualPtr BaseArrowVisual::Shaft() const + { + return std::dynamic_pointer_cast(this->ChildByIndex(1)); + } + + ////////////////////////////////////////////////// + template + VisualPtr BaseArrowVisual::Rotation() const { return std::dynamic_pointer_cast(this->ChildByIndex(0)); } @@ -79,6 +122,18 @@ namespace ignition ////////////////////////////////////////////////// template void BaseArrowVisual::ShowArrowHead(bool _b) + { + NodePtr child = this->ChildByIndex(2); + VisualPtr visual = std::dynamic_pointer_cast(child); + if (visual) + { + visual->SetVisible(_b); + } + } + + ////////////////////////////////////////////////// + template + void BaseArrowVisual::ShowArrowShaft(bool _b) { NodePtr child = this->ChildByIndex(1); VisualPtr visual = std::dynamic_pointer_cast(child); @@ -88,6 +143,37 @@ namespace ignition } } + ////////////////////////////////////////////////// + template + void BaseArrowVisual::ShowArrowRotation(bool _b) + { + NodePtr child = this->ChildByIndex(0); + VisualPtr visual = std::dynamic_pointer_cast(child); + if (visual) + { + visual->SetVisible(_b); + this->rotationVisible = _b; + } + } + + ////////////////////////////////////////////////// + template + void BaseArrowVisual::SetVisible(bool _visible) + { + T::SetVisible(_visible); + + NodePtr child = this->ChildByIndex(0); + VisualPtr visual = std::dynamic_pointer_cast(child); + if (visual) + { + // Force rotation visual visibility to false + // if the arrow visual is not visible. + // Else, rotation visual's visibility overrides + // its parent's visibility. + visual->SetVisible(this->rotationVisible && _visible); + } + } + ////////////////////////////////////////////////// template void BaseArrowVisual::Init() @@ -108,6 +194,18 @@ namespace ignition cylinder->SetLocalScale(0.05, 0.05, 0.5); this->AddChild(cylinder); + common::MeshManager *meshMgr = common::MeshManager::Instance(); + std::string rotMeshName = "arrow_rotation"; + if (!meshMgr->HasMesh(rotMeshName)) + meshMgr->CreateTube(rotMeshName, 0.070f, 0.075f, 0.01f, 1, 32); + + VisualPtr rotationVis = this->Scene()->CreateVisual(); + rotationVis->AddGeometry(this->Scene()->CreateMesh(rotMeshName)); + rotationVis->SetOrigin(0, 0, -0.125); + rotationVis->SetLocalPosition(0, 0, 0); + rotationVis->SetVisible(this->rotationVisible); + this->AddChild(rotationVis); + this->SetOrigin(0, 0, -0.5); } } diff --git a/include/ignition/rendering/base/BaseAxisVisual.hh b/include/ignition/rendering/base/BaseAxisVisual.hh index 2b98dec4a..4e3f301ca 100644 --- a/include/ignition/rendering/base/BaseAxisVisual.hh +++ b/include/ignition/rendering/base/BaseAxisVisual.hh @@ -38,6 +38,9 @@ namespace ignition public: virtual void Init() override; + // Documentation inherited. + protected: virtual void Destroy() override; + // Documentation inherited. public: virtual void SetLocalScale( const math::Vector3d &_scale) override; @@ -47,6 +50,12 @@ namespace ignition // Documentation inherited. public: void ShowAxisHead(bool _b) override; + + // Documentation inherited. + public: void ShowAxisHead(unsigned int _axis, bool _b) override; + + // Documentation inherited. + public: virtual void SetVisible(bool _visible) override; }; ////////////////////////////////////////////////// @@ -61,6 +70,21 @@ namespace ignition { } + ///////////////////////////////////////////////// + template + void BaseAxisVisual::Destroy() + { + for (unsigned int i = 0; i < this->ChildCount(); ++i) + { + auto arrow = std::dynamic_pointer_cast( + this->ChildByIndex(i)); + if (arrow) + { + arrow->Destroy(); + } + } + } + ////////////////////////////////////////////////// template math::Vector3d BaseAxisVisual::LocalScale() const @@ -89,6 +113,21 @@ namespace ignition { auto arrow = std::dynamic_pointer_cast( this->ChildByIndex(i)); + if (arrow) + { + arrow->ShowArrowHead(_b); + } + } + } + + ////////////////////////////////////////////////// + template + void BaseAxisVisual::ShowAxisHead(unsigned int _axis, bool _b) + { + auto arrow = std::dynamic_pointer_cast( + this->ChildByIndex(2u - _axis)); + if (arrow) + { arrow->ShowArrowHead(_b); } } @@ -117,6 +156,20 @@ namespace ignition zArrow->SetMaterial("Default/TransBlue"); this->AddChild(zArrow); } + + ////////////////////////////////////////////////// + template + void BaseAxisVisual::SetVisible(bool _visible) + { + T::SetVisible(_visible); + + for (unsigned int i = 0; i < this->ChildCount(); ++i) + { + auto arrow = std::dynamic_pointer_cast( + this->ChildByIndex(i)); + arrow->SetVisible(_visible); + } + } } } } diff --git a/include/ignition/rendering/base/BaseJointVisual.hh b/include/ignition/rendering/base/BaseJointVisual.hh new file mode 100644 index 000000000..6cf08acfa --- /dev/null +++ b/include/ignition/rendering/base/BaseJointVisual.hh @@ -0,0 +1,543 @@ +/* + * Copyright (C) 2021 Open Source Robotics 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. + * + */ +#ifndef IGNITION_RENDERING_BASE_BASEJOINTVISUAL_HH_ +#define IGNITION_RENDERING_BASE_BASEJOINTVISUAL_HH_ + +#include +#include + +#include "ignition/common/Console.hh" + +#include "ignition/rendering/ArrowVisual.hh" +#include "ignition/rendering/AxisVisual.hh" +#include "ignition/rendering/JointVisual.hh" +#include "ignition/rendering/Scene.hh" +#include "ignition/rendering/base/BaseObject.hh" +#include "ignition/rendering/base/BaseRenderTypes.hh" + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + // + /// \brief Base implementation of a joint visual + template + class BaseJointVisual : + public virtual JointVisual, + public virtual T + { + /// \brief Constructor + protected: BaseJointVisual(); + + /// \brief Destructor + public: virtual ~BaseJointVisual(); + + // Documentation inherited. + protected: virtual void Init() override; + + // Documentation inherited. + protected: virtual void PreRender() override; + + // Documentation inherited. + protected: virtual void Destroy() override; + + // Documentation inherited. + public: virtual void SetAxis(const ignition::math::Vector3d &_axis, + bool _useParentFrame) override; + + // Documentation inherited. + public: virtual ignition::math::Vector3d Axis() const override; + + // Documentation inherited. + public: virtual void SetParentAxis( + const ignition::math::Vector3d &_axis, + const std::string &_parentName, + bool _useParentFrame) override; + + // Documentation inherited. + public: virtual ignition::math::Vector3d ParentAxis() const override; + + // Documentation inherited. + public: virtual bool UpdateAxis(const ignition::math::Vector3d &_axis, + bool _useParentFrame) override; + + // Documentation inherited. + public: virtual bool UpdateParentAxis( + const ignition::math::Vector3d &_axis, + bool _useParentFrame) override; + + // Documentation inherited. + public: virtual void SetType(const JointVisualType _type) override; + + // Documentation inherited. + public: virtual JointVisualType Type() const override; + + // Documentation inherited. + public: virtual JointVisualPtr ParentAxisVisual() const override; + + // Documentation inherited. + public: virtual ArrowVisualPtr ArrowVisual() const override; + + // Documentation inherited. + public: virtual void SetVisible(bool _visible) override; + + /// \brief Implementation for updating an axis' arrow visual. + /// \param[in] _arrowVisual Arrow visual to be updated. + /// \param[in] _axis Axis vector. + /// \param[in] _useParentFrame True if the axis vector is + /// expressed in the joint parent frame. + protected: void UpdateAxisImpl(ArrowVisualPtr _arrowVisual, + const ignition::math::Vector3d &_axis, + bool _useParentFrame); + + /// \brief Helper function to create axis visual. + protected: void CreateAxis(); + + /// \brief Helper function to create parent axis visual. + protected: void CreateParentAxis(); + + /// \brief Scale the joint visual according to the joint's child. + protected: void ScaleToChild(); + + /// \brief Type of joint visualization. + protected: JointVisualType jointVisualType = + JointVisualType::JVT_NONE; + + /// \brief The joint's XYZ frame visual. + protected: AxisVisualPtr axisVisual = nullptr; + + /// \brief The visual representing the one joint axis. There can be only + /// one axis visual per joint visual, so joints with two axes have a 2nd + /// JointVisual with its own arrowVisual. + protected: ArrowVisualPtr arrowVisual = nullptr; + + /// \brief Second joint visual for hinge2 and universal joints. It is a + /// simplified visual without an XYZ frame. + protected: JointVisualPtr parentAxisVis = nullptr; + + /// \brief Scale based on the size of the joint's child. + protected: ignition::math::Vector3d scaleToChild = + ignition::math::Vector3d::One; + + /// \brief Flag to indicate joint visual type has changed. + protected: bool dirtyJointType = false; + + /// \brief Flag to indicate axis data has changed. + protected: bool dirtyAxis = false; + + /// \brief Flag to indicate parent axis data has changed. + protected: bool dirtyParentAxis = false; + + /// \brief Joint visual axis vector. + protected: ignition::math::Vector3d axis = + ignition::math::Vector3d::Zero; + + /// \brief Flag to indicate whether axis vector is + /// expressed in joint parent frame. + protected: bool useParentFrame = false; + + /// \brief Flag to update the axis visual. + protected: bool updateAxis = false; + + /// \brief Parent axis vector. + protected: ignition::math::Vector3d parentAxis = + ignition::math::Vector3d::Zero; + + /// \brief Joint parent name. + protected: std::string jointParentName = ""; + + /// \brief Flag to indicate whether parent axis vector is + /// expressed in joint parent frame. + protected: bool parentAxisUseParentFrame = false; + + /// \brief Flag to update the parent axis visual. + protected: bool updateParentAxis = false; + }; + + ////////////////////////////////////////////////// + template + BaseJointVisual::BaseJointVisual() + { + } + + ////////////////////////////////////////////////// + template + BaseJointVisual::~BaseJointVisual() + { + } + + ///////////////////////////////////////////////// + template + void BaseJointVisual::PreRender() + { + T::PreRender(); + + if (this->ParentAxisVisual()) + { + this->ParentAxisVisual()->PreRender(); + } + + if (this->dirtyJointType) + { + this->UpdateAxis(this->axis, this->useParentFrame); + this->UpdateParentAxis(this->parentAxis, + this->parentAxisUseParentFrame); + + this->dirtyJointType = false; + } + + if (this->dirtyAxis) + { + this->CreateAxis(); + this->dirtyAxis = false; + } + + if (this->dirtyParentAxis) + { + this->CreateParentAxis(); + this->dirtyParentAxis = false; + } + + if (this->updateAxis) + { + this->updateAxis = + !this->UpdateAxis(this->axis, this->useParentFrame); + } + + if (this->updateParentAxis) + { + this->updateParentAxis = + !this->UpdateParentAxis(this->parentAxis, + this->parentAxisUseParentFrame); + } + } + + ////////////////////////////////////////////////// + template + void BaseJointVisual::Init() + { + T::Init(); + + this->axisVisual = this->Scene()->CreateAxisVisual(); + this->AddChild(this->axisVisual); + this->SetInheritScale(false); + } + + ///////////////////////////////////////////////// + template + void BaseJointVisual::Destroy() + { + if (this->arrowVisual != nullptr) + { + this->arrowVisual->Destroy(); + this->arrowVisual.reset(); + } + + if (this->axisVisual != nullptr) + { + this->axisVisual->Destroy(); + this->axisVisual.reset(); + } + + if (this->parentAxisVis != nullptr) + { + this->parentAxisVis->Destroy(); + this->parentAxisVis.reset(); + } + + this->dirtyJointType = false; + this->dirtyAxis = false; + this->dirtyParentAxis = false; + } + + ///////////////////////////////////////////////// + template + void BaseJointVisual::SetAxis( + const ignition::math::Vector3d &_axis, + bool _useParentFrame) + { + this->axis = _axis; + this->useParentFrame = _useParentFrame; + this->dirtyAxis = true; + } + + ///////////////////////////////////////////////// + template + void BaseJointVisual::CreateAxis() + { + if (this->arrowVisual) + { + this->arrowVisual->Destroy(); + this->arrowVisual.reset(); + } + + this->arrowVisual = this->Scene()->CreateArrowVisual(); + this->arrowVisual->SetMaterial("Default/TransYellow"); + this->arrowVisual->SetLocalPosition(0, 0, 0); + this->arrowVisual->SetLocalRotation(0, 0, 0); + this->AddChild(this->arrowVisual); + + this->updateAxis = true; + this->ScaleToChild(); + } + + ///////////////////////////////////////////////// + template + void BaseJointVisual::SetParentAxis( + const ignition::math::Vector3d &_axis, + const std::string &_parentName, + bool _useParentFrame) + { + if (this->Type() != JointVisualType::JVT_REVOLUTE2 && + this->Type() != JointVisualType::JVT_UNIVERSAL) + { + ignlog << "Joint visual is not of type Revolute2 or " + << " Universal " + << " so the parent axis will not be shown\n"; + return; + } + + this->parentAxis = _axis; + this->parentAxisUseParentFrame = _useParentFrame; + this->jointParentName = _parentName; + this->dirtyParentAxis = true; + } + + ///////////////////////////////////////////////// + template + void BaseJointVisual::CreateParentAxis() + { + auto jointParentVis = this->Scene()->NodeByName(this->jointParentName); + if (jointParentVis == nullptr) + { + ignlog << "Joint parent with name " << this->jointParentName + << " does not exist" + << " so the parent axis will not be shown\n"; + return; + } + + if (this->parentAxisVis) + { + this->parentAxisVis->Destroy(); + this->parentAxisVis.reset(); + } + + this->parentAxisVis = this->Scene()->CreateJointVisual(); + jointParentVis->AddChild(this->parentAxisVis); + this->parentAxisVis->SetType(this->Type()); + this->parentAxisVis->SetAxis(this->parentAxis, + this->parentAxisUseParentFrame); + + this->updateParentAxis = true; + this->ScaleToChild(); + } + + ////////////////////////////////////////////////// + template + bool BaseJointVisual::UpdateAxis(const ignition::math::Vector3d &_axis, + bool _useParentFrame) + { + if (this->ArrowVisual() && this->HasParent()) + { + this->UpdateAxisImpl(this->ArrowVisual(), _axis, _useParentFrame); + return true; + } + + return false; + } + + ////////////////////////////////////////////////// + template + bool BaseJointVisual::UpdateParentAxis( + const ignition::math::Vector3d &_axis, + bool _useParentFrame) + { + if (this->ParentAxisVisual() && + this->ParentAxisVisual()->ArrowVisual() && + this->ParentAxisVisual()->HasParent()) + { + this->UpdateAxisImpl(this->ParentAxisVisual()->ArrowVisual(), + _axis, _useParentFrame); + return true; + } + + return false; + } + + ////////////////////////////////////////////////// + template + void BaseJointVisual::UpdateAxisImpl(ArrowVisualPtr _arrowVisual, + const ignition::math::Vector3d &_axis, + bool _useParentFrame) + { + // Get rotation to axis vector + ignition::math::Vector3d axisDir = _axis; + ignition::math::Vector3d u = axisDir.Normalize(); + ignition::math::Vector3d v = ignition::math::Vector3d::UnitZ; + double cosTheta = v.Dot(u); + double angle = acos(cosTheta); + ignition::math::Quaterniond quat; + // check the parallel case + if (ignition::math::equal(angle, IGN_PI)) + quat.Axis(u.Perpendicular(), angle); + else + quat.Axis((v.Cross(u)).Normalize(), angle); + _arrowVisual->SetLocalRotation(quat); + + if (_useParentFrame) + { + ignition::math::Pose3d parentInitPose = + this->Parent()->InitialLocalPose(); + + // get rotation of joint visual in model frame + ignition::math::Quaterniond quatFromModel = + (this->LocalPose() + parentInitPose).Rot(); + + // rotate arrow visual so that the axis vector applies to the model + // frame. + _arrowVisual->SetLocalRotation(quatFromModel.Inverse() * + _arrowVisual->LocalRotation()); + } + + _arrowVisual->ShowArrowRotation( + this->Type() == JointVisualType::JVT_REVOLUTE || + this->Type() == JointVisualType::JVT_REVOLUTE2 || + this->Type() == JointVisualType::JVT_UNIVERSAL || + this->Type() == JointVisualType::JVT_GEARBOX); + + if (this->axisVisual) + _arrowVisual->SetVisible(true); + else + return; + + // Hide existing arrow head if it overlaps with the axis + auto axisWorldRotation = _arrowVisual->WorldPose().Rot(); + auto jointWorldRotation = this->WorldPose().Rot(); + + this->axisVisual->ShowAxisHead(true); + _arrowVisual->ShowArrowShaft(true); + + auto axisWorld = axisWorldRotation * ignition::math::Vector3d::UnitZ; + if (axisWorld == jointWorldRotation * ignition::math::Vector3d::UnitX) + { + this->axisVisual->ShowAxisHead(0, false); + _arrowVisual->ShowArrowShaft(false); + } + else if (axisWorld == + jointWorldRotation * ignition::math::Vector3d::UnitY) + { + this->axisVisual->ShowAxisHead(1, false); + _arrowVisual->ShowArrowShaft(false); + } + else if (axisWorld == + jointWorldRotation * ignition::math::Vector3d::UnitZ) + { + this->axisVisual->ShowAxisHead(2, false); + _arrowVisual->ShowArrowShaft(false); + } + } + + ////////////////////////////////////////////////// + template + void BaseJointVisual::ScaleToChild() + { + if (!this->HasParent()) + return; + + // Joint visual is attached to the child's visual + VisualPtr parentVisual = + std::dynamic_pointer_cast(this->Parent()); + + if (parentVisual) + { + double childSize = + std::max(0.1, parentVisual->BoundingBox().Size().Length()); + this->scaleToChild = ignition::math::Vector3d(childSize * 0.2, + childSize * 0.2, childSize * 0.2); + this->SetLocalScale(this->scaleToChild); + if (this->ParentAxisVisual()) + this->ParentAxisVisual()->SetLocalScale(this->scaleToChild); + } + } + + ////////////////////////////////////////////////// + template + void BaseJointVisual::SetType(const JointVisualType _type) + { + this->jointVisualType = _type; + this->dirtyJointType = true; + } + + ////////////////////////////////////////////////// + template + ignition::math::Vector3d BaseJointVisual::Axis() const + { + return this->axis; + } + + ////////////////////////////////////////////////// + template + ignition::math::Vector3d BaseJointVisual::ParentAxis() const + { + return this->parentAxis; + } + + ////////////////////////////////////////////////// + template + JointVisualType BaseJointVisual::Type() const + { + return this->jointVisualType; + } + + ////////////////////////////////////////////////// + template + JointVisualPtr BaseJointVisual::ParentAxisVisual() const + { + return this->parentAxisVis; + } + + ////////////////////////////////////////////////// + template + ArrowVisualPtr BaseJointVisual::ArrowVisual() const + { + return this->arrowVisual; + } + + ////////////////////////////////////////////////// + template + void BaseJointVisual::SetVisible(bool _visible) + { + T::SetVisible(_visible); + + if (this->ArrowVisual()) + this->ArrowVisual()->SetVisible(_visible); + + if (this->Type() == JointVisualType::JVT_REVOLUTE2 || + this->Type() == JointVisualType::JVT_UNIVERSAL) + { + if (this->ParentAxisVisual()) + this->ParentAxisVisual()->SetVisible(_visible); + } + + if (this->axisVisual) + this->axisVisual->SetVisible(_visible); + } + } + } +} +#endif diff --git a/include/ignition/rendering/base/BaseNode.hh b/include/ignition/rendering/base/BaseNode.hh index 84b37b33f..65a642364 100644 --- a/include/ignition/rendering/base/BaseNode.hh +++ b/include/ignition/rendering/base/BaseNode.hh @@ -48,6 +48,9 @@ namespace ignition public: virtual math::Pose3d LocalPose() const override; + // Documentation inherited + public: virtual math::Pose3d InitialLocalPose() const override; + public: virtual void SetLocalPose(const math::Pose3d &_pose) override; public: virtual void SetLocalPosition(double _x, double _y, double _z) @@ -196,6 +199,14 @@ namespace ignition protected: math::Vector3d origin; + /// \brief Flag to indicate whether initial local pose + /// is set for this node. + protected: bool initialLocalPoseSet = false; + + /// \brief Initial local pose for this node. + protected: ignition::math::Pose3d initialLocalPose = + ignition::math::Pose3d::Zero; + /// \brief A map of custom key value data protected: std::map userData; }; @@ -332,9 +343,22 @@ namespace ignition return; } + if (!initialLocalPoseSet) + { + this->initialLocalPose = pose; + this->initialLocalPoseSet = true; + } + this->SetRawLocalPose(pose); } + ////////////////////////////////////////////////// + template + math::Pose3d BaseNode::InitialLocalPose() const + { + return this->initialLocalPose; + } + ////////////////////////////////////////////////// template math::Vector3d BaseNode::LocalPosition() const diff --git a/include/ignition/rendering/base/BaseScene.hh b/include/ignition/rendering/base/BaseScene.hh index 56ad9569e..f4520329b 100644 --- a/include/ignition/rendering/base/BaseScene.hh +++ b/include/ignition/rendering/base/BaseScene.hh @@ -268,18 +268,28 @@ namespace ignition /// \brief Implementation for creating CoM visual. /// \param[in] _id Unique id /// \param[in] _name Name of CoM visual + /// \return Pointer to a CoM visual object protected: virtual COMVisualPtr CreateCOMVisualImpl(unsigned int _id, const std::string &_name) = 0; /// \brief Implementation for creating Inertia visual. /// \param[in] _id Unique id /// \param[in] _name Name of inertia visual + /// \return Pointer to a inertia visual object protected: virtual InertiaVisualPtr CreateInertiaVisualImpl( unsigned int _id, const std::string &_name) = 0; + /// \brief Implementation for creating Joint visual. + /// \param[in] _id Unique id + /// \param[in] _name Name of Joint visual + /// \return Pointer to a joint visual object + protected: virtual JointVisualPtr CreateJointVisualImpl(unsigned int _id, + const std::string &_name) = 0; + /// \brief Implementation for creating Light visual. /// \param[in] _id Unique id /// \param[in] _name Name of light visual + /// \return Pointer to a light visual object protected: virtual LightVisualPtr CreateLightVisualImpl(unsigned int _id, const std::string &_name) = 0; @@ -401,6 +411,21 @@ namespace ignition public: virtual InertiaVisualPtr CreateInertiaVisual(unsigned int _id, const std::string &_name) override; + // Documentation inherited + public: virtual JointVisualPtr CreateJointVisual() override; + + // Documentation inherited + public: virtual JointVisualPtr CreateJointVisual(unsigned int _id) + override; + + // Documentation inherited + public: virtual JointVisualPtr CreateJointVisual( + const std::string &_name) override; + + // Documentation inherited + public: virtual JointVisualPtr CreateJointVisual(unsigned int _id, + const std::string &_name) override; + // Documentation inherited public: virtual LightVisualPtr CreateLightVisual() override; diff --git a/ogre/include/ignition/rendering/ogre/OgreJointVisual.hh b/ogre/include/ignition/rendering/ogre/OgreJointVisual.hh new file mode 100644 index 000000000..fb57497cc --- /dev/null +++ b/ogre/include/ignition/rendering/ogre/OgreJointVisual.hh @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2021 Open Source Robotics 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. + * + */ +#ifndef IGNITION_RENDERING_OGRE_OGREJOINTVISUAL_HH_ +#define IGNITION_RENDERING_OGRE_OGREJOINTVISUAL_HH_ + +#include "ignition/rendering/base/BaseJointVisual.hh" +#include "ignition/rendering/ogre/OgreVisual.hh" + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + // + class IGNITION_RENDERING_OGRE_VISIBLE OgreJointVisual : + public BaseJointVisual + { + /// \brief Constructor + protected: OgreJointVisual(); + + /// \brief Destructor + public: virtual ~OgreJointVisual(); + + /// \brief Only scene can instantiate this class + private: friend class OgreScene; + }; + } + } +} +#endif diff --git a/ogre/include/ignition/rendering/ogre/OgreScene.hh b/ogre/include/ignition/rendering/ogre/OgreScene.hh index 59d09faa3..81278e160 100644 --- a/ogre/include/ignition/rendering/ogre/OgreScene.hh +++ b/ogre/include/ignition/rendering/ogre/OgreScene.hh @@ -86,6 +86,10 @@ namespace ignition protected: virtual InertiaVisualPtr CreateInertiaVisualImpl( unsigned int _id, const std::string &_name) override; + // Documentation inherited + protected: virtual JointVisualPtr CreateJointVisualImpl(unsigned int _id, + const std::string &_name) override; + protected: virtual LightVisualPtr CreateLightVisualImpl(unsigned int _id, const std::string &_name) override; diff --git a/ogre/src/OgreJointVisual.cc b/ogre/src/OgreJointVisual.cc new file mode 100644 index 000000000..981fa8e35 --- /dev/null +++ b/ogre/src/OgreJointVisual.cc @@ -0,0 +1,30 @@ +/* + * Copyright (C) 2021 Open Source Robotics 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. + * + */ +#include "ignition/rendering/ogre/OgreJointVisual.hh" + +using namespace ignition; +using namespace rendering; + +////////////////////////////////////////////////// +OgreJointVisual::OgreJointVisual() +{ +} + +////////////////////////////////////////////////// +OgreJointVisual::~OgreJointVisual() +{ +} diff --git a/ogre/src/OgreScene.cc b/ogre/src/OgreScene.cc index caddb476d..54e046fde 100644 --- a/ogre/src/OgreScene.cc +++ b/ogre/src/OgreScene.cc @@ -31,6 +31,7 @@ #include "ignition/rendering/ogre/OgreHeightmap.hh" #include "ignition/rendering/ogre/OgreIncludes.hh" #include "ignition/rendering/ogre/OgreInertiaVisual.hh" +#include "ignition/rendering/ogre/OgreJointVisual.hh" #include "ignition/rendering/ogre/OgreLidarVisual.hh" #include "ignition/rendering/ogre/OgreLightVisual.hh" #include "ignition/rendering/ogre/OgreMarker.hh" @@ -394,6 +395,15 @@ InertiaVisualPtr OgreScene::CreateInertiaVisualImpl(unsigned int _id, return (result) ? visual : nullptr; } +////////////////////////////////////////////////// +JointVisualPtr OgreScene::CreateJointVisualImpl(unsigned int _id, + const std::string &_name) +{ + OgreJointVisualPtr visual(new OgreJointVisual); + bool result = this->InitObject(visual, _id, _name); + return (result) ? visual : nullptr; +} + ////////////////////////////////////////////////// LightVisualPtr OgreScene::CreateLightVisualImpl(unsigned int _id, const std::string &_name) diff --git a/ogre2/include/ignition/rendering/ogre2/Ogre2JointVisual.hh b/ogre2/include/ignition/rendering/ogre2/Ogre2JointVisual.hh new file mode 100644 index 000000000..4a7879384 --- /dev/null +++ b/ogre2/include/ignition/rendering/ogre2/Ogre2JointVisual.hh @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2021 Open Source Robotics 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. + * + */ +#ifndef IGNITION_RENDERING_OGRE2_OGRE2JOINTVISUAL_HH_ +#define IGNITION_RENDERING_OGRE2_OGRE2JOINTVISUAL_HH_ + +#include "ignition/rendering/base/BaseJointVisual.hh" +#include "ignition/rendering/ogre2/Ogre2Visual.hh" + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + // + class IGNITION_RENDERING_OGRE2_VISIBLE Ogre2JointVisual : + public BaseJointVisual + { + /// \brief Constructor + protected: Ogre2JointVisual(); + + /// \brief Destructor + public: virtual ~Ogre2JointVisual(); + + /// \brief Only scene can instantiate this class + private: friend class Ogre2Scene; + }; + } + } +} +#endif diff --git a/ogre2/include/ignition/rendering/ogre2/Ogre2RenderTypes.hh b/ogre2/include/ignition/rendering/ogre2/Ogre2RenderTypes.hh index a10f56586..64048814f 100644 --- a/ogre2/include/ignition/rendering/ogre2/Ogre2RenderTypes.hh +++ b/ogre2/include/ignition/rendering/ogre2/Ogre2RenderTypes.hh @@ -40,6 +40,7 @@ namespace ignition class Ogre2GpuRays; class Ogre2Grid; class Ogre2InertiaVisual; + class Ogre2JointVisual; class Ogre2Light; class Ogre2LightVisual; class Ogre2LidarVisual; @@ -87,6 +88,7 @@ namespace ignition typedef shared_ptr Ogre2GpuRaysPtr; typedef shared_ptr Ogre2GridPtr; typedef shared_ptr Ogre2InertiaVisualPtr; + typedef shared_ptr Ogre2JointVisualPtr; typedef shared_ptr Ogre2LightPtr; typedef shared_ptr Ogre2LightVisualPtr; typedef shared_ptr Ogre2LidarVisualPtr; diff --git a/ogre2/include/ignition/rendering/ogre2/Ogre2Scene.hh b/ogre2/include/ignition/rendering/ogre2/Ogre2Scene.hh index cb438fff4..fb1c854b0 100644 --- a/ogre2/include/ignition/rendering/ogre2/Ogre2Scene.hh +++ b/ogre2/include/ignition/rendering/ogre2/Ogre2Scene.hh @@ -193,9 +193,14 @@ namespace ignition protected: virtual COMVisualPtr CreateCOMVisualImpl(unsigned int _id, const std::string &_name) override; + // Documentation inherited protected: virtual InertiaVisualPtr CreateInertiaVisualImpl( unsigned int _id, const std::string &_name) override; + // Documentation inherited + protected: virtual JointVisualPtr CreateJointVisualImpl(unsigned int _id, + const std::string &_name) override; + // Documentation inherited protected: virtual LightVisualPtr CreateLightVisualImpl(unsigned int _id, const std::string &_name) override; diff --git a/ogre2/src/Ogre2JointVisual.cc b/ogre2/src/Ogre2JointVisual.cc new file mode 100644 index 000000000..a47c8fed7 --- /dev/null +++ b/ogre2/src/Ogre2JointVisual.cc @@ -0,0 +1,30 @@ +/* + * Copyright (C) 2021 Open Source Robotics 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. + * + */ +#include "ignition/rendering/ogre2/Ogre2JointVisual.hh" + +using namespace ignition; +using namespace rendering; + +////////////////////////////////////////////////// +Ogre2JointVisual::Ogre2JointVisual() +{ +} + +////////////////////////////////////////////////// +Ogre2JointVisual::~Ogre2JointVisual() +{ +} diff --git a/ogre2/src/Ogre2Scene.cc b/ogre2/src/Ogre2Scene.cc index 9209f0d28..4100b61a5 100644 --- a/ogre2/src/Ogre2Scene.cc +++ b/ogre2/src/Ogre2Scene.cc @@ -29,6 +29,7 @@ #include "ignition/rendering/ogre2/Ogre2GpuRays.hh" #include "ignition/rendering/ogre2/Ogre2Grid.hh" #include "ignition/rendering/ogre2/Ogre2InertiaVisual.hh" +#include "ignition/rendering/ogre2/Ogre2JointVisual.hh" #include "ignition/rendering/ogre2/Ogre2Light.hh" #include "ignition/rendering/ogre2/Ogre2LightVisual.hh" #include "ignition/rendering/ogre2/Ogre2LidarVisual.hh" @@ -950,6 +951,15 @@ InertiaVisualPtr Ogre2Scene::CreateInertiaVisualImpl(unsigned int _id, return (result) ? visual : nullptr; } +////////////////////////////////////////////////// +JointVisualPtr Ogre2Scene::CreateJointVisualImpl(unsigned int _id, + const std::string &_name) +{ + Ogre2JointVisualPtr visual(new Ogre2JointVisual); + bool result = this->InitObject(visual, _id, _name); + return (result) ? visual : nullptr; +} + ////////////////////////////////////////////////// LightVisualPtr Ogre2Scene::CreateLightVisualImpl(unsigned int _id, const std::string &_name) diff --git a/src/ArrowVisual_TEST.cc b/src/ArrowVisual_TEST.cc index 623493b52..0be3c0854 100644 --- a/src/ArrowVisual_TEST.cc +++ b/src/ArrowVisual_TEST.cc @@ -64,20 +64,32 @@ void ArrowVisualTest::ArrowVisual(const std::string &_renderEngine) EXPECT_EQ(math::Vector3d(0.2, 0.3, 0.4), visual->LocalScale()); // check children and geometry - EXPECT_EQ(2u, visual->ChildCount()); + EXPECT_EQ(3u, visual->ChildCount()); NodePtr node = visual->ChildByIndex(0u); VisualPtr child = std::dynamic_pointer_cast(node); ASSERT_NE(nullptr, child); EXPECT_EQ(1u, child->GeometryCount()); - EXPECT_EQ(node, visual->Shaft()); + EXPECT_EQ(node, visual->Rotation()); node = visual->ChildByIndex(1u); child = std::dynamic_pointer_cast(node); ASSERT_NE(nullptr, child); EXPECT_EQ(1u, child->GeometryCount()); + EXPECT_EQ(node, visual->Shaft()); + + node = visual->ChildByIndex(2u); + child = std::dynamic_pointer_cast(node); + ASSERT_NE(nullptr, child); + EXPECT_EQ(1u, child->GeometryCount()); EXPECT_EQ(node, visual->Head()); + // test destroy + ArrowVisualPtr visual2 = scene->CreateArrowVisual(); + ASSERT_NE(nullptr, visual2); + visual2->Destroy(); + EXPECT_EQ(0u, visual2->ChildCount()); + // Clean up engine->DestroyScene(scene); rendering::unloadEngine(engine->Name()); diff --git a/src/AxisVisual_TEST.cc b/src/AxisVisual_TEST.cc index 5e89e5dd3..717bb87de 100644 --- a/src/AxisVisual_TEST.cc +++ b/src/AxisVisual_TEST.cc @@ -73,7 +73,7 @@ void AxisVisualTest::AxisVisual(const std::string &_renderEngine) ArrowVisualPtr arrow = std::dynamic_pointer_cast(node); ASSERT_NE(nullptr, arrow); - EXPECT_EQ(2u, arrow->ChildCount()); + EXPECT_EQ(3u, arrow->ChildCount()); NodePtr childNode = arrow->ChildByIndex(0u); VisualPtr child = std::dynamic_pointer_cast(childNode); EXPECT_EQ(1u, child->GeometryCount()); @@ -81,6 +81,10 @@ void AxisVisualTest::AxisVisual(const std::string &_renderEngine) childNode = arrow->ChildByIndex(1u); child = std::dynamic_pointer_cast(childNode); EXPECT_EQ(1u, child->GeometryCount()); + + childNode = arrow->ChildByIndex(2u); + child = std::dynamic_pointer_cast(childNode); + EXPECT_EQ(1u, child->GeometryCount()); } // Clean up diff --git a/src/JointVisual_TEST.cc b/src/JointVisual_TEST.cc new file mode 100644 index 000000000..0a3757cec --- /dev/null +++ b/src/JointVisual_TEST.cc @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2021 Open Source Robotics 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. + * +*/ + +#include +#include + +#include + +#include "test_config.h" // NOLINT(build/include) + +#include "ignition/rendering/JointVisual.hh" +#include "ignition/rendering/RenderEngine.hh" +#include "ignition/rendering/RenderingIface.hh" +#include "ignition/rendering/Scene.hh" + +using namespace ignition; +using namespace rendering; + +class JointVisualTest : public testing::Test, + public testing::WithParamInterface +{ + /// \brief Test basic API + public: void JointVisual(const std::string &_renderEngine); +}; + +///////////////////////////////////////////////// +void JointVisualTest::JointVisual(const std::string &_renderEngine) +{ + RenderEngine *engine = rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + ScenePtr scene = engine->CreateScene("scene"); + + // create visual + JointVisualPtr jointVisual = scene->CreateJointVisual(); + ASSERT_NE(nullptr, jointVisual); + + // create joint child visual + VisualPtr jointChildVisual = scene->CreateVisual("joint_child"); + + // create joint parent visual + VisualPtr jointParentVisual = scene->CreateVisual("joint_parent"); + + // check initial values + EXPECT_EQ(JointVisualType::JVT_NONE, jointVisual->Type()); + EXPECT_EQ(nullptr, jointVisual->ArrowVisual()); + EXPECT_EQ(nullptr, jointVisual->ParentAxisVisual()); + EXPECT_EQ(math::Vector3d::Zero, jointVisual->Axis()); + EXPECT_EQ(math::Vector3d::Zero, jointVisual->ParentAxis()); + + // set joint type + jointVisual->SetType(JointVisualType::JVT_REVOLUTE2); + EXPECT_EQ(JointVisualType::JVT_REVOLUTE2, jointVisual->Type()); + + // set child axis + math::Vector3d axis2(0.0, 1.0, 0.0); + bool useParentFrame = false; + jointChildVisual->AddChild(jointVisual); + jointVisual->SetAxis(axis2, useParentFrame); + jointVisual->PreRender(); + EXPECT_NE(nullptr, jointVisual->ArrowVisual()); + EXPECT_EQ(axis2, jointVisual->Axis()); + EXPECT_EQ(math::Vector3d::Zero, jointVisual->ParentAxis()); + EXPECT_EQ(nullptr, jointVisual->ParentAxisVisual()); + + // set parent axis + math::Vector3d axis1(0.0, 1.0, 0.0); + useParentFrame = true; + jointVisual->SetParentAxis(axis1, "joint_parent", true); + jointVisual->PreRender(); + EXPECT_NE(nullptr, jointVisual->ArrowVisual()); + EXPECT_EQ(axis2, jointVisual->Axis()); + EXPECT_EQ(axis1, jointVisual->ParentAxis()); + EXPECT_NE(nullptr, jointVisual->ParentAxisVisual()); + + // set visibility + jointVisual->SetVisible(false); + jointVisual->SetVisible(true); + + // Clean up + engine->DestroyScene(scene); + rendering::unloadEngine(engine->Name()); +} + +///////////////////////////////////////////////// +TEST_P(JointVisualTest, JointVisual) +{ + JointVisual(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(Visual, JointVisualTest, + RENDER_ENGINE_VALUES, + ignition::rendering::PrintToStringParam()); + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/base/BaseScene.cc b/src/base/BaseScene.cc index c24d0ed8c..08650db21 100644 --- a/src/base/BaseScene.cc +++ b/src/base/BaseScene.cc @@ -29,6 +29,7 @@ #include "ignition/rendering/AxisVisual.hh" #include "ignition/rendering/COMVisual.hh" #include "ignition/rendering/InertiaVisual.hh" +#include "ignition/rendering/JointVisual.hh" #include "ignition/rendering/LidarVisual.hh" #include "ignition/rendering/LightVisual.hh" #include "ignition/rendering/Camera.hh" @@ -979,6 +980,36 @@ InertiaVisualPtr BaseScene::CreateInertiaVisual(unsigned int _id, return (result) ? visual : nullptr; } +////////////////////////////////////////////////// +JointVisualPtr BaseScene::CreateJointVisual() +{ + unsigned int objId = this->CreateObjectId(); + return this->CreateJointVisual(objId); +} + +////////////////////////////////////////////////// +JointVisualPtr BaseScene::CreateJointVisual(unsigned int _id) +{ + std::string objName = this->CreateObjectName(_id, "JointVisual"); + return this->CreateJointVisual(_id, objName); +} + +////////////////////////////////////////////////// +JointVisualPtr BaseScene::CreateJointVisual(const std::string &_name) +{ + unsigned int objId = this->CreateObjectId(); + return this->CreateJointVisual(objId, _name); +} + +////////////////////////////////////////////////// +JointVisualPtr BaseScene::CreateJointVisual(unsigned int _id, + const std::string &_name) +{ + JointVisualPtr visual = this->CreateJointVisualImpl(_id, _name); + bool result = this->RegisterVisual(visual); + return (result) ? visual : nullptr; +} + ////////////////////////////////////////////////// LightVisualPtr BaseScene::CreateLightVisual() {