30 #include <OgreSceneNode.h> 31 #include <OgreSceneManager.h> 52 "Advanced: set the size of the incoming Temperature message queue. " 53 " Increasing this is useful if your incoming TF data is delayed significantly " 54 "from your Temperature data, but it can greatly increase memory usage if the messages are big.",
87 sensor_msgs::PointCloud2Ptr filtered(
new sensor_msgs::PointCloud2);
90 sensor_msgs::PointField
x;
93 x.datatype = sensor_msgs::PointField::FLOAT32;
95 sensor_msgs::PointField
y;
98 y.datatype = sensor_msgs::PointField::FLOAT32;
100 sensor_msgs::PointField
z;
103 z.datatype = sensor_msgs::PointField::FLOAT32;
105 sensor_msgs::PointField temperature;
106 temperature.name =
"temperature";
107 temperature.offset = 12;
108 temperature.datatype = sensor_msgs::PointField::FLOAT64;
109 temperature.count = 1;
112 filtered->header = msg->header;
113 filtered->fields.push_back(x);
114 filtered->fields.push_back(y);
115 filtered->fields.push_back(z);
116 filtered->fields.push_back(temperature);
117 filtered->data.resize(20);
118 const float zero_float = 0.0;
119 memcpy(&filtered->data[x.offset], &zero_float, 4);
120 memcpy(&filtered->data[y.offset], &zero_float, 4);
121 memcpy(&filtered->data[z.offset], &zero_float, 4);
122 memcpy(&filtered->data[temperature.offset], &msg->temperature, 8);
123 filtered->height = 1;
125 filtered->is_bigendian =
false;
126 filtered->point_step = 20;
127 filtered->row_step = 1;
virtual void onInitialize()
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
virtual bool setValue(const QVariant &new_value)
Set the new value for this property. Returns true if the new value is different from the old value...
virtual int getInt() const
Return the internal property value as an integer.
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
ros::CallbackQueueInterface * getCallbackQueue()
void initialize(DisplayContext *context, Ogre::SceneNode *scene_node)
virtual void reset()
Called to tell the display to clear its state.
tf::MessageFilter< sensor_msgs::Temperature > * tf_filter_
TFSIMD_FORCE_INLINE const tfScalar & y() const
Property specialized to provide max/min enforcement for integers.
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Displays a point cloud of type sensor_msgs::PointCloud.
virtual void setQueueSize(uint32_t new_queue_size)
virtual Property * subProp(const QString &sub_name)
Return the first child Property with the given name, or the FailureProperty if no child has the name...
PointCloudCommon * point_cloud_common_
void addMessage(const sensor_msgs::PointCloudConstPtr &cloud)
void setCallbackQueue(CallbackQueueInterface *queue)
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual void update(float wall_dt, float ros_dt)
Called periodically by the visualization manager.
TFSIMD_FORCE_INLINE const tfScalar & z() const
virtual void onInitialize()
Do initialization. Overridden from MessageFilterDisplay.
Displays a Temperature message of type sensor_msgs::Temperature.
void hide()
Hide this Property in any PropertyTreeWidgets.
virtual void processMessage(const sensor_msgs::TemperatureConstPtr &msg)
Process a single message. Overridden from MessageFilterDisplay.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void update(float wall_dt, float ros_dt)
IntProperty * queue_size_property_