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 <ros/ros.h>
00033
00034 #include <OGRE/OgreVector3.h>
00035 #include <OGRE/OgreMatrix3.h>
00036 #include <OGRE/OgreSceneNode.h>
00037 #include <OGRE/OgreSceneManager.h>
00038
00039 #include "marker_with_covariance_array/marker_with_covariance_array_visual.h"
00040
00041 namespace marker_rviz_plugin {
00042
00043 MarkerWithCovarianceArrayVisual::MarkerWithCovarianceArrayVisual(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 MarkerWithCovarianceArrayVisual::~MarkerWithCovarianceArrayVisual() {
00054
00055 scene_manager_->destroySceneNode(frame_node_);
00056 }
00057
00058 void MarkerWithCovarianceArrayVisual::setMessage(const marker_msgs::MarkerWithCovarianceArray::ConstPtr &msg) {
00059 _markers.resize(msg->markers.size());
00060
00061 for (size_t i = 0; i < msg->markers.size(); i++) {
00062 marker_msgs::MarkerWithCovariance marker_cov = msg->markers[i];
00063 marker_msgs::Marker marker = marker_cov.marker;
00064
00065 double p_x = marker.pose.position.x;
00066 double p_y = marker.pose.position.y;
00067 double p_z = marker.pose.position.z;
00068 double o_x = marker.pose.orientation.x;
00069 double o_y = marker.pose.orientation.y;
00070 double o_z = marker.pose.orientation.z;
00071 double o_w = marker.pose.orientation.w;
00072
00073 int id = -1;
00074 if (marker.ids.size() > 0)
00075 id = marker.ids[0];
00076
00077 MarkerWithCovariance *m = new MarkerWithCovariance(scene_manager_, frame_node_, id);
00078 m->setPosition(Ogre::Vector3(p_x, p_y, p_z));
00079 m->setOrientation(Ogre::Quaternion(o_w, o_x, o_y, o_z));
00080 m->setShowMarker(_showMarker);
00081 m->setShowAxes(_showAxes);
00082 m->setShowLabel(_showLabel);
00083 m->setScale(Ogre::Vector3(_scale, _scale, _scale));
00084 m->setCovarianceMatrix(msg->markers[i].covariance);
00085 _markers[i].reset(m);
00086 }
00087 }
00088
00089 void MarkerWithCovarianceArrayVisual::setFramePosition(const Ogre::Vector3 &position) {
00090 frame_node_->setPosition(position);
00091 }
00092
00093 void MarkerWithCovarianceArrayVisual::setFrameOrientation(const Ogre::Quaternion &orientation) {
00094 frame_node_->setOrientation(orientation);
00095 }
00096
00097 void MarkerWithCovarianceArrayVisual::setShowAxes(bool showAxes) {
00098 for (size_t i = 0; i < _markers.size(); i++) {
00099 _markers[i]->setShowAxes(showAxes);
00100 }
00101
00102 _showAxes = showAxes;
00103 }
00104
00105 void MarkerWithCovarianceArrayVisual::setShowMarker(bool showMarker) {
00106 for (size_t i = 0; i < _markers.size(); i++) {
00107 _markers[i]->setShowMarker(showMarker);
00108 }
00109
00110 _showMarker = showMarker;
00111 }
00112
00113 void MarkerWithCovarianceArrayVisual::setShowLabel(bool showLabel) {
00114 for (size_t i = 0; i < _markers.size(); i++) {
00115 _markers[i]->setShowLabel(showLabel);
00116 }
00117
00118 _showLabel = showLabel;
00119 }
00120
00121 void MarkerWithCovarianceArrayVisual::setScale(float scale) {
00122 for (size_t i = 0; i < _markers.size(); i++) {
00123 _markers[i]->setScale(Ogre::Vector3(scale, scale, scale));
00124 }
00125
00126 _scale = scale;
00127 }
00128
00129 }