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 "points_marker.h"
00031 #include "rviz/default_plugin/marker_display.h"
00032 #include "rviz/display_context.h"
00033 #include "rviz/selection/selection_manager.h"
00034 #include "marker_selection_handler.h"
00035
00036 #include <rviz/ogre_helpers/point_cloud.h>
00037
00038 #include <OGRE/OgreVector3.h>
00039 #include <OGRE/OgreQuaternion.h>
00040 #include <OGRE/OgreSceneNode.h>
00041 #include <OGRE/OgreSceneManager.h>
00042
00043 namespace rviz
00044 {
00045
00046 PointsMarker::PointsMarker(MarkerDisplay* owner, DisplayContext* context, Ogre::SceneNode* parent_node)
00047 : MarkerBase(owner, context, parent_node)
00048 , points_(0)
00049 {
00050 }
00051
00052 PointsMarker::~PointsMarker()
00053 {
00054 delete points_;
00055 }
00056
00057 void PointsMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
00058 {
00059 ROS_ASSERT(new_message->type == visualization_msgs::Marker::POINTS ||
00060 new_message->type == visualization_msgs::Marker::CUBE_LIST ||
00061 new_message->type == visualization_msgs::Marker::SPHERE_LIST);
00062
00063 if (!points_)
00064 {
00065 points_ = new PointCloud();
00066 scene_node_->attachObject(points_);
00067 }
00068
00069 Ogre::Vector3 pos, scale;
00070 Ogre::Quaternion orient;
00071 transform(new_message, pos, orient, scale);
00072
00073 switch (new_message->type)
00074 {
00075 case visualization_msgs::Marker::POINTS:
00076 points_->setRenderMode(PointCloud::RM_SQUARES);
00077 points_->setDimensions(new_message->scale.x, new_message->scale.y, 0.0f);
00078 break;
00079 case visualization_msgs::Marker::CUBE_LIST:
00080 points_->setRenderMode(PointCloud::RM_BOXES);
00081 points_->setDimensions(scale.x, scale.y, scale.z);
00082 break;
00083 case visualization_msgs::Marker::SPHERE_LIST:
00084 points_->setRenderMode(PointCloud::RM_SPHERES);
00085 points_->setDimensions(scale.x, scale.y, scale.z);
00086 break;
00087 }
00088
00089 setPosition(pos);
00090 setOrientation(orient);
00091
00092 points_->clear();
00093
00094 if (new_message->points.empty())
00095 {
00096 return;
00097 }
00098
00099 float r = new_message->color.r;
00100 float g = new_message->color.g;
00101 float b = new_message->color.b;
00102 float a = new_message->color.a;
00103
00104 bool has_per_point_color = new_message->colors.size() == new_message->points.size();
00105
00106 bool has_nonzero_alpha = false;
00107 bool has_per_point_alpha = false;
00108
00109 typedef std::vector< PointCloud::Point > V_Point;
00110 V_Point points;
00111 points.resize(new_message->points.size());
00112 std::vector<geometry_msgs::Point>::const_iterator it = new_message->points.begin();
00113 std::vector<geometry_msgs::Point>::const_iterator end = new_message->points.end();
00114 for (int i = 0; it != end; ++it, ++i)
00115 {
00116 const geometry_msgs::Point& p = *it;
00117 PointCloud::Point& point = points[i];
00118
00119 Ogre::Vector3 v(p.x, p.y, p.z);
00120
00121 point.position.x = v.x;
00122 point.position.y = v.y;
00123 point.position.z = v.z;
00124
00125 if (has_per_point_color)
00126 {
00127 const std_msgs::ColorRGBA& color = new_message->colors[i];
00128 r = color.r;
00129 g = color.g;
00130 b = color.b;
00131 a = color.a;
00132 has_nonzero_alpha = has_nonzero_alpha || a != 0.0;
00133 has_per_point_alpha = has_per_point_alpha || a != 1.0;
00134 }
00135
00136 point.setColor(r, g, b, a);
00137 }
00138
00139 if (has_per_point_color)
00140 {
00141 if (!has_nonzero_alpha && owner_)
00142 {
00143 owner_->setMarkerStatus(getID(), StatusProperty::Warn, "All points have a zero alpha value.");
00144 }
00145 points_->setAlpha(1.0, has_per_point_alpha);
00146 }
00147 else
00148 {
00149 points_->setAlpha(a);
00150 }
00151
00152 points_->addPoints(&points.front(), points.size());
00153
00154 handler_.reset( new MarkerSelectionHandler( this, MarkerID( new_message->ns, new_message->id ), context_ ));
00155 points_->setPickColor( SelectionManager::handleToColor( handler_->getHandle() ));
00156 }
00157
00158 void PointsMarker::setHighlightColor( float r, float g, float b )
00159 {
00160 points_->setHighlightColor( r, g, b );
00161 }
00162
00163 }