32 #include <OgreSceneManager.h> 33 #include <OgreSceneNode.h> 34 #include <OgreWireBoundingBox.h> 64 ,
message( (uint64_t) _message )
76 ((uint) (iam.
message & 0xffffffff));
89 , cloud_info_( cloud_info )
97 QHash<IndexAndMessage, Property*>::const_iterator iter;
131 Ogre::Vector3
pointFromCloud(
const sensor_msgs::PointCloud2ConstPtr& cloud, uint32_t index)
137 const uint32_t xoff = cloud->fields[xi].offset;
138 const uint32_t yoff = cloud->fields[yi].offset;
139 const uint32_t zoff = cloud->fields[zi].offset;
140 const uint8_t type = cloud->fields[xi].datatype;
141 const uint32_t point_step = cloud->point_step;
142 float x = valueFromCloud<float>(cloud, xoff, type, point_step, index);
143 float y = valueFromCloud<float>(cloud, yoff, type, point_step, index);
144 float z = valueFromCloud<float>(cloud, zoff, type, point_step, index);
145 return Ogre::Vector3(x, y, z);
150 typedef std::set<int> S_int;
155 for (; it != end; ++it)
157 uint64_t handle = *it;
158 indices.insert((handle & 0xffffffff) - 1);
163 S_int::iterator it = indices.begin();
164 S_int::iterator end = indices.end();
165 for (; it != end; ++it)
173 Property* cat =
new Property( QString(
"Point %1 [cloud 0x%2]" ).arg( index ).arg( (uint64_t) message.get() ),
174 QVariant(),
"", parent_property );
182 for(
size_t field = 0; field < message->fields.size(); ++field )
184 const sensor_msgs::PointField&
f = message->fields[ field ];
185 const std::string& name = f.name;
187 if( name ==
"x" || name ==
"y" || name ==
"z" || name ==
"X" || name ==
"Y" || name ==
"Z" )
191 if( name ==
"rgb" || name ==
"rgba")
193 float float_val = valueFromCloud<float>( message, f.offset, f.datatype, message->point_step, index );
195 uint32_t val = *((uint32_t*) &float_val);
197 QColor( (val >> 16) & 0xff, (val >> 8) & 0xff, val & 0xff),
"", cat );
205 float val = valueFromCloud<float>( message, f.offset, f.datatype, message->point_step, index );
208 prop->setReadOnly(
true );
218 typedef std::set<int> S_int;
223 for (; it != end; ++it)
225 uint64_t handle = *it;
226 indices.insert((handle & 0xffffffff) - 1);
231 S_int::iterator it = indices.begin();
232 S_int::iterator end = indices.end();
233 for (; it != end; ++it)
250 for (; it != end; ++it)
252 M_HandleToBox::iterator find_it =
boxes_.find(std::make_pair(obj.
handle, *it - 1));
253 if (find_it !=
boxes_.end())
255 Ogre::WireBoundingBox* box = find_it->second.second;
257 aabbs.push_back(box->getWorldBoundingBox());
266 for (; it != end; ++it)
268 int index = (*it & 0xffffffff) - 1;
277 Ogre::AxisAlignedBox aabb(pos - size, pos + size);
287 for (; it != end; ++it)
289 int global_index = (*it & 0xffffffff) - 1;
324 "Whether or not the points in this point cloud are selectable.",
328 "Rendering mode to use, in order of computational complexity.",
337 "Point size in meters.",
342 "Point size in pixels.",
347 "Amount of transparency to apply to the points. Note that this is experimental and does not always look correct.",
353 "Duration, in seconds, to keep the incoming points. 0 means only show the latest points.",
358 "Set the transformer to use to set the position of the points.",
364 "Set the transformer to use to set the color of the points.",
399 std::vector<std::string>::iterator ci;
401 for( ci = classes.begin(); ci != classes.end(); ci++ )
403 const std::string& lookup_name = *ci;
408 ROS_ERROR(
"Transformer type [%s] is already loaded.", name.c_str() );
414 connect( trans.get(), SIGNAL( needRetransform() ),
this, SLOT(
causeRetransform() ));
418 info.readable_name = name;
419 info.lookup_name = lookup_name;
468 cloud_infos_[i]->cloud_->setPickColor( Ogre::ColourValue( 0.0
f, 0.0
f, 0.0
f, 0.0
f ) );
504 cloud_infos_[i]->cloud_->setDimensions( size, size, size );
558 for (; it != end; ++it)
560 if ( !(*it)->selection_handler_.get() ||
561 !(*it)->selection_handler_->hasSelections() )
580 for (; it != end; ++it)
584 V_CloudInfo::iterator next = it; next++;
586 if ( next != end && now.
toSec() - cloud_info->receive_time_.toSec() > point_decay_time ) {
590 bool per_point_alpha =
findChannelIndex(cloud_info->message_,
"rgba") != -1;
593 cloud_info->cloud_->setRenderMode( mode );
594 cloud_info->cloud_->addPoints( &(cloud_info->transformed_points_.front()), cloud_info->transformed_points_.size() );
596 cloud_info->cloud_->setDimensions( size, size, size );
601 cloud_info->scene_node_ =
scene_node_->createChildSceneNode( cloud_info->position_, cloud_info->orientation_ );
603 cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
617 if( lock.owns_lock() )
623 for (; it != end; ++it)
625 const std::string& name = it->first;
643 for(
int i = 0; i < props.size(); i++ )
645 props[ i ]->setHidden( hide );
658 typedef std::set<std::pair<uint8_t, std::string> >
S_string;
659 S_string valid_xyz, valid_color;
660 bool cur_xyz_valid =
false;
661 bool cur_color_valid =
false;
662 bool has_rgb_transformer =
false;
663 M_TransformerInfo::iterator trans_it =
transformers_.begin();
665 for(;trans_it != trans_end; ++trans_it)
667 const std::string& name = trans_it->first;
669 uint32_t mask = trans->supports(cloud);
672 valid_xyz.insert(std::make_pair(trans->score(cloud), name));
673 if (name == xyz_name)
675 cur_xyz_valid =
true;
682 valid_color.insert(std::make_pair(trans->score(cloud), name));
683 if (name == color_name)
685 cur_color_valid =
true;
689 has_rgb_transformer =
true;
697 if( !valid_xyz.empty() )
703 if( !cur_color_valid )
705 if( !valid_color.empty() )
707 if (has_rgb_transformer)
720 std::stringstream ss;
728 info->message_ = cloud;
800 for (; it != end; ++it)
804 cloud_info->cloud_->clear();
805 cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(), cloud_info->transformed_points_.size());
812 if ( !cloud_info->scene_node_ )
816 std::stringstream ss;
823 Ogre::Matrix4 transform;
824 transform.makeTransform( cloud_info->position_, Ogre::Vector3(1,1,1), cloud_info->orientation_ );
827 cloud_points.clear();
829 size_t size = cloud_info->message_->width * cloud_info->message_->height;
831 default_pt.
color = Ogre::ColourValue(1, 1, 1);
832 default_pt.
position = Ogre::Vector3::ZERO;
833 cloud_points.resize(size, default_pt);
837 if( update_transformers )
846 std::stringstream ss;
847 ss <<
"No position transformer available for cloud";
854 std::stringstream ss;
855 ss <<
"No color transformer available for cloud";
864 for (V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point)
868 cloud_point->position.x = 999999.0f;
869 cloud_point->position.y = 999999.0f;
870 cloud_point->position.z = 999999.0f;
879 output.header = input.header;
880 output.width = input.points.size ();
882 output.fields.resize (3 + input.channels.size ());
884 output.fields[0].name =
"x"; output.fields[1].name =
"y"; output.fields[2].name =
"z";
887 for (
size_t d = 0;
d < output.fields.size (); ++
d, offset += 4)
889 output.fields[
d].offset = offset;
890 output.fields[
d].datatype = sensor_msgs::PointField::FLOAT32;
892 output.point_step = offset;
893 output.row_step = output.point_step * output.width;
895 for (
size_t d = 0;
d < input.channels.size (); ++
d)
896 output.fields[3 +
d].name = input.channels[
d].name;
897 output.data.resize (input.points.size () * output.point_step);
898 output.is_bigendian =
false;
899 output.is_dense =
false;
902 for (
size_t cp = 0; cp < input.points.size (); ++cp)
904 memcpy (&output.data[cp * output.point_step + output.fields[0].offset], &input.points[cp].x, sizeof (
float));
905 memcpy (&output.data[cp * output.point_step + output.fields[1].offset], &input.points[cp].y, sizeof (
float));
906 memcpy (&output.data[cp * output.point_step + output.fields[2].offset], &input.points[cp].z, sizeof (
float));
907 for (
size_t d = 0;
d < input.channels.size (); ++
d)
909 if (input.channels[
d].values.size() == input.points.size())
911 memcpy (&output.data[cp * output.point_step + output.fields[3 +
d].offset], &input.channels[
d].values[cp], sizeof (
float));
920 sensor_msgs::PointCloud2Ptr out(
new sensor_msgs::PointCloud2);
956 const sensor_msgs::PointCloud2ConstPtr& msg =
cloud_infos_.front()->message_;
960 for (; it != end; ++it)
963 if ((trans->supports(msg) & mask) == mask)
965 prop->
addOption( QString::fromStdString( it->first ));
virtual void getAABBs(const Picked &obj, V_AABB &aabbs)
float getSelectionBoxSize()
void setXyzTransformerOptions(EnumProperty *prop)
void setStatusStd(StatusProperty::Level level, const std::string &name, const std::string &text)
Show status level and text, using a std::string. Convenience function which converts std::string to Q...
FloatProperty * alpha_property_
void setStringStd(const std::string &str)
Set the value of this property to the given std::string. Does not force the value to be one of the li...
pluginlib::ClassLoader< PointCloudTransformer > * transformer_class_loader_
std::set< std::string > S_string
void destroyBox(const std::pair< CollObjectHandle, uint64_t > &handles)
Destroy the box associated with the given handle-int pair, if there is one.
void updateTransformers(const sensor_msgs::PointCloud2ConstPtr &cloud)
uint qHash(IndexAndMessage iam)
CollObjectHandle getHandle() const
virtual void destroyProperties(const Picked &obj, Property *parent_property)
Destroy all properties for the given picked object(s).
PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr &cloud)
PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr &cloud)
std::vector< PointCloud::Point > transformed_points_
void createBox(const std::pair< CollObjectHandle, uint64_t > &handles, const Ogre::AxisAlignedBox &aabb, const std::string &material_name)
Create or update a box for the given handle-int pair, with the box specified by aabb.
boost::mutex new_clouds_mutex_
void updateXyzTransformer()
void emitTimeSignal(ros::Time time)
Emit a time signal that other Displays can synchronize to.
FloatProperty * point_world_size_property_
FloatProperty * decay_time_property_
A single element of a property tree, with a name, value, description, and possibly children...
IndexAndMessage(int _index, const void *_message)
virtual float getFloat() const
Ogre::SceneNode * scene_node_
virtual void clearOptions()
Clear the list of options.
EnumProperty * xyz_transformer_property_
bool new_xyz_transformer_
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
virtual void preRenderPass(uint32_t pass)
ros::AsyncSpinner spinner_
static Ogre::ColourValue handleToColor(CollObjectHandle handle)
void setPropertiesHidden(const QList< Property * > &props, bool hide)
friend class PointCloudSelectionHandler
Property specialized to enforce floating point max/min.
bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
TFSIMD_FORCE_INLINE const tfScalar & y() const
QHash< IndexAndMessage, Property * > property_hash_
boost::shared_ptr< PointCloudTransformer > PointCloudTransformerPtr
boost::recursive_mutex transformers_mutex_
Ogre::SceneManager * manager_
virtual void preRenderPass(uint32_t pass)
std::string getStdString()
virtual bool getBool() const
PointCloudCommon(Display *display)
ros::CallbackQueue cbqueue_
Pure-virtual base class for objects which give Display subclasses context in which to work...
std::vector< Ogre::AxisAlignedBox > V_AABB
bool validateFloats(const sensor_msgs::CameraInfo &msg)
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
virtual void addOption(const QString &option, int value=0)
EnumProperty * style_property_
void fillTransformerOptions(EnumProperty *prop, uint32_t mask)
virtual ~PointCloudSelectionHandler()
EnumProperty * color_transformer_property_
void show()
Show this Property in any PropertyTreeWidgets.
bool new_color_transformer_
Representation of a point, with x/y/z position and r/g/b color.
BoolProperty * selectable_property_
PointCloudCommon::CloudInfo * cloud_info_
std::vector< PointCloud::Point > V_PointCloudPoint
virtual void setReadOnly(bool read_only)
Overridden from Property to propagate read-only-ness to children.
M_TransformerInfo transformers_
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual void onDeselect(const Picked &obj)
void addOptionStd(const std::string &option, int value=0)
void setColorTransformerOptions(EnumProperty *prop)
FloatProperty * point_pixel_size_property_
DisplayContext * context_
L_CloudInfo obsolete_cloud_infos_
bool operator==(IndexAndMessage a, IndexAndMessage b)
virtual void onSelect(const Picked &obj)
const std::string & getFixedFrame()
Return the current fixed frame name.
virtual void createProperties(const Picked &obj, Property *parent_property)
Override to create properties of the given picked object(s).
TFSIMD_FORCE_INLINE const tfScalar & z() const
virtual Ogre::SceneManager * getSceneManager() const =0
Returns the Ogre::SceneManager used for the main RenderPanel.
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
int32_t findChannelIndex(const sensor_msgs::PointCloud2ConstPtr &cloud, const std::string &channel)
boost::shared_ptr< PointCloud > cloud_
Property specialized to provide getter for booleans.
A visual representation of a set of points.
void processMessage(const sensor_msgs::PointCloud2ConstPtr &cloud)
Ogre::Vector3 pointFromCloud(const sensor_msgs::PointCloud2ConstPtr &cloud, uint32_t index)
virtual void postRenderPass(uint32_t pass)
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
void setAutoSize(bool auto_size)
Ogre::SceneNode * scene_node_
V_CloudInfo new_cloud_infos_
bool transformCloud(const CloudInfoPtr &cloud, bool fully_update_transformers)
Transforms the cloud into the correct frame, and sets up our renderable cloud.
void hide()
Hide this Property in any PropertyTreeWidgets.
sensor_msgs::PointCloud2ConstPtr message_
void updateColorTransformer()
virtual void postRenderPass(uint32_t pass)
virtual void setReadOnly(bool read_only)
Prevent or allow users to edit this property from a PropertyTreeWidget.
virtual int getOptionInt()
Return the int value of the currently-chosen option, or 0 if the current option string does not have ...
PointCloudSelectionHandler(float box_size, PointCloudCommon::CloudInfo *cloud_info, DisplayContext *context)
void updateBillboardSize()
void update(float wall_dt, float ros_dt)