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