Skip to content

Commit

Permalink
Extension: now you can select several parts of the point cloud and ad…
Browse files Browse the repository at this point in the history 
…d this points to the selection before publishing it. This allows a much precise selection
  • Loading branch information
@roberto-martinmartin
roberto-martinmartin committed Feb 20, 2015
1 parent 83c9633 commit 4784d87
Show file tree
Hide file tree
Showing 2 changed files with 125 additions and 21 deletions.
13 changes: 12 additions & 1 deletion ...-plugin-selected-points-topic/include/selected_points_publisher/SelectedPointsPublisher.h
View file
Expand Up @@ -64,6 +64,8 @@
#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/filters/extract_indices.h>

namespace rviz_plugin_selected_points_publisher
{

Expand Down Expand Up @@ -94,10 +96,12 @@ public Q_SLOTS:

protected:

int _processSelectedAreaAndPublishPoints();
int _processSelectedAreaAndFindPoints();
int _publishAccumulatedPoints();
ros::NodeHandle nh_;
ros::Publisher rviz_selected_pub_;
ros::Publisher real_selected_pub_;
ros::Publisher partial_pc_pub_;
ros::Publisher bb_marker_pub_;
ros::Subscriber pc_subs_;

Expand All @@ -109,6 +113,13 @@ public Q_SLOTS:
bool selecting_;

pcl::PointCloud<pcl::PointXYZRGB>::Ptr current_pc_;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr selected_segment_pc_;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr accumulated_segment_pc_;

pcl::ExtractIndices<pcl::PointXYZRGB>::Ptr extract_indices_filter_;

int num_acc_points_;
int num_selected_points_;
};
} // end namespace rviz_plugin_selected_points_publisher

Expand Down
133 changes: 113 additions & 20 deletions rviz-plugin-selected-points-topic/src/SelectedPointsPublisher.cpp
View file
Expand Up @@ -26,7 +26,7 @@
#include <pcl/common/common.h>

#include <pcl/filters/impl/box_clipper3D.hpp>
#include <pcl/filters/extract_indices.h>


#include <visualization_msgs/Marker.h>

Expand Down Expand Up @@ -54,6 +54,7 @@ void SelectedPointsPublisher::updateTopic()

rviz_selected_pub_ = nh_.advertise<sensor_msgs::PointCloud2>( rviz_cloud_topic_.c_str(), 1 );
real_selected_pub_ = nh_.advertise<sensor_msgs::PointCloud2>( real_cloud_topic_.c_str(), 1 );
partial_pc_pub_ = nh_.advertise<sensor_msgs::PointCloud2>( subs_cloud_topic_.c_str(), 1 );
bb_marker_pub_ = nh_.advertise<visualization_msgs::Marker>(bb_marker_topic_.c_str(), 1);
pc_subs_ = nh_.subscribe(subs_cloud_topic_.c_str(),1,&SelectedPointsPublisher::PointCloudsCallback, this);

Expand All @@ -62,11 +63,19 @@ void SelectedPointsPublisher::updateTopic()
ROS_INFO_STREAM_NAMED("SelectedPointsPublisher.updateTopic", "Publishing bounding box marker on topic " << nh_.resolveName (bb_marker_topic_) );//<< " with frame_id " << context_->getFixedFrame().toStdString() );

current_pc_.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
accumulated_segment_pc_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);

num_acc_points_ = 0;
num_selected_points_ = 0;
}

