Program Listing for File mapped_covariance_visual.hpp

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

/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2017, Ellon Paiva Mendes @ LAAS-CNRS
 * 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__MAPPED_COVARIANCE_VISUAL_HPP_
#define FUSE_VIZ__MAPPED_COVARIANCE_VISUAL_HPP_

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

#include <Eigen/Dense>
#include <cmath>

#include <rviz_rendering/objects/object.hpp>
#include <rviz_rendering/objects/shape.hpp>

#include <geometry_msgs/msg/pose_with_covariance.hpp>


namespace Ogre
{
class SceneManager;
class SceneNode;
class Any;
}  // namespace Ogre

namespace Eigen
{
typedef Matrix<double, 6, 6> Matrix6d;
}

namespace fuse_viz
{

class MappedCovarianceVisual : public rviz_rendering::Object
{
public:
  enum ShapeIndex
  {
    kRoll = 0,
    kPitch = 1,
    kYaw = 2,
    kYaw2D = 3,
    kNumOriShapes
  };

private:
  MappedCovarianceVisual(
    Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, bool is_local_rotation,
    bool is_visible = true, float pos_scale = 1.0f, float ori_scale = 0.1f,
    float ori_offset = 0.1f);

public:
  ~MappedCovarianceVisual() override;

  void setScales(float pos_scale, float ori_scale);
  void setPositionScale(float pos_scale);
  void setOrientationOffset(float ori_offset);
  void setOrientationScale(float ori_scale);

  virtual void setPositionColor(float r, float g, float b, float a);
  void setPositionColor(const Ogre::ColourValue & color);

  virtual void setOrientationColor(float r, float g, float b, float a);
  void setOrientationColor(const Ogre::ColourValue & color);
  void setOrientationColorToRGB(float a);

  virtual void setCovariance(const geometry_msgs::msg::PoseWithCovariance & pose);

  virtual const Ogre::Vector3 & getPositionCovarianceScale();
  virtual const Ogre::Quaternion & getPositionCovarianceOrientation();

  Ogre::SceneNode * getPositionSceneNode()
  {
    return position_scale_node_;
  }

  Ogre::SceneNode * getOrientationSceneNode()
  {
    return orientation_root_node_;
  }

  rviz_rendering::Shape * getPositionShape()
  {
    return position_shape_;
  }

  rviz_rendering::Shape * getOrientationShape(ShapeIndex index);

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

  virtual void setVisible(bool visible);

  virtual void setPositionVisible(bool visible);

  virtual void setOrientationVisible(bool visible);

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

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

  virtual void setRotatingFrame(bool use_rotating_frame);

private:
  void updatePosition(const Eigen::Matrix6d & covariance);
  void updateOrientation(const Eigen::Matrix6d & covariance, ShapeIndex index);
  void updateOrientationVisibility();

  Ogre::SceneNode * root_node_ = nullptr;
  Ogre::SceneNode * fixed_orientation_node_ = nullptr;
  Ogre::SceneNode * position_scale_node_ = nullptr;
  Ogre::SceneNode * position_node_ = nullptr;

  Ogre::SceneNode * orientation_root_node_ = nullptr;
  Ogre::SceneNode * orientation_offset_node_[kNumOriShapes];

  rviz_rendering::Shape * position_shape_;
  rviz_rendering::Shape * orientation_shape_[kNumOriShapes];

  bool local_rotation_;

  bool pose_2d_;

  bool orientation_visible_;

  Ogre::Vector3 current_ori_scale_[kNumOriShapes];
  float current_ori_scale_factor_;

  static const float max_degrees;

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

  // Make MappedCovarianceProperty friend class so it create MappedCovarianceVisual objects
  friend class MappedCovarianceProperty;
};

}  // namespace fuse_viz

#endif  // FUSE_VIZ__MAPPED_COVARIANCE_VISUAL_HPP_