Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <OGRE/OgreVector3.h>
00033 #include <OGRE/OgreSceneNode.h>
00034 #include <OGRE/OgreSceneManager.h>
00035 #include <OGRE/OgreEntity.h>
00036
00037 #include "rviz/ogre_helpers/axes.h"
00038
00039 #include "marker_with_covariance/marker_with_covariance_visual.h"
00040
00041 namespace marker_rviz_plugin {
00042
00043 MarkerWithCovarianceVisual::MarkerWithCovarianceVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node) {
00044 scene_manager_ = scene_manager;
00045 frame_node_ = parent_node->createChildSceneNode();
00046
00047 _showAxes = true;
00048 _showMarker = true;
00049 _showLabel = true;
00050 _scale = 1;
00051 }
00052
00053 MarkerWithCovarianceVisual::~MarkerWithCovarianceVisual() {
00054
00055 scene_manager_->destroySceneNode(frame_node_);
00056 }
00057
00058 void MarkerWithCovarianceVisual::setMessage(const marker_msgs::MarkerWithCovarianceStamped::ConstPtr &msg) {
00059 marker_msgs::MarkerWithCovariance marker_cov = msg->marker;
00060 marker_msgs::Marker marker = marker_cov.marker;
00061
00062 double p_x = marker.pose.position.x;
00063 double p_y = marker.pose.position.y;
00064 double p_z = marker.pose.position.z;
00065 double o_x = marker.pose.orientation.x;
00066 double o_y = marker.pose.orientation.y;
00067 double o_z = marker.pose.orientation.z;
00068 double o_w = marker.pose.orientation.w;
00069
00070 int id = -1;
00071 if (marker.ids.size() > 0)
00072 id = marker.ids[0];
00073
00074 MarkerWithCovariance *m = new MarkerWithCovariance(scene_manager_, frame_node_, id);
00075 m->setPosition(Ogre::Vector3(p_x, p_y, p_z));
00076 m->setOrientation(Ogre::Quaternion(o_w, o_x, o_y, o_z));
00077 m->setShowMarker(_showMarker);
00078 m->setShowAxes(_showAxes);
00079 m->setShowLabel(_showLabel);
00080 m->setScale(Ogre::Vector3(_scale, _scale, _scale));
00081 m->setCovarianceMatrix(msg->marker.covariance);
00082 _marker.reset(m);
00083 }
00084
00085 void MarkerWithCovarianceVisual::setFramePosition(const Ogre::Vector3 &position) {
00086 frame_node_->setPosition(position);
00087 }
00088
00089 void MarkerWithCovarianceVisual::setFrameOrientation(const Ogre::Quaternion &orientation) {
00090 frame_node_->setOrientation(orientation);
00091 }
00092
00093 void MarkerWithCovarianceVisual::setShowAxes(bool showAxes) {
00094 _marker->setShowAxes(showAxes);
00095 _showAxes = showAxes;
00096 }
00097
00098 void MarkerWithCovarianceVisual::setShowMarker(bool showMarker) {
00099 _marker->setShowMarker(showMarker);
00100 _showMarker = showMarker;
00101 }
00102
00103 void MarkerWithCovarianceVisual::setShowLabel(bool showLabel) {
00104 _marker->setShowLabel(showLabel);
00105 _showLabel = showLabel;
00106 }
00107
00108 void MarkerWithCovarianceVisual::setScale(float scale) {
00109 _marker->setScale(Ogre::Vector3(scale, scale, scale));
00110 _scale = scale;
00111 }
00112
00113 }