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/OgreManualObject.h>
00033 #include <OGRE/OgreEntity.h>
00034 #include <OGRE/OgreMeshManager.h>
00035 #include <OGRE/OgreSceneNode.h>
00036 #include <OGRE/OgreSceneManager.h>
00037
00038 #include <ros/package.h>
00039
00040 #include <tf/transform_listener.h>
00041
00042 #include <rviz/visualization_manager.h>
00043 #include <rviz/properties/color_property.h>
00044 #include <rviz/properties/float_property.h>
00045 #include <rviz/properties/int_property.h>
00046 #include <rviz/properties/bool_property.h>
00047 #include <rviz/frame_manager.h>
00048 #include "rviz/ogre_helpers/axes.h"
00049
00050 #include "marker_with_covariance/marker_with_covariance_display.h"
00051 #include "marker_with_covariance/marker_with_covariance_visual.h"
00052
00053 namespace marker_rviz_plugin {
00054
00055 MarkerWithCovarianceDisplay::MarkerWithCovarianceDisplay() {
00056
00057 _showAxesProperty = new rviz::BoolProperty("Show Axes", true, "Show or hide axes.", this, SLOT (updateVisual()));
00058 _showMarkerProperty = new rviz::BoolProperty("Show Marker", false, "Show or hide marker image.", this, SLOT (updateVisual()));
00059 _showLabelProperty = new rviz::BoolProperty("Show Label", true, "Show or hide marker label.", this, SLOT (updateVisual()));
00060
00061 _markerSizeProperty = new rviz::FloatProperty("Marker Size", 0.3, "Size of the marker image.", this, SLOT (updateVisual()));
00062 _markerSizeProperty->setMin(0);
00063
00064
00065 std::string rviz_path = ros::package::getPath(ROS_PACKAGE_NAME);
00066 Ogre::ResourceGroupManager::getSingleton().addResourceLocation(rviz_path + "/media", "FileSystem", ROS_PACKAGE_NAME);
00067 Ogre::ResourceGroupManager::getSingleton().initialiseResourceGroup(ROS_PACKAGE_NAME);
00068
00069 }
00070
00071 void MarkerWithCovarianceDisplay::onInitialize() {
00072 MFDClass::onInitialize();
00073
00074 _visual = new MarkerWithCovarianceVisual(context_->getSceneManager(), scene_node_);
00075 }
00076
00077 MarkerWithCovarianceDisplay::~MarkerWithCovarianceDisplay() {
00078 }
00079
00080
00081 void MarkerWithCovarianceDisplay::reset() {
00082 MFDClass::reset();
00083 }
00084
00085 void MarkerWithCovarianceDisplay::updateVisual() {
00086 _visual->setShowAxes(_showAxesProperty->getBool());
00087 _visual->setShowMarker(_showMarkerProperty->getBool());
00088 _visual->setShowLabel(_showLabelProperty->getBool());
00089 _visual->setScale(_markerSizeProperty->getFloat());
00090 }
00091
00092
00093 void MarkerWithCovarianceDisplay::processMessage(const marker_msgs::MarkerWithCovarianceStamped::ConstPtr &msg) {
00094
00095
00096
00097
00098 Ogre::Quaternion orientation;
00099 Ogre::Vector3 position;
00100 if (!context_->getFrameManager()->getTransform(msg->header.frame_id, msg->header.stamp, position, orientation)) {
00101 ROS_DEBUG ("Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
00102 return;
00103 }
00104
00105
00106 _visual->setMessage(msg);
00107 _visual->setFramePosition(position);
00108 _visual->setFrameOrientation(orientation);
00109 _visual->setShowAxes(_showAxesProperty->getBool());
00110 _visual->setShowMarker(_showMarkerProperty->getBool());
00111 _visual->setShowLabel(_showLabelProperty->getBool());
00112 _visual->setScale(_markerSizeProperty->getFloat());
00113
00114 context_->queueRender();
00115 }
00116
00117 }
00118
00119
00120
00121 #include <pluginlib/class_list_macros.h>
00122 PLUGINLIB_EXPORT_CLASS ( marker_rviz_plugin::MarkerWithCovarianceDisplay,rviz::Display )