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 "rviz/default_plugin/marker_display.h"
00032 #include "rviz/visualization_manager.h"
00033
00034 #include <rviz/ogre_helpers/billboard_line.h>
00035
00036 #include <OGRE/OgreVector3.h>
00037 #include <OGRE/OgreQuaternion.h>
00038 #include <OGRE/OgreSceneNode.h>
00039
00040 namespace rviz
00041 {
00042
00043 LineListMarker::LineListMarker(MarkerDisplay* owner, VisualizationManager* manager, Ogre::SceneNode* parent_node)
00044 : MarkerBase(owner, manager, parent_node)
00045 , lines_(0)
00046 {
00047 }
00048
00049 LineListMarker::~LineListMarker()
00050 {
00051 delete lines_;
00052 }
00053
00054 void LineListMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
00055 {
00056 ROS_ASSERT(new_message->type == visualization_msgs::Marker::LINE_LIST);
00057
00058 if (!lines_)
00059 {
00060 lines_ = new BillboardLine(vis_manager_->getSceneManager(), scene_node_);
00061 }
00062
00063 Ogre::Vector3 pos, scale;
00064 Ogre::Quaternion orient;
00065 transform(new_message, pos, orient, scale);
00066
00067 setPosition(pos);
00068 setOrientation(orient);
00069 lines_->setScale(scale);
00070 lines_->setColor(new_message->color.r, new_message->color.g, new_message->color.b, new_message->color.a);
00071
00072 lines_->clear();
00073
00074 if (new_message->points.empty())
00075 {
00076 return;
00077 }
00078
00079 bool has_per_point_color = new_message->colors.size() == new_message->points.size();
00080
00081 if (new_message->points.size() % 2 == 0)
00082 {
00083 lines_->setLineWidth( new_message->scale.x );
00084 lines_->setMaxPointsPerLine(2);
00085 lines_->setNumLines(new_message->points.size() / 2);
00086
00087 size_t i = 0;
00088 std::vector<geometry_msgs::Point>::const_iterator it = new_message->points.begin();
00089 std::vector<geometry_msgs::Point>::const_iterator end = new_message->points.end();
00090 for ( ; it != end; )
00091 {
00092 if (it != new_message->points.begin())
00093 {
00094 lines_->newLine();
00095 }
00096
00097 for (uint32_t j = 0; j < 2; ++j, ++it, ++i)
00098 {
00099 const geometry_msgs::Point& p = *it;
00100
00101 Ogre::ColourValue c;
00102 if (has_per_point_color)
00103 {
00104 const std_msgs::ColorRGBA& color = new_message->colors[i];
00105 c.r = color.r;
00106 c.g = color.g;
00107 c.b = color.b;
00108 c.a = new_message->color.a;
00109 }
00110 else
00111 {
00112 c.r = new_message->color.r;
00113 c.g = new_message->color.g;
00114 c.b = new_message->color.b;
00115 c.a = new_message->color.a;
00116 }
00117
00118 Ogre::Vector3 v( p.x, p.y, p.z );
00119 lines_->addPoint( v, c );
00120 }
00121 }
00122 }
00123 else
00124 {
00125 std::stringstream ss;
00126 ss << "Line list marker [" << getStringID() << "] has an odd number of points.";
00127 if ( owner_ )
00128 {
00129 owner_->setMarkerStatus(getID(), status_levels::Error, ss.str());
00130 }
00131 ROS_DEBUG("%s", ss.str().c_str());
00132 }
00133 }
00134
00135 S_MaterialPtr LineListMarker::getMaterials()
00136 {
00137 S_MaterialPtr materials;
00138 materials.insert( lines_->getMaterial() );
00139 return materials;
00140 }
00141
00142 }