Program Listing for File covariance_visual.hpp

↰ Return to documentation for file (/tmp/ws/src/rviz/rviz_rendering/include/rviz_rendering/objects/covariance_visual.hpp)

/*
 * Copyright (c) 2017, Ellon Paiva Mendes @ LAAS-CNRS
 * Copyright (c) 2018, Bosch Software Innovations GmbH.
 * 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 holders 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 RVIZ_RENDERING__OBJECTS__COVARIANCE_VISUAL_HPP_
#define RVIZ_RENDERING__OBJECTS__COVARIANCE_VISUAL_HPP_

#include <array>
#include <memory>
#include <sstream>  // needed for Eigen until https://gitlab.com/libeigen/eigen/-/merge_requests/65
#include <vector>

// GCC 11 has a false positive warning about uninitialized variables in Eigen.  There is an open
// issue about it at https://gitlab.com/libeigen/eigen/-/issues/2304 .  Just disable the warning
// for Eigen for now.
// Also Version 3.4.0 of Eigen in Ubuntu 22.04 has a bug that causes -Wclass-memaccess warnings on
// aarch64.  Upstream Eigen has already fixed this in
// https://gitlab.com/libeigen/eigen/-/merge_requests/645 .  The Debian fix for this is in
// https://salsa.debian.org/science-team/eigen3/-/merge_requests/1 .
// However, it is not clear that that fix is going to make it into Ubuntu 22.04 before it
// freezes, so disable the warning here.
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#pragma GCC diagnostic ignored "-Wclass-memaccess"
#endif
#include <Eigen/Dense>  // NOLINT: cpplint cannot handle correct include here
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif

#include <OgreVector3.h>
#include <OgreColourValue.h>

#include "rviz_rendering/objects/shape.hpp"
#include "rviz_rendering/visibility_control.hpp"

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

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

namespace rviz_rendering
{

enum RVIZ_RENDERING_PUBLIC Frame
{
  Local,
  Fixed,
};

enum RVIZ_RENDERING_PUBLIC ColorStyle
{
  Unique,
  RGB,
};

struct RVIZ_RENDERING_PUBLIC CovarianceUserData
{
  bool visible;

  bool position_visible;
  Ogre::ColourValue position_color;
  float position_scale;

  bool orientation_visible;
  Frame orientation_frame;
  ColorStyle orientation_color_style;
  Ogre::ColourValue orientation_color;
  float orientation_offset;
  float orientation_scale;
};

class CovarianceVisual
{
public:
  enum ShapeIndex
  {
    kRoll = 0,
    kPitch = 1,
    kYaw = 2,
    kYaw2D = 3,
    kNumOrientationShapes
  };

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

public:
  RVIZ_RENDERING_PUBLIC
  virtual ~CovarianceVisual();

  RVIZ_RENDERING_PUBLIC
  void updateUserData(CovarianceUserData user_data);

  RVIZ_RENDERING_PUBLIC
  virtual void setCovariance(
    const Ogre::Quaternion & pose_orientation,
    const std::array<double, 36> & covariances);

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

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

  RVIZ_RENDERING_PUBLIC
  virtual void setVisible(bool visible);

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

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

  RVIZ_RENDERING_PUBLIC
  Ogre::AxisAlignedBox getPositionBoundingBox();

  RVIZ_RENDERING_PUBLIC
  std::vector<Ogre::AxisAlignedBox> getOrientationBoundingBoxes();

private:
  void setScales(float pos_scale, float ori_scale);

  void setPositionScale(float pos_scale);

  void setOrientationOffset(float ori_offset);

  void setOrientationScale(float ori_scale);

  void setPositionColor(const Ogre::ColourValue & color);

  void setOrientationColor(const Ogre::ColourValue & color);

  void setOrientationColorToRGB(float alpha);

  std::shared_ptr<rviz_rendering::Shape> getPositionShape() {return position_shape_;}

  virtual void setPositionVisible(bool visible);

  virtual void setOrientationVisible(bool visible);

  virtual void setRotatingFrame(bool use_rotating_frame);

  void updatePosition(const Eigen::Matrix6d & covariance);

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

  void updateOrientationVisibility();

  Ogre::SceneManager * scene_manager_;

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

  Ogre::SceneNode * orientation_root_node_;
  std::array<Ogre::SceneNode *, kNumOrientationShapes> orientation_offset_nodes_;

  std::shared_ptr<rviz_rendering::Shape> position_shape_;
  std::array<std::shared_ptr<rviz_rendering::Shape>, kNumOrientationShapes> orientation_shapes_;

  bool local_rotation_;

  bool pose_2d_;

  bool orientation_visible_;

  std::array<Ogre::Vector3, kNumOrientationShapes> current_orientation_scales_;
  float current_orientation_scale_factor_;

  static constexpr float kMaxDegrees = 89.0f;
};

}  // namespace rviz_rendering

#endif  // RVIZ_RENDERING__OBJECTS__COVARIANCE_VISUAL_HPP_