points_marker.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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 <OgreVector3.h>
00039 #include <OgreQuaternion.h>
00040 #include <OgreSceneNode.h>
00041 #include <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 } // end namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27