select_point_cloud_publish_action.cpp
Go to the documentation of this file.
1 #include <stdio.h>
2 
3 #include <QPainter>
4 #include <QLineEdit>
5 #include <QPushButton>
6 #include <QVBoxLayout>
7 #include <QHBoxLayout>
8 #include <QLabel>
9 #include <QTimer>
10 #include <QColor>
11 #include <QFont>
12 
16 
17 #include "rviz/config.h"
23 
24 #include <std_msgs/Empty.h>
25 #include <sensor_msgs/PointCloud2.h>
26 
28 #include "ros/time.h"
29 
30 using namespace rviz;
31 
32 namespace jsk_rviz_plugins
33 {
34 
35  SelectPointCloudPublishAction::SelectPointCloudPublishAction( QWidget* parent )
36  : rviz::Panel( parent )
37  {
38  select_pointcloud_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>("selected_pointcloud", 1);
39  layout = new QVBoxLayout;
40 
41  //Button to send cancel topic
42  publish_pointcloud_button_ = new QPushButton("SelectPointCloudPublish Action");
43  layout->addWidget( publish_pointcloud_button_ );
44 
45  setLayout( layout );
46 
47  connect( publish_pointcloud_button_, SIGNAL( clicked() ), this, SLOT( publishPointCloud ()));
48  }
49 
52  int num_children = model_->rowCount();
53  if( num_children > 0 )
54  {
55  ROS_INFO("num > %d!", num_children);
56  sensor_msgs::PointCloud2 pc2;
57  pc2.header.stamp = ros::Time::now();
58  pc2.header.frame_id = "camera_depth_optical_frame";
59  pc2.height = 1;
60  pc2.width = num_children;
61 
62  pc2.fields.resize(4);
63  pc2.fields[0].name = "x";
64  pc2.fields[1].name = "y";
65  pc2.fields[2].name = "z";
66  pc2.fields[3].name = "rgb";
67  pc2.fields[0].offset = 0;
68  pc2.fields[1].offset = 4;
69  pc2.fields[2].offset = 8;
70  pc2.fields[3].offset = 12;
71  pc2.fields[0].count = pc2.fields[1].count = pc2.fields[2].count = pc2.fields[3].count = 1;
72  pc2.fields[0].datatype = pc2.fields[1].datatype = pc2.fields[2].datatype = pc2.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
73 
74  pc2.data.resize(num_children * 4 * sizeof(float));
75  for( int i = 0; i < num_children; i++ )
76  {
77  QModelIndex child_index = model_->index( i, 0, QModelIndex());
78  VectorProperty* vec_data = qobject_cast<VectorProperty* >(model_->getProp( child_index )->childAt(0));
79  ColorProperty* color_data = qobject_cast<ColorProperty* >(model_->getProp( child_index )->childAt(1));
80 
81  Ogre::Vector3 point_vec = vec_data->getVector();
82  // check if color_data is available
83  // if not color_data is available, set the color to black(0,0,0)
84  int rgb_int = 0;
85  if (color_data != NULL && color_data->getColor().isValid()) {
86  Ogre::ColourValue point_color = color_data->getOgreColor();
87  rgb_int = (int)point_color.r << 16 | (int)point_color.g << 8 | (int)point_color.b << 0;
88  }
89  float x = point_vec.x, y = point_vec.y, z = point_vec.z;
90  //Tty to add color, but point_color's value are all zero!!!!!!
91  float rgb_float = *reinterpret_cast<float*>(&rgb_int);
92  memcpy(&pc2.data[i*4*sizeof(float)], &x, sizeof(float));
93  memcpy(&pc2.data[(i*4+1)*sizeof(float)], &y, sizeof(float));
94  memcpy(&pc2.data[(i*4+2)*sizeof(float)], &z, sizeof(float));
95  memcpy(&pc2.data[(i*4+3)*sizeof(float)], &rgb_float, sizeof(float));
96  }
97 
98  pc2.point_step = 16;
99  pc2.row_step = pc2.point_step * pc2.width;
100  pc2.is_dense = false;
102  }
103  }
104 
106  {
107  rviz::Panel::save( config );
108  }
109 
110  // Load all configuration data for this panel from the given Config object.
112  {
113  rviz::Panel::load( config );
114  }
115 
116 }
117 
#define NULL
VisualizationManager * vis_manager_
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
int rowCount(const QModelIndex &parent=QModelIndex()) const override
QModelIndex index(int row, int column, const QModelIndex &parent=QModelIndex()) const override
virtual Ogre::Vector3 getVector() const
virtual void save(Config config) const
void publish(const boost::shared_ptr< M > &message) const
#define ROS_INFO(...)
Property * getProp(const QModelIndex &index) const
x
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
y
SelectionManager * getSelectionManager() const override
Property * childAt(int index) const
static Time now()
virtual void load(const Config &config)
PropertyTreeModel * getPropertyModel()
z


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Thu Jun 1 2023 02:45:58