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
00031
00032
00033
00034
00035
00036
00037
00046
00047 #include <ros/ros.h>
00048
00049 #include "pcl/io/pcd_io.h"
00050 #include "pcl/point_types.h"
00051 #include "pcl/common/common.h"
00052
00053 #include <tf/transform_listener.h>
00054 #include <visualization_msgs/Marker.h>
00055
00056 using namespace std;
00057
00058 class HullContractNode
00059 {
00060 protected:
00061 ros::NodeHandle nh_;
00062
00063 public:
00064 string output_cloud_topic_, input_cloud_topic_;
00065
00066 ros::Subscriber sub_;
00067 ros::Publisher pub_;
00068 ros::Publisher vis_pub_;
00069
00070 sensor_msgs::PointCloud2 output_cloud_;
00071 pcl::PointCloud<pcl::PointXYZ> cloud_in_;
00072 pcl::PointXYZ point_min_;
00073 pcl::PointXYZ point_max_;
00074 pcl::PointXYZ point_center_;
00075 visualization_msgs::Marker marker_;
00076
00077 double padding_, offset_x_;
00079 HullContractNode (ros::NodeHandle &n) : nh_(n)
00080 {
00081
00082 nh_.param("input_cloud_topic", input_cloud_topic_, std::string("cloud_pcd"));
00083 nh_.param("padding", padding_, 0.8);
00084
00085
00086 nh_.param("offset_x", offset_x_, 0.02);
00087 output_cloud_topic_ = input_cloud_topic_ + "_padded";
00088 sub_ = nh_.subscribe (input_cloud_topic_, 1, &HullContractNode::cloud_cb, this);
00089 ROS_INFO ("[HullContractNode:] Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ());
00090 pub_ = nh_.advertise<sensor_msgs::PointCloud2>(output_cloud_topic_, 1);
00091 ROS_INFO ("[HullContractNode:] Will be publishing data on topic %s.", nh_.resolveName (output_cloud_topic_).c_str ());
00092 vis_pub_ = nh_.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
00093 }
00094
00096
00097 void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
00098 {
00099 pcl::fromROSMsg(*pc, cloud_in_);
00100
00101 getMinMax3D (cloud_in_, point_min_, point_max_);
00102
00103 point_center_.x = (point_max_.x + point_min_.x)/2;
00104 point_center_.y = (point_max_.y + point_min_.y)/2;
00105 point_center_.z = (point_max_.z + point_min_.z)/2;
00106
00107
00108
00109
00110 for (unsigned long i = 0; i < cloud_in_.points.size(); i++)
00111 {
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125 double dist_to_center = sqrt((point_center_.x - cloud_in_.points[i].x) * (point_center_.x - cloud_in_.points[i].x) +
00126 (point_center_.y - cloud_in_.points[i].y) * (point_center_.y - cloud_in_.points[i].y));
00127 ROS_DEBUG("[HullContractNode:] Dist to center: %lf", dist_to_center);
00128 double angle;
00129 angle= atan2((cloud_in_.points[i].y - point_center_.y), (cloud_in_.points[i].x - point_center_.x));
00130 double new_dist_to_center = padding_ * dist_to_center;
00131 cloud_in_.points[i].y = point_center_.y + sin(angle) * new_dist_to_center;
00132 cloud_in_.points[i].x = point_center_.x + cos(angle) * new_dist_to_center;
00133 cloud_in_.points[i].x = cloud_in_.points[i].x - offset_x_;
00134 }
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146 pcl::toROSMsg (cloud_in_, output_cloud_);
00147 ROS_INFO("[HullContractNode:] Published contracted hull to topic %s", output_cloud_topic_.c_str());
00148 pub_.publish (output_cloud_);
00149
00150 marker_.header.frame_id = output_cloud_.header.frame_id;
00151 marker_.header.stamp = ros::Time();
00152 marker_.ns = "my_namespace";
00153 marker_.id = 0;
00154 marker_.type = visualization_msgs::Marker::SPHERE;
00155 marker_.action = visualization_msgs::Marker::ADD;
00156 marker_.pose.position.x = point_center_.x;
00157 marker_.pose.position.y = point_center_.y;
00158 marker_.pose.position.z = point_center_.z;
00159 marker_.pose.orientation.x = 0.0;
00160 marker_.pose.orientation.y = 0.0;
00161 marker_.pose.orientation.z = 0.0;
00162 marker_.pose.orientation.w = 1.0;
00163 marker_.scale.x = 0.1;
00164 marker_.scale.y = 0.1;
00165 marker_.scale.z = 0.1;
00166 marker_.color.a = 1.0;
00167 marker_.color.r = 0.0;
00168 marker_.color.g = 1.0;
00169 marker_.color.b = 0.0;
00170 vis_pub_.publish( marker_ );
00171 }
00172
00173 void publish_center_radius()
00174 {
00175
00176 }
00177 };
00178
00179 int main (int argc, char** argv)
00180 {
00181 ros::init (argc, argv, "transform_pointcloud_node");
00182 ros::NodeHandle n("~");
00183 HullContractNode tp(n);
00184 ros::spin ();
00185 return (0);
00186 }