void SelectedPointsPublisher::PointCloudsCallback(const sensor_msgs::PointCloud2ConstPtr &pc_msg)
{
ROS_INFO_STREAM_NAMED( "SelectedPointsPublisher::PointCloudsCallback", "Received PC");
// We only publish a message with the reception of the original pc (maybe we also do not need to copy the received pc, because is what we published!)
if(this->accumulated_segment_pc_->points.size() == 0)
{
ROS_INFO_STREAM_NAMED( "SelectedPointsPublisher::PointCloudsCallback", "Received PC");
}
// Convert ROS PC message into a pcl point cloud
pcl::fromROSMsg(*pc_msg, *this->current_pc_);
}
Expand All @@ -77,7 +86,7 @@ int SelectedPointsPublisher::processKeyEvent( QKeyEvent* event, rviz::RenderPane
{
if(event->key() == 'c' || event->key() == 'C')
{
ROS_INFO_STREAM_NAMED( "SelectedPointsPublisher::processKeyEvent", "Cleaning a previous selection");
ROS_INFO_STREAM_NAMED( "SelectedPointsPublisher::processKeyEvent", "Cleaning ALL previous selection (selected area and points).");
rviz::SelectionManager* sel_manager = context_->getSelectionManager();
rviz::M_Picked selection = sel_manager->getSelection();
sel_manager->removeSelection(selection);
Expand All @@ -91,12 +100,75 @@ int SelectedPointsPublisher::processKeyEvent( QKeyEvent* event, rviz::RenderPane
marker.action = visualization_msgs::Marker::DELETE;
marker.lifetime = ros::Duration();

selected_segment_pc_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
accumulated_segment_pc_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
bb_marker_pub_.publish(marker);
}
else if(event->key() == 'r' || event->key() == 'R')
{
ROS_INFO_STREAM_NAMED( "SelectedPointsPublisher.processKeyEvent", "Reusing the previously selected area to find a new bounding box and publish the points inside of it");
this->_processSelectedAreaAndPublishPoints();
ROS_INFO_STREAM_NAMED( "SelectedPointsPublisher.processKeyEvent", "Reusing the LAST selected area to find a NEW bounding box.");
this->_processSelectedAreaAndFindPoints();
}
else if(event->key() == 'y' || event->key() == 'Y')
{
this->_publishAccumulatedPoints();
}
else if(event->key() == '+' )
{
ROS_INFO_STREAM_NAMED( "SelectedPointsPublisher.processKeyEvent",
"Adding the points to the accumulated point cloud. Removing them from the original point cloud. Clearing the LAST selected area.");
rviz::SelectionManager* sel_manager = context_->getSelectionManager();
rviz::M_Picked selection = sel_manager->getSelection();
sel_manager->removeSelection(selection);
visualization_msgs::Marker marker;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
marker.header.frame_id = context_->getFixedFrame().toStdString().c_str();
marker.header.stamp = ros::Time::now();
marker.ns = "basic_shapes";
marker.id = 0;
marker.type = visualization_msgs::Marker::CUBE;
marker.action = visualization_msgs::Marker::DELETE;
marker.lifetime = ros::Duration();
bb_marker_pub_.publish(marker);

// First remove the selected point of the original point cloud so that they cannot be selected again:
pcl::PointCloud<pcl::PointXYZRGB> temp_new_pc;
extract_indices_filter_->setKeepOrganized(true);
extract_indices_filter_->setNegative(true);
temp_new_pc.header = this->current_pc_->header;
extract_indices_filter_->filter(temp_new_pc);
pcl::copyPointCloud(temp_new_pc, *this->current_pc_);

sensor_msgs::PointCloud2 partial_pc_ros;
pcl::toROSMsg(*this->current_pc_, partial_pc_ros);
partial_pc_pub_.publish(partial_pc_ros);

// Then I copy the that were selected before in the accumulated point cloud (except if it is the first selected segment, then I copy the whole dense point cloud)
if(this->accumulated_segment_pc_->points.size() == 0)
{
pcl::copyPointCloud(*this->selected_segment_pc_, *this->accumulated_segment_pc_);
this->num_acc_points_ = this->num_selected_points_;
}else{
//Both are dense organized point clouds and the points were selected_segment_pc_ has a not NaN value must be NaN in the accumulated_segment_pc_ (and viceversa)
for(unsigned int idx_selected = 0; idx_selected < this->selected_segment_pc_->points.size(); idx_selected++)
{
if(pcl::isFinite(this->selected_segment_pc_->points.at(idx_selected)))
{
this->accumulated_segment_pc_->points.at(idx_selected) = this->selected_segment_pc_->points.at(idx_selected);
}
}

this->num_acc_points_ += this->num_selected_points_;
}

selected_segment_pc_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
this->num_selected_points_ = 0;

ROS_INFO_STREAM_NAMED("SelectedPointsPublisher._processSelectedAreaAndFindPoints",
"Number of accumulated points (not published): "<< this->num_acc_points_);

ROS_INFO_STREAM_NAMED( "SelectedPointsPublisher.processKeyEvent",
"Select a new area and press '+' again to accumulate more points, or press 'y' to publish the accumulated point cloud.");
}
}
}
Expand All @@ -123,19 +195,19 @@ int SelectedPointsPublisher::processMouseEvent( rviz::ViewportMouseEvent& event
if( event.leftUp() )
{
ROS_INFO_STREAM_NAMED( "SelectedPointsPublisher.processKeyEvent", "Using selected area to find a new bounding box and publish the points inside of it");
this->_processSelectedAreaAndPublishPoints();
this->_processSelectedAreaAndFindPoints();
}
}
return flags;
}

