Program Listing for File relative_pose_2d_stamped_constraint_visual.hpp

↰ Return to documentation for file (/tmp/ws/src/fuse/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp)

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, Clearpath Robotics
 *  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 copyright holder 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 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.
 */

#ifndef FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_VISUAL_HPP_
#define FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_VISUAL_HPP_

#include <OgreColourValue.h>
#include <Ogre.h>

#include <memory>
#include <string>

#include <fuse_viz/mapped_covariance_property.hpp>

#include <rviz_rendering/objects/axes.hpp>
#include <rviz_rendering/objects/billboard_line.hpp>
#include <rviz_rendering/objects/movable_text.hpp>
#include <rviz_rendering/objects/object.hpp>


namespace Ogre
{

class SceneManager;
class SceneNode;
class Any;

}  // namespace Ogre

namespace fuse_core
{

class Graph;

}  // namespace fuse_core

namespace fuse_constraints
{

class RelativePose2DStampedConstraint;

}  // namespace fuse_constraints

namespace fuse_viz
{

using rviz_rendering::Axes;
using rviz_rendering::BillboardLine;
using rviz_rendering::MovableText;

class Pose2DStampedVisual;
class RelativePose2DStampedConstraintProperty;

class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object
{
private:
  RelativePose2DStampedConstraintVisual(
    Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node,
    const fuse_constraints::RelativePose2DStampedConstraint & constraint,
    const bool visible = true);

public:
  using CovarianceVisualPtr = MappedCovarianceProperty::MappedCovarianceVisualPtr;

  ~RelativePose2DStampedConstraintVisual() override;

  void setConstraint(
    const fuse_constraints::RelativePose2DStampedConstraint & constraint,
    const fuse_core::Graph & graph);

  Ogre::SceneNode * getSceneNode()
  {
    return root_node_;
  }

  void setUserData(const Ogre::Any & data) override;

  void setRelativePoseLineWidth(const float line_width);

  void setErrorLineWidth(const float line_width);

  void setLossMinBrightness(const float min_brightness);

  void setRelativePoseLineColor(const float r, const float g, const float b, const float a);

  void setErrorLineColor(const float r, const float g, const float b, const float a);

  void setRelativePoseAxesAlpha(const float alpha);

  void setRelativePoseAxesScale(const Ogre::Vector3 & scale);

  void setTextScale(const Ogre::Vector3 & scale);

  void setTextVisible(const bool visible);

  void setVisible(bool visible);

  void setPosition(const Ogre::Vector3 & position) override;

  void setOrientation(const Ogre::Quaternion & orientation) override;

  const CovarianceVisualPtr & getCovariance() const
  {
    return covariance_;
  }

  void setCovariance(const CovarianceVisualPtr & covariance)
  {
    covariance_ = covariance;
  }

  const std::string & getSource() const
  {
    return source_;
  }

private:
  Ogre::SceneNode * root_node_ = nullptr;
  Ogre::SceneNode * relative_pose_line_node_ = nullptr;
  Ogre::SceneNode * error_line_node_ = nullptr;
  Ogre::SceneNode * relative_pose_axes_node_ = nullptr;
  Ogre::SceneNode * text_node_ = nullptr;

  std::shared_ptr<BillboardLine> relative_pose_line_;
  std::shared_ptr<BillboardLine> error_line_;
  std::shared_ptr<Axes> relative_pose_axes_;
  MovableText * text_;
  CovarianceVisualPtr covariance_;
  std::string source_;

  float loss_scale_{-1.0};
  float min_brightness_{0.0};
  Ogre::ColourValue error_line_color_;

  bool visible_;

private:
  // Hide Object methods we don't want to expose
  // NOTE: Apparently we still need to define them...
  void setScale(const Ogre::Vector3 &) override {}
  void setColor(float, float, float, float) override {}
  const Ogre::Vector3 & getPosition() override;
  const Ogre::Quaternion & getOrientation() override;

  Ogre::ColourValue computeLossErrorLineColor(
    const Ogre::ColourValue & color,
    const float loss_scale);

  // Make RelativePose2DStampedConstraintProperty friend class so it create
  // RelativePose2DStampedConstraintVisual objects
  friend class RelativePose2DStampedConstraintProperty;
};

}  // namespace fuse_viz

#endif  // FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_VISUAL_HPP_