select_point_cloud_publish_action.cpp
Go to the documentation of this file.
00001 #include <stdio.h>
00002 
00003 #include <QPainter>
00004 #include <QLineEdit>
00005 #include <QPushButton>
00006 #include <QVBoxLayout>
00007 #include <QHBoxLayout>
00008 #include <QLabel>
00009 #include <QTimer>
00010 #include <QColor>
00011 #include <QFont>
00012 
00013 #include "rviz/properties/property_tree_widget.h"
00014 #include "rviz/selection/selection_manager.h"
00015 #include "rviz/visualization_manager.h"
00016 
00017 #include "rviz/config.h"
00018 #include "rviz/properties/property_tree_model.h"
00019 #include "rviz/properties/status_list.h"
00020 #include "rviz/properties/property.h"
00021 #include "rviz/properties/vector_property.h"
00022 #include "rviz/properties/color_property.h"
00023 
00024 #include <std_msgs/Empty.h>
00025 #include <sensor_msgs/PointCloud2.h>
00026 
00027 #include "select_point_cloud_publish_action.h"
00028 #include "ros/time.h"
00029 
00030 using namespace rviz;
00031 
00032 namespace jsk_rviz_plugins
00033 {
00034 
00035   SelectPointCloudPublishAction::SelectPointCloudPublishAction( QWidget* parent )
00036     : rviz::Panel( parent )
00037   {
00038     select_pointcloud_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>("selected_pointcloud", 1);
00039     layout = new QVBoxLayout;
00040 
00041     //Button to send cancel topic
00042     publish_pointcloud_button_ = new QPushButton("SelectPointCloudPublish Action");
00043     layout->addWidget( publish_pointcloud_button_ );
00044 
00045     setLayout( layout );
00046 
00047     connect( publish_pointcloud_button_, SIGNAL( clicked() ), this, SLOT( publishPointCloud ()));
00048   }
00049 
00050   void SelectPointCloudPublishAction::publishPointCloud(){
00051     PropertyTreeModel* model_ =vis_manager_->getSelectionManager()->getPropertyModel();
00052     int num_children = model_->rowCount();
00053     if( num_children > 0 )
00054       {
00055         ROS_INFO("num > %d!", num_children);
00056         sensor_msgs::PointCloud2 pc2;
00057         pc2.header.stamp = ros::Time::now();
00058         pc2.header.frame_id = "camera_depth_optical_frame";
00059         pc2.height = 1;
00060         pc2.width  = num_children;
00061 
00062         pc2.fields.resize(4);
00063         pc2.fields[0].name = "x";
00064         pc2.fields[1].name = "y";
00065         pc2.fields[2].name = "z";
00066         pc2.fields[3].name = "rgb";
00067         pc2.fields[0].offset = 0;
00068         pc2.fields[1].offset = 4;
00069         pc2.fields[2].offset = 8;
00070         pc2.fields[3].offset = 12;
00071         pc2.fields[0].count =  pc2.fields[1].count =  pc2.fields[2].count =  pc2.fields[3].count = 1;
00072         pc2.fields[0].datatype =  pc2.fields[1].datatype =  pc2.fields[2].datatype =  pc2.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
00073 
00074         pc2.data.resize(num_children * 4 * sizeof(float));
00075         for( int i = 0; i < num_children; i++ )
00076           {
00077             QModelIndex child_index = model_->index( i, 0, QModelIndex());
00078             VectorProperty* vec_data = qobject_cast<VectorProperty* >(model_->getProp( child_index )->childAt(0));
00079             ColorProperty* color_data = qobject_cast<ColorProperty* >(model_->getProp( child_index )->childAt(1));
00080 
00081             Ogre::Vector3 point_vec = vec_data->getVector();
00082             // check if color_data is available
00083             // if not color_data is available, set the color to black(0,0,0)
00084             int rgb_int = 0;
00085             if (color_data != NULL && color_data->getColor().isValid()) {
00086               Ogre::ColourValue point_color = color_data->getOgreColor();
00087               rgb_int = (int)point_color.r << 16 | (int)point_color.g << 8 |  (int)point_color.b << 0;
00088             }
00089             float x = point_vec.x, y = point_vec.y, z = point_vec.z;
00090             //Tty to add color, but point_color's value are all zero!!!!!!
00091             float rgb_float = *reinterpret_cast<float*>(&rgb_int);
00092             memcpy(&pc2.data[i*4*sizeof(float)], &x, sizeof(float));
00093             memcpy(&pc2.data[(i*4+1)*sizeof(float)], &y, sizeof(float));
00094             memcpy(&pc2.data[(i*4+2)*sizeof(float)], &z, sizeof(float));
00095             memcpy(&pc2.data[(i*4+3)*sizeof(float)], &rgb_float, sizeof(float));
00096           }
00097 
00098         pc2.point_step = 16;
00099         pc2.row_step = pc2.point_step * pc2.width;
00100         pc2.is_dense = false;
00101         select_pointcloud_publisher_.publish(pc2);
00102       }
00103   }
00104 
00105   void SelectPointCloudPublishAction::save( rviz::Config config ) const
00106   {
00107     rviz::Panel::save( config );
00108   }
00109 
00110   // Load all configuration data for this panel from the given Config object.
00111   void SelectPointCloudPublishAction::load( const rviz::Config& config )
00112   {
00113     rviz::Panel::load( config );
00114   }
00115 
00116 }
00117 
00118 #include <pluginlib/class_list_macros.h>
00119 PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::SelectPointCloudPublishAction, rviz::Panel )


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03