38 #include <OgreVector3.h> 39 #include <OgreQuaternion.h> 40 #include <OgreSceneNode.h> 41 #include <OgreSceneManager.h> 59 ROS_ASSERT(new_message->type == visualization_msgs::Marker::POINTS ||
60 new_message->type == visualization_msgs::Marker::CUBE_LIST ||
61 new_message->type == visualization_msgs::Marker::SPHERE_LIST);
69 Ogre::Vector3 pos, scale;
70 Ogre::Quaternion orient;
71 transform(new_message, pos, orient, scale);
73 switch (new_message->type)
75 case visualization_msgs::Marker::POINTS:
79 case visualization_msgs::Marker::CUBE_LIST:
83 case visualization_msgs::Marker::SPHERE_LIST:
94 if (new_message->points.empty())
99 float r = new_message->color.r;
100 float g = new_message->color.g;
101 float b = new_message->color.b;
102 float a = new_message->color.a;
104 bool has_per_point_color = new_message->colors.size() == new_message->points.size();
106 bool has_nonzero_alpha =
false;
107 bool has_per_point_alpha =
false;
109 typedef std::vector< PointCloud::Point > V_Point;
111 points.resize(new_message->points.size());
112 std::vector<geometry_msgs::Point>::const_iterator it = new_message->points.begin();
113 std::vector<geometry_msgs::Point>::const_iterator end = new_message->points.end();
114 for (
int i = 0; it != end; ++it, ++i)
116 const geometry_msgs::Point& p = *it;
119 Ogre::Vector3 v(p.x, p.y, p.z);
125 if (has_per_point_color)
127 const std_msgs::ColorRGBA& color = new_message->colors[i];
132 has_nonzero_alpha = has_nonzero_alpha || a != 0.0;
133 has_per_point_alpha = has_per_point_alpha || a != 1.0;
139 if (has_per_point_color)
void addPoints(Point *points, uint32_t num_points)
Add points to this point cloud.
Ogre::SceneNode * scene_node_
bool transform(const MarkerConstPtr &message, Ogre::Vector3 &pos, Ogre::Quaternion &orient, Ogre::Vector3 &scale)
void setRenderMode(RenderMode mode)
Set what type of rendering primitives should be used, currently points, billboards and boxes are supp...
void setPickColor(const Ogre::ColourValue &color)
PointsMarker(MarkerDisplay *owner, DisplayContext *context, Ogre::SceneNode *parent_node)
static Ogre::ColourValue handleToColor(CollObjectHandle handle)
virtual void setPosition(const Ogre::Vector3 &position)
std::pair< std::string, int32_t > MarkerID
Pure-virtual base class for objects which give Display subclasses context in which to work...
boost::shared_ptr< MarkerSelectionHandler > handler_
Representation of a point, with x/y/z position and r/g/b color.
void setDimensions(float width, float height, float depth)
Set the dimensions of the billboards used to render each point.
DisplayContext * context_
void setAlpha(float alpha, bool per_point_alpha=false)
void clear()
Clear all the points.
visualization_msgs::Marker::ConstPtr MarkerConstPtr
virtual void onNewMessage(const MarkerConstPtr &old_message, const MarkerConstPtr &new_message)
void setHighlightColor(float r, float g, float b)
A visual representation of a set of points.
virtual void setOrientation(const Ogre::Quaternion &orientation)
Displays "markers" sent in by other ROS nodes on the "visualization_marker" topic.
void setHighlightColor(float r, float g, float b)
void setColor(float r, float g, float b, float a=1.0)