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 "illuminance_display.h"
00044
00045 namespace rviz
00046 {
00047
00048 IlluminanceDisplay::IlluminanceDisplay()
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 Illuminance message queue. "
00053 " Increasing this is useful if your incoming TF data is delayed significantly "
00054 "from your Illuminance 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 IlluminanceDisplay::~IlluminanceDisplay()
00063 {
00064 delete point_cloud_common_;
00065 }
00066
00067 void IlluminanceDisplay::onInitialize()
00068 {
00069 MFDClass::onInitialize();
00070 point_cloud_common_->initialize( context_, scene_node_ );
00071
00072
00073 subProp("Channel Name")->setValue("illuminance");
00074 subProp("Autocompute Intensity Bounds")->setValue(false);
00075 subProp("Min Intensity")->setValue(0);
00076 subProp("Max Intensity")->setValue(1000);
00077 }
00078
00079 void IlluminanceDisplay::updateQueueSize()
00080 {
00081 tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() );
00082 }
00083
00084 void IlluminanceDisplay::processMessage( const sensor_msgs::IlluminanceConstPtr& msg )
00085 {
00086
00087
00088
00089 sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2);
00090
00091
00092 sensor_msgs::PointField x;
00093 x.name = "x";
00094 x.offset = 0;
00095 x.datatype = sensor_msgs::PointField::FLOAT32;
00096 x.count = 1;
00097 sensor_msgs::PointField y;
00098 y.name = "y";
00099 y.offset = 4;
00100 y.datatype = sensor_msgs::PointField::FLOAT32;
00101 y.count = 1;
00102 sensor_msgs::PointField z;
00103 z.name = "z";
00104 z.offset = 8;
00105 z.datatype = sensor_msgs::PointField::FLOAT32;
00106 z.count = 1;
00107 sensor_msgs::PointField illuminance;
00108 illuminance.name = "illuminance";
00109 illuminance.offset = 12;
00110 illuminance.datatype = sensor_msgs::PointField::FLOAT64;
00111 illuminance.count = 1;
00112
00113
00114 filtered->header = msg->header;
00115 filtered->fields.push_back(x);
00116 filtered->fields.push_back(y);
00117 filtered->fields.push_back(z);
00118 filtered->fields.push_back(illuminance);
00119 filtered->data.resize(20);
00120 const float zero_float = 0.0;
00121 memcpy(&filtered->data[x.offset], &zero_float, 4);
00122 memcpy(&filtered->data[y.offset], &zero_float, 4);
00123 memcpy(&filtered->data[z.offset], &zero_float, 4);
00124 memcpy(&filtered->data[illuminance.offset], &msg->illuminance, 8);
00125 filtered->height = 1;
00126 filtered->width = 1;
00127 filtered->is_bigendian = false;
00128 filtered->point_step = 20;
00129 filtered->row_step = 1;
00130
00131
00132 point_cloud_common_->addMessage( filtered );
00133 }
00134
00135
00136 void IlluminanceDisplay::update( float wall_dt, float ros_dt )
00137 {
00138 point_cloud_common_->update( wall_dt, ros_dt );
00139
00140
00141 subProp("Position Transformer")->hide();
00142 subProp("Color Transformer")->hide();
00143 subProp("Channel Name")->hide();
00144 subProp("Autocompute Intensity Bounds")->hide();
00145 }
00146
00147 void IlluminanceDisplay::reset()
00148 {
00149 MFDClass::reset();
00150 point_cloud_common_->reset();
00151 }
00152
00153 }
00154
00155 #include <pluginlib/class_list_macros.h>
00156 PLUGINLIB_EXPORT_CLASS( rviz::IlluminanceDisplay, rviz::Display )