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 <OgreSceneNode.h>
00031 #include <OgreSceneManager.h>
00032
00033 #include <ros/time.h>
00034
00035 #include "rviz/default_plugin/point_cloud_common.h"
00036 #include "rviz/default_plugin/point_cloud_transformers.h"
00037 #include "rviz/display_context.h"
00038 #include "rviz/frame_manager.h"
00039 #include "rviz/ogre_helpers/point_cloud.h"
00040 #include "rviz/properties/int_property.h"
00041 #include "rviz/validate_floats.h"
00042
00043 #include "temperature_display.h"
00044
00045 namespace rviz
00046 {
00047
00048 TemperatureDisplay::TemperatureDisplay()
00049 : point_cloud_common_( new PointCloudCommon( this ))
00050 {
00051 queue_size_property_ = new IntProperty( "Queue Size", 10,
00052 "Advanced: set the size of the incoming Temperature message queue. "
00053 " Increasing this is useful if your incoming TF data is delayed significantly "
00054 "from your Temperature data, but it can greatly increase memory usage if the messages are big.",
00055 this, SLOT( updateQueueSize() ));
00056
00057
00058
00059 update_nh_.setCallbackQueue( point_cloud_common_->getCallbackQueue() );
00060 }
00061
00062 TemperatureDisplay::~TemperatureDisplay()
00063 {
00064 delete point_cloud_common_;
00065 }
00066
00067 void TemperatureDisplay::onInitialize()
00068 {
00069 MFDClass::onInitialize();
00070 point_cloud_common_->initialize( context_, scene_node_ );
00071
00072
00073 subProp("Channel Name")->setValue("temperature");
00074 subProp("Autocompute Intensity Bounds")->setValue(false);
00075 subProp("Invert Rainbow")->setValue(true);
00076 subProp("Min Intensity")->setValue(0);
00077 subProp("Max Intensity")->setValue(100);
00078 }
00079
00080 void TemperatureDisplay::updateQueueSize()
00081 {
00082 tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() );
00083 }
00084
00085 void TemperatureDisplay::processMessage( const sensor_msgs::TemperatureConstPtr& msg )
00086 {
00087 sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2);
00088
00089
00090 sensor_msgs::PointField x;
00091 x.name = "x";
00092 x.offset = 0;
00093 x.datatype = sensor_msgs::PointField::FLOAT32;
00094 x.count = 1;
00095 sensor_msgs::PointField y;
00096 y.name = "y";
00097 y.offset = 4;
00098 y.datatype = sensor_msgs::PointField::FLOAT32;
00099 y.count = 1;
00100 sensor_msgs::PointField z;
00101 z.name = "z";
00102 z.offset = 8;
00103 z.datatype = sensor_msgs::PointField::FLOAT32;
00104 z.count = 1;
00105 sensor_msgs::PointField temperature;
00106 temperature.name = "temperature";
00107 temperature.offset = 12;
00108 temperature.datatype = sensor_msgs::PointField::FLOAT64;
00109 temperature.count = 1;
00110
00111
00112 filtered->header = msg->header;
00113 filtered->fields.push_back(x);
00114 filtered->fields.push_back(y);
00115 filtered->fields.push_back(z);
00116 filtered->fields.push_back(temperature);
00117 filtered->data.resize(20);
00118 const float zero_float = 0.0;
00119 memcpy(&filtered->data[x.offset], &zero_float, 4);
00120 memcpy(&filtered->data[y.offset], &zero_float, 4);
00121 memcpy(&filtered->data[z.offset], &zero_float, 4);
00122 memcpy(&filtered->data[temperature.offset], &msg->temperature, 8);
00123 filtered->height = 1;
00124 filtered->width = 1;
00125 filtered->is_bigendian = false;
00126 filtered->point_step = 20;
00127 filtered->row_step = 1;
00128
00129
00130 point_cloud_common_->addMessage( filtered );
00131 }
00132
00133
00134 void TemperatureDisplay::update( float wall_dt, float ros_dt )
00135 {
00136 point_cloud_common_->update( wall_dt, ros_dt );
00137
00138
00139 subProp("Position Transformer")->hide();
00140 subProp("Color Transformer")->hide();
00141 subProp("Channel Name")->hide();
00142 subProp("Invert Rainbow")->hide();
00143 subProp("Autocompute Intensity Bounds")->hide();
00144 }
00145
00146 void TemperatureDisplay::reset()
00147 {
00148 MFDClass::reset();
00149 point_cloud_common_->reset();
00150 }
00151
00152 }
00153
00154 #include <pluginlib/class_list_macros.h>
00155 PLUGINLIB_EXPORT_CLASS( rviz::TemperatureDisplay, rviz::Display )