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 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros_utils/planar_pointcloud_simulator.h"
00037 #include <image_geometry/pinhole_camera_model.h>
00038
00039 namespace jsk_pcl_ros_utils
00040 {
00041 PlanarPointCloudSimulator::PlanarPointCloudSimulator()
00042 {
00043
00044 }
00045
00046 PlanarPointCloudSimulator::~PlanarPointCloudSimulator()
00047 {
00048
00049 }
00050
00051 void PlanarPointCloudSimulator::generate(
00052 const sensor_msgs::CameraInfo& info, double distance,
00053 pcl::PointCloud<pcl::PointXYZ>& cloud)
00054 {
00055 image_geometry::PinholeCameraModel model;
00056 model.fromCameraInfo(info);
00057 cloud.points.resize(info.width * info.height);
00058 cloud.is_dense = true;
00059 for (size_t j = 0; j < info.height; j++) {
00060 for (size_t i = 0; i < info.width; i++) {
00061 cv::Point3d ray = model.projectPixelTo3dRay(cv::Point2d(i, j));
00062 pcl::PointXYZ p;
00063 p.x = ray.x * distance;
00064 p.y = ray.y * distance;
00065 p.z = ray.z * distance;
00066 cloud.points[j * info.width + i] = p;
00067 }
00068 }
00069 cloud.width = info.width;
00070 cloud.height = info.height;
00071 }
00072
00073 void PlanarPointCloudSimulatorNodelet::onInit()
00074 {
00075 DiagnosticNodelet::onInit();
00076 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00077 typename dynamic_reconfigure::Server<Config>::CallbackType f =
00078 boost::bind (&PlanarPointCloudSimulatorNodelet::configCallback, this, _1, _2);
00079 srv_->setCallback (f);
00080
00081 pub_ = advertise<sensor_msgs::PointCloud2>(
00082 *pnh_, "output", 1);
00083
00084 onInitPostProcess();
00085 }
00086
00087 void PlanarPointCloudSimulatorNodelet::subscribe()
00088 {
00089 sub_ = pnh_->subscribe(
00090 "input", 1, &PlanarPointCloudSimulatorNodelet::generate, this);
00091 }
00092
00093 void PlanarPointCloudSimulatorNodelet::unsubscribe()
00094 {
00095 sub_.shutdown();
00096 }
00097
00098 void PlanarPointCloudSimulatorNodelet::configCallback(
00099 Config &config, uint32_t level)
00100 {
00101 boost::mutex::scoped_lock lock(mutex_);
00102 distance_ = config.distance;
00103 }
00104
00105 void PlanarPointCloudSimulatorNodelet::generate(
00106 const sensor_msgs::CameraInfo::ConstPtr& info_msg)
00107 {
00108 boost::mutex::scoped_lock lock(mutex_);
00109 vital_checker_->poke();
00110 pcl::PointCloud<pcl::PointXYZ>::Ptr
00111 cloud(new pcl::PointCloud<pcl::PointXYZ>);
00112 impl_.generate(*info_msg, distance_, *cloud);
00113 sensor_msgs::PointCloud2 ros_cloud;
00114 pcl::toROSMsg(*cloud, ros_cloud);
00115 ros_cloud.header = info_msg->header;
00116 pub_.publish(ros_cloud);
00117 }
00118
00119 }
00120
00121 #include <pluginlib/class_list_macros.h>
00122 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PlanarPointCloudSimulatorNodelet,
00123 nodelet::Nodelet);