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 #include "line_strip_marker.h"
00031
00032 #include "marker_selection_handler.h"
00033 #include "rviz/default_plugin/marker_display.h"
00034 #include "rviz/display_context.h"
00035
00036 #include <rviz/ogre_helpers/billboard_line.h>
00037
00038 #include <OGRE/OgreVector3.h>
00039 #include <OGRE/OgreQuaternion.h>
00040 #include <OGRE/OgreSceneNode.h>
00041
00042 namespace rviz
00043 {
00044
00045 LineStripMarker::LineStripMarker(MarkerDisplay* owner, DisplayContext* context, Ogre::SceneNode* parent_node)
00046 : MarkerBase(owner, context, parent_node)
00047 , lines_(0)
00048 {
00049 }
00050
00051 LineStripMarker::~LineStripMarker()
00052 {
00053 delete lines_;
00054 }
00055
00056 void LineStripMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
00057 {
00058 ROS_ASSERT(new_message->type == visualization_msgs::Marker::LINE_STRIP);
00059
00060 if (!lines_)
00061 {
00062 lines_ = new BillboardLine(context_->getSceneManager(), scene_node_);
00063 }
00064
00065 Ogre::Vector3 pos, scale;
00066 Ogre::Quaternion orient;
00067 transform(new_message, pos, orient, scale);
00068
00069 setPosition(pos);
00070 setOrientation(orient);
00071 lines_->setScale(scale);
00072 lines_->setColor(new_message->color.r, new_message->color.g, new_message->color.b, new_message->color.a);
00073
00074 lines_->clear();
00075 if (new_message->points.empty())
00076 {
00077 return;
00078 }
00079
00080 lines_->setLineWidth(new_message->scale.x);
00081 lines_->setMaxPointsPerLine(new_message->points.size());
00082
00083 bool has_per_point_color = new_message->colors.size() == new_message->points.size();
00084
00085 size_t i = 0;
00086 std::vector<geometry_msgs::Point>::const_iterator it = new_message->points.begin();
00087 std::vector<geometry_msgs::Point>::const_iterator end = new_message->points.end();
00088 for ( ; it != end; ++it, ++i )
00089 {
00090 const geometry_msgs::Point& p = *it;
00091
00092 Ogre::Vector3 v( p.x, p.y, p.z );
00093
00094 Ogre::ColourValue c;
00095 if (has_per_point_color)
00096 {
00097 const std_msgs::ColorRGBA& color = new_message->colors[i];
00098 c.r = color.r;
00099 c.g = color.g;
00100 c.b = color.b;
00101 c.a = color.a;
00102 }
00103 else
00104 {
00105 c.r = new_message->color.r;
00106 c.g = new_message->color.g;
00107 c.b = new_message->color.b;
00108 c.a = new_message->color.a;
00109 }
00110
00111 lines_->addPoint( v, c );
00112 }
00113
00114 handler_.reset( new MarkerSelectionHandler( this, MarkerID( new_message->ns, new_message->id ), context_ ));
00115 handler_->addTrackedObjects( lines_->getSceneNode() );
00116 }
00117
00118 S_MaterialPtr LineStripMarker::getMaterials()
00119 {
00120 S_MaterialPtr materials;
00121 materials.insert( lines_->getMaterial() );
00122 return materials;
00123 }
00124
00125 }