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_list_marker.h"
00031 #include "marker_selection_handler.h"
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 <OgreVector3.h>
00038 #include <OgreQuaternion.h>
00039 #include <OgreSceneNode.h>
00040
00041 namespace rviz
00042 {
00043
00044 LineListMarker::LineListMarker(MarkerDisplay* owner, DisplayContext* context, Ogre::SceneNode* parent_node)
00045 : MarkerBase(owner, context, parent_node)
00046 , lines_(0)
00047 {
00048 }
00049
00050 LineListMarker::~LineListMarker()
00051 {
00052 delete lines_;
00053 }
00054
00055 void LineListMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
00056 {
00057 ROS_ASSERT(new_message->type == visualization_msgs::Marker::LINE_LIST);
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
00075 if (new_message->points.empty())
00076 {
00077 return;
00078 }
00079
00080 bool has_per_point_color = new_message->colors.size() == new_message->points.size();
00081
00082 if (new_message->points.size() % 2 == 0)
00083 {
00084 lines_->setLineWidth( new_message->scale.x );
00085 lines_->setMaxPointsPerLine(2);
00086 lines_->setNumLines(new_message->points.size() / 2);
00087
00088 size_t i = 0;
00089 std::vector<geometry_msgs::Point>::const_iterator it = new_message->points.begin();
00090 std::vector<geometry_msgs::Point>::const_iterator end = new_message->points.end();
00091 for ( ; it != end; )
00092 {
00093 if (it != new_message->points.begin())
00094 {
00095 lines_->newLine();
00096 }
00097
00098 for (uint32_t j = 0; j < 2; ++j, ++it, ++i)
00099 {
00100 const geometry_msgs::Point& p = *it;
00101
00102 Ogre::ColourValue c;
00103 if (has_per_point_color)
00104 {
00105 const std_msgs::ColorRGBA& color = new_message->colors[i];
00106 c.r = color.r;
00107 c.g = color.g;
00108 c.b = color.b;
00109 c.a = color.a;
00110 }
00111 else
00112 {
00113 c.r = new_message->color.r;
00114 c.g = new_message->color.g;
00115 c.b = new_message->color.b;
00116 c.a = new_message->color.a;
00117 }
00118
00119 Ogre::Vector3 v( p.x, p.y, p.z );
00120 lines_->addPoint( v, c );
00121 }
00122 }
00123
00124 handler_.reset( new MarkerSelectionHandler( this, MarkerID( new_message->ns, new_message->id ), context_ ));
00125 handler_->addTrackedObjects( lines_->getSceneNode() );
00126 }
00127 else
00128 {
00129 std::stringstream ss;
00130 ss << "Line list marker [" << getStringID() << "] has an odd number of points.";
00131 if ( owner_ )
00132 {
00133 owner_->setMarkerStatus(getID(), StatusProperty::Error, ss.str());
00134 }
00135 ROS_DEBUG("%s", ss.str().c_str());
00136 }
00137 }
00138
00139 S_MaterialPtr LineListMarker::getMaterials()
00140 {
00141 S_MaterialPtr materials;
00142 materials.insert( lines_->getMaterial() );
00143 return materials;
00144 }
00145
00146 }