int SelectedPointsPublisher::_processSelectedAreaAndPublishPoints()
int SelectedPointsPublisher::_processSelectedAreaAndFindPoints()
{
rviz::SelectionManager* sel_manager = context_->getSelectionManager();
rviz::M_Picked selection = sel_manager->getSelection();
rviz::PropertyTreeModel *model = sel_manager->getPropertyModel();
int num_points = model->rowCount();
ROS_INFO_STREAM_NAMED( "_processSelectedArea.processMouseEvent", "Number of points in the selected area: " << num_points);
ROS_INFO_STREAM_NAMED( "SelectedPointsPublisher._processSelectedAreaAndFindPoints", "Number of points in the selected area: " << num_points);

// Generate a ros point cloud message with the selected points in rviz
sensor_msgs::PointCloud2 selected_points_ros;
Expand Down Expand Up @@ -239,20 +311,24 @@ int SelectedPointsPublisher::_processSelectedAreaAndPublishPoints()
pcl::PointIndices::Ptr inliers( new pcl::PointIndices() );
crop_filter.filter(inliers->indices);

pcl::PointCloud<pcl::PointXYZRGB>::Ptr selected_segment_pc(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::ExtractIndices<pcl::PointXYZRGB> extract_indices_filter;
extract_indices_filter.setIndices(inliers);
extract_indices_filter.setKeepOrganized(true);
extract_indices_filter.setInputCloud(this->current_pc_);
selected_segment_pc->header = this->current_pc_->header;
extract_indices_filter.filter(*selected_segment_pc);
this->selected_segment_pc_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);

ROS_INFO_STREAM_NAMED("_processSelectedArea.processMouseEvent", "Real number of points of the point cloud in the selected area: "<< inliers->indices.size());
extract_indices_filter_.reset(new pcl::ExtractIndices<pcl::PointXYZRGB>());
extract_indices_filter_->setIndices(inliers);
extract_indices_filter_->setKeepOrganized(true);
extract_indices_filter_->setInputCloud(this->current_pc_);
this->selected_segment_pc_->header = this->current_pc_->header;
extract_indices_filter_->filter(*this->selected_segment_pc_);

sensor_msgs::PointCloud2 selected_segment_ros;
pcl::toROSMsg(*selected_segment_pc, selected_segment_ros);
this->real_selected_pub_.publish(selected_segment_ros);
this->num_selected_points_ = inliers->indices.size();

ROS_INFO_STREAM_NAMED("SelectedPointsPublisher._processSelectedAreaAndFindPoints",
"Real number of points of the point cloud in the selected area (NOT published, NOT added): "<< this->num_selected_points_);
ROS_INFO_STREAM_NAMED("SelectedPointsPublisher._processSelectedAreaAndFindPoints",
"\n\tPress '+' to add the current selection to the accumulated point cloud." << std::endl<<
"\tPress 'c' to clear the selection. " <<std::endl<<
"\tPress 'r' to recompute the bounding box with the current inlier points in the bounding box."<< std::endl<<
"\tSelect a different region (or an empty region) to clean this LAST selection.");

// Publish the bounding box as a rectangular marker
visualization_msgs::Marker marker;
Expand All @@ -278,8 +354,25 @@ int SelectedPointsPublisher::_processSelectedAreaAndPublishPoints()
marker.color.b = 0.0f;
marker.color.a = 0.5;
marker.lifetime = ros::Duration();

bb_marker_pub_.publish(marker);

return 0;
}

int SelectedPointsPublisher::_publishAccumulatedPoints()
{
ROS_INFO_STREAM_NAMED("SelectedPointsPublisher._processSelectedAreaAndFindPoints",
"Publishing the accumulated point cloud (" << this->num_acc_points_ < " points)");
accumulated_segment_pc_->header = this->current_pc_->header;
sensor_msgs::PointCloud2 accumulated_segment_ros;
pcl::toROSMsg(*this->accumulated_segment_pc_, accumulated_segment_ros);
real_selected_pub_.publish(accumulated_segment_ros);

ROS_WARN_STREAM_NAMED("SelectedPointsPublisher._processSelectedAreaAndFindPoints",
"Cleaning the accumulated point cloud after publishing");
accumulated_segment_pc_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
this->num_acc_points_ = 0;
return 0;
}

} // end namespace rviz_plugin_selected_points_topic
Expand Down

0 comments on commit 4784d87

Please sign in to comment.