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/visualization_manager.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, VisualizationManager* manager, Ogre::SceneNode* parent_node)
00047 : MarkerBase(owner, manager, 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_BILLBOARDS);
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_BILLBOARD_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 points_->setAlpha(a);
00104
00105 bool has_per_point_color = new_message->colors.size() == new_message->points.size();
00106
00107 typedef std::vector< PointCloud::Point > V_Point;
00108 V_Point points;
00109 points.resize(new_message->points.size());
00110 std::vector<geometry_msgs::Point>::const_iterator it = new_message->points.begin();
00111 std::vector<geometry_msgs::Point>::const_iterator end = new_message->points.end();
00112 for (int i = 0; it != end; ++it, ++i)
00113 {
00114 const geometry_msgs::Point& p = *it;
00115 PointCloud::Point& point = points[i];
00116
00117 Ogre::Vector3 v(p.x, p.y, p.z);
00118
00119 point.x = v.x;
00120 point.y = v.y;
00121 point.z = v.z;
00122
00123 if (has_per_point_color)
00124 {
00125 const std_msgs::ColorRGBA& color = new_message->colors[i];
00126 r = color.r;
00127 g = color.g;
00128 b = color.b;
00129 }
00130
00131 point.setColor(r, g, b);
00132 }
00133
00134 points_->addPoints(&points.front(), points.size());
00135
00136 vis_manager_->getSelectionManager()->removeObject(coll_);
00137 coll_ = vis_manager_->getSelectionManager()->createHandle();
00138
00139 float p_r = ((coll_ >> 16) & 0xff) / 255.0f;
00140 float p_g = ((coll_ >> 8) & 0xff) / 255.0f;
00141 float p_b = (coll_ & 0xff) / 255.0f;
00142 Ogre::ColourValue col(p_r, p_g, p_b, 1.0f);
00143 points_->setPickColor(col);
00144
00145 SelectionHandlerPtr handler( new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id)) );
00146 vis_manager_->getSelectionManager()->addObject( coll_, handler );
00147 }
00148
00149 void PointsMarker::setHighlightColor( float r, float g, float b )
00150 {
00151 points_->setHighlightColor( r, g, b );
00152 }
00153
00154 }