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/spherical_pointcloud_simulator.h"
00037
00038 namespace jsk_pcl_ros_utils
00039 {
00040 void SphericalPointCloudSimulator::onInit()
00041 {
00042 DiagnosticNodelet::onInit();
00043 pnh_->getParam("frame_id", frame_id_);
00044 rotate_velocity_ = 0.5;
00045 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00046 typename dynamic_reconfigure::Server<Config>::CallbackType f =
00047 boost::bind (&SphericalPointCloudSimulator::configCallback, this, _1, _2);
00048 srv_->setCallback (f);
00049
00050 double rate;
00051 if (pnh_->getParam("rate", rate)) {
00052 timer_ = pnh_->createTimer(
00053 ros::Duration(1 / rate), boost::bind(
00054 &SphericalPointCloudSimulator::timerCallback,
00055 this,
00056 _1));
00057 }
00058 pub_ = advertise<sensor_msgs::PointCloud2>(
00059 *pnh_, "output", 1);
00060 onInitPostProcess();
00061 }
00062
00063 void SphericalPointCloudSimulator::subscribe()
00064 {
00065 sub_ = pnh_->subscribe(
00066 "input", 1, &SphericalPointCloudSimulator::generate, this);
00067 }
00068
00069 void SphericalPointCloudSimulator::unsubscribe()
00070 {
00071 sub_.shutdown();
00072 }
00073
00074 void SphericalPointCloudSimulator::configCallback(
00075 Config &config, uint32_t level)
00076 {
00077 boost::mutex::scoped_lock lock(mutex_);
00078 r_ = config.r;
00079 min_phi_ = config.min_phi;
00080 max_phi_ = config.max_phi;
00081 scan_range_ = config.scan_range;
00082 scan_num_ = config.scan_num;
00083 fps_ = config.fps;
00084 }
00085
00086 void SphericalPointCloudSimulator::timerCallback(
00087 const ros::TimerEvent& event)
00088 {
00089
00090 sensor_msgs::PointCloud2 dummy_cloud;
00091 dummy_cloud.header.stamp = ros::Time::now();
00092 dummy_cloud.header.frame_id = "map";
00093 generate(boost::make_shared<sensor_msgs::PointCloud2>(dummy_cloud));
00094 }
00095
00096 void SphericalPointCloudSimulator::generate(
00097 const sensor_msgs::PointCloud2::ConstPtr& msg)
00098 {
00099 boost::mutex::scoped_lock lock(mutex_);
00100 vital_checker_->poke();
00101 pcl::PointCloud<pcl::PointXYZ>::Ptr
00102 cloud (new pcl::PointCloud<pcl::PointXYZ>);
00103
00104 int phi_num = 2 * M_PI / rotate_velocity_ * fps_;
00105 int point_num = phi_num * scan_num_;
00106 cloud->points.resize(point_num);
00107 int i = 0;
00108 for (int phi_i = 0; phi_i < phi_num; phi_i++) {
00109 double phi = (float)phi_i / phi_num * (max_phi_ - min_phi_) + min_phi_;
00110 Eigen::Affine3f trans = getPlane(phi);
00111 for (int theta_i = 0; theta_i < scan_num_; theta_i++) {
00112 double theta = theta_i * scan_range_ / scan_num_ - scan_range_ / 2.0;
00113 pcl::PointXYZ p = getPoint(r_, theta, trans);
00114 cloud->points[i] = p;
00115 i++;
00116 }
00117 }
00118 sensor_msgs::PointCloud2 ros_cloud;
00119 pcl::toROSMsg(*cloud, ros_cloud);
00120 ros_cloud.header.stamp = msg->header.stamp;
00121 if (!frame_id_.empty()) {
00122 ros_cloud.header.frame_id = frame_id_;
00123 }
00124 else {
00125 ros_cloud.header.frame_id = cloud->header.frame_id;
00126 }
00127 pub_.publish(ros_cloud);
00128 }
00129
00130 Eigen::Affine3f SphericalPointCloudSimulator::getPlane(double phi)
00131 {
00132 Eigen::Vector3f norm(0, sin(phi), cos(phi));
00133 Eigen::Quaternionf rot;
00134 rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), norm);
00135 Eigen::Affine3f trans = Eigen::Affine3f::Identity();
00136 trans = trans * rot;
00137 return trans;
00138 }
00139
00140 pcl::PointXYZ SphericalPointCloudSimulator::getPoint(
00141 double r, double theta, const Eigen::Affine3f& trans)
00142 {
00143 Eigen::Vector3f local_p(r * cos(theta), r * sin(theta), 0);
00144 Eigen::Vector3f world_p = trans * local_p;
00145 pcl::PointXYZ p;
00146 p.getVector3fMap() = world_p;
00147 return p;
00148 }
00149
00150 }
00151
00152 #include <pluginlib/class_list_macros.h>
00153 PLUGINLIB_EXPORT_CLASS (
00154 jsk_pcl_ros_utils::SphericalPointCloudSimulator, nodelet::Nodelet);