Skip to content
Snippets Groups Projects
Commit ef661027 authored by Antoine Lima's avatar Antoine Lima
Browse files

Removed old files

parent de6a7409
No related branches found
No related tags found
No related merge requests found
#include <OGRE/OgreVector3.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <rviz/ogre_helpers/arrow.h>
#include <geometry_msgs/Vector3.h>
#include "perception_visual.h"
namespace rviz_multiception
{
PerceptionVisual::PerceptionVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node)
{
scene_manager_ = scene_manager;
// Ogre::SceneNode s form a tree, with each node storing the
// transform (position and orientation) of itself relative to its
// parent. Ogre does the math of combining those transforms when it
// is time to render.
//
// Here we create a node to store the pose of the header frame
// relative to the RViz fixed frame.
frame_node_ = parent_node->createChildSceneNode();
// We create the arrow object within the frame node so that we can
// set its position and direction relative to its header frame.
pose_arrow_.reset(new rviz::Arrow(scene_manager_, frame_node_));
}
PerceptionVisual::~PerceptionVisual()
{
// Destroy the frame node since we don't need it anymore.
scene_manager_->destroySceneNode(frame_node_);
}
void PerceptionVisual::setMessage(const multiception::Perception* msg)
{
if(msg==nullptr) return;
// Convert the geometry_msgs::Vector3 to an Ogre::Vector3.
Ogre::Vector3 pose(msg->state.x, msg->state.y, 0);
pose_arrow_->setPosition(pose);
// Find the magnitude of the velocity vector.
float length = 2;//msg->state.v;
// Scale the arrow's thickness in each dimension along with its length.
Ogre::Vector3 scale(length, length, length);
pose_arrow_->setScale(scale);
// Set the orientation of the arrow according to the heading.
Ogre::Vector3 direction(std::cos(msg->state.yaw), std::sin(msg->state.yaw), 0);
pose_arrow_->setDirection(direction);
}
// Position and orientation are passed through to the SceneNode.
void PerceptionVisual::setFramePosition(const Ogre::Vector3& position)
{
frame_node_->setPosition(position);
}
void PerceptionVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
{
frame_node_->setOrientation(orientation);
}
// Color is passed through to the Arrow object.
void PerceptionVisual::setColor( float r, float g, float b, float a )
{
pose_arrow_->setColor(r, g, b, a);
}
} // namespace rviz_multiception
#ifndef PERCEPTION_VISUAL_H
#define PERCEPTION_VISUAL_H
#include <multiception/Perception.h>
namespace Ogre
{
class Vector3;
class Quaternion;
}
namespace rviz
{
class Arrow;
}
namespace rviz_multiception
{
// Declare the visual class for this display.
//
// Each instance of PerceptionVisual represents the visualization of a single
// multiception::Perception message. Currently it just shows an arrow with
// the direction and magnitude of the acceleration vector, but could
// easily be expanded to include more of the message data.
class PerceptionVisual
{
public:
// Constructor. Creates the visual stuff and puts it into the
// scene, but in an unconfigured state.
PerceptionVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node);
// Destructor. Removes the visual stuff from the scene.
virtual ~PerceptionVisual();
// Configure the visual to show the data in the message.
void setMessage(const multiception::Perception* msg);
// Set the pose of the coordinate frame the message refers to.
// These could be done inside setMessage(), but that would require
// calls to FrameManager and error handling inside setMessage(),
// which doesn't seem as clean. This way PerceptionVisual is only
// responsible for visualization.
void setFramePosition(const Ogre::Vector3& position);
void setFrameOrientation(const Ogre::Quaternion& orientation);
// Set the color and alpha of the visual, which are user-editable
// parameters and therefore don't come from the Imu message.
void setColor(float r, float g, float b, float a);
private:
// The object implementing the actual arrow shape
boost::shared_ptr<rviz::Arrow> pose_arrow_;
// A SceneNode whose pose is set to match the coordinate frame of
// the Imu message header.
Ogre::SceneNode* frame_node_;
// The SceneManager, kept here only so the destructor can ask it to
// destroy the ``frame_node_``.
Ogre::SceneManager* scene_manager_;
};
} // namespace rviz_multiception
#endif // PERCEPTION_VISUAL_H
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment