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 #include <jsk_pcl_ros_utils/static_polygon_array_publisher.h>
00036 #include <pluginlib/class_list_macros.h>
00037 #include <jsk_topic_tools/rosparam_utils.h>
00038
00039 namespace jsk_pcl_ros_utils
00040 {
00041
00042 void StaticPolygonArrayPublisher::onInit()
00043 {
00044 ConnectionBasedNodelet::onInit();
00045 pnh_->param("use_periodic", use_periodic_, false);
00046 pnh_->param("use_message", use_message_, false);
00047 pnh_->param("use_trigger", use_trigger_, false);
00048 pnh_->param("periodic_rate", periodic_rate_, 10.0);
00049
00050 bool frame_id_read_p
00051 = jsk_topic_tools::readVectorParameter(*pnh_, "frame_ids",
00052 frame_ids_);
00053 if (!frame_id_read_p) {
00054 NODELET_FATAL("failed to read frame_ids from ~frame_ids");
00055 return;
00056 }
00057
00058 bool polygon_read_p = readPolygonArray("polygon_array");
00059 if (!polygon_read_p) {
00060 NODELET_FATAL("failed to read polygons from ~polygon_array");
00061 return;
00062 }
00063
00064 if (frame_ids_.size() != polygons_.polygons.size()) {
00065 NODELET_FATAL("the size of frame_ids(%lu) does not match the size of polygons(%lu)",
00066 frame_ids_.size(), polygons_.polygons.size());
00067 return;
00068 }
00069 else {
00070 for (size_t i = 0; i < frame_ids_.size(); i++) {
00071 polygons_.polygons[i].header.frame_id = frame_ids_[i];
00072 coefficients_.coefficients[i].header.frame_id = frame_ids_[i];
00073 }
00074 }
00075
00076 if (!use_periodic_ && !use_message_ && !use_trigger_) {
00077 NODELET_FATAL("~use_periodic, ~use_trigger nor ~use_message is not true");
00078 return;
00079 }
00080 polygons_.header.frame_id = frame_ids_[0];
00081 coefficients_.header.frame_id = frame_ids_[0];
00082
00083 if (!use_periodic_) {
00084 polygon_pub_ = advertise<jsk_recognition_msgs::PolygonArray>(
00085 *pnh_, "output_polygons", 1);
00086 coefficients_pub_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
00087 *pnh_, "output_coefficients", 1);
00088 }
00089 else {
00090 polygon_pub_ = pnh_->advertise<jsk_recognition_msgs::PolygonArray>(
00091 "output_polygons", 1);
00092 coefficients_pub_ = pnh_->advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
00093 "output_coefficients", 1);
00094 subscribe();
00095 timer_ = pnh_->createTimer(ros::Duration(1.0 / periodic_rate_), &StaticPolygonArrayPublisher::timerCallback, this);
00096 }
00097 onInitPostProcess();
00098 }
00099
00100 void StaticPolygonArrayPublisher::subscribe()
00101 {
00102 if (use_message_) {
00103 sub_ = pnh_->subscribe("input", 1, &StaticPolygonArrayPublisher::inputCallback, this);
00104 }
00105 if (use_trigger_) {
00106 sub_input_.subscribe(*pnh_, "input", 1);
00107 sub_trigger_.subscribe(*pnh_, "trigger", 1);
00108 sync_
00109 = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00110 sync_->connectInput(sub_input_, sub_trigger_);
00111 sync_->registerCallback(boost::bind(
00112 &StaticPolygonArrayPublisher::triggerCallback,
00113 this, _1, _2));
00114 }
00115 }
00116
00117 void StaticPolygonArrayPublisher::unsubscribe()
00118 {
00119 if (use_message_) {
00120 sub_.shutdown();
00121 }
00122 if (use_trigger_) {
00123 sub_input_.unsubscribe();
00124 sub_trigger_.unsubscribe();
00125 }
00126 }
00127
00128 void StaticPolygonArrayPublisher::triggerCallback(
00129 const sensor_msgs::PointCloud2::ConstPtr& input,
00130 const jsk_recognition_msgs::Int32Stamped::ConstPtr& trigger)
00131 {
00132 publishPolygon(input->header.stamp);
00133 }
00134
00135 PCLModelCoefficientMsg StaticPolygonArrayPublisher::polygonToModelCoefficients(const geometry_msgs::PolygonStamped& polygon)
00136 {
00137 Eigen::Vector3d A, B, C;
00138 A[0] = polygon.polygon.points[0].x;
00139 A[1] = polygon.polygon.points[0].y;
00140 A[2] = polygon.polygon.points[0].z;
00141 B[0] = polygon.polygon.points[1].x;
00142 B[1] = polygon.polygon.points[1].y;
00143 B[2] = polygon.polygon.points[1].z;
00144 C[0] = polygon.polygon.points[2].x;
00145 C[1] = polygon.polygon.points[2].y;
00146 C[2] = polygon.polygon.points[2].z;
00147 Eigen::Vector3d n = (B - A).cross(C - A).normalized();
00148 double a = n[0];
00149 double b = n[1];
00150 double c = n[2];
00151 double d = -(a * A[0] + b * A[1] + c * A[2]);
00152 PCLModelCoefficientMsg coefficient;
00153 coefficient.header = polygon.header;
00154 coefficient.values.push_back(a);
00155 coefficient.values.push_back(b);
00156 coefficient.values.push_back(c);
00157 coefficient.values.push_back(d);
00158 return coefficient;
00159 }
00160
00161
00162
00163
00164
00165 bool StaticPolygonArrayPublisher::readPolygonArray(const std::string& param_name)
00166 {
00167 if (pnh_->hasParam(param_name)) {
00168 XmlRpc::XmlRpcValue v;
00169 pnh_->param(param_name, v, v);
00170 if (v.getType() == XmlRpc::XmlRpcValue::TypeArray) {
00171 for (size_t toplevel_i = 0; toplevel_i < v.size(); toplevel_i++) {
00172 XmlRpc::XmlRpcValue polygon_v = v[toplevel_i];
00173 geometry_msgs::PolygonStamped polygon;
00174 if (polygon_v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
00175 polygon_v.size() >= 3) {
00176 for (size_t secondlevel_i = 0; secondlevel_i < polygon_v.size(); secondlevel_i++) {
00177 XmlRpc::XmlRpcValue vertex_v = polygon_v[secondlevel_i];
00178 if (vertex_v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
00179 vertex_v.size() == 3 ) {
00180 double x = getXMLDoubleValue(vertex_v[0]);
00181 double y = getXMLDoubleValue(vertex_v[1]);
00182 double z = getXMLDoubleValue(vertex_v[2]);
00183 geometry_msgs::Point32 point;
00184 point.x = x;
00185 point.y = y;
00186 point.z = z;
00187 polygon.polygon.points.push_back(point);
00188 }
00189 else {
00190 NODELET_FATAL("%s[%lu][%lu] is not array or the length is not 3",
00191 param_name.c_str(), toplevel_i, secondlevel_i);
00192 return false;
00193 }
00194 }
00195 polygons_.polygons.push_back(polygon);
00196
00197 coefficients_.coefficients.push_back(polygonToModelCoefficients(polygon));
00198 }
00199 else {
00200 NODELET_FATAL("%s[%lu] is not array or not enough points", param_name.c_str(), toplevel_i);
00201 return false;
00202 }
00203 }
00204 return true;
00205 }
00206 else {
00207 NODELET_FATAL("%s is not array", param_name.c_str());
00208 return false;
00209 }
00210 }
00211 else {
00212 NODELET_FATAL("no %s is available on parameter server", param_name.c_str());
00213 return false;
00214 }
00215 return true;
00216 }
00217
00218 double StaticPolygonArrayPublisher::getXMLDoubleValue(XmlRpc::XmlRpcValue val) {
00219 switch(val.getType()) {
00220 case XmlRpc::XmlRpcValue::TypeInt:
00221 return (double)((int)val);
00222 case XmlRpc::XmlRpcValue::TypeDouble:
00223 return (double)val;
00224 default:
00225 return 0;
00226 }
00227 }
00228
00229
00230 void StaticPolygonArrayPublisher::inputCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00231 {
00232 publishPolygon(msg->header.stamp);
00233 }
00234
00235 void StaticPolygonArrayPublisher::timerCallback(const ros::TimerEvent& event)
00236 {
00237 publishPolygon(event.current_expected);
00238 }
00239
00240 void StaticPolygonArrayPublisher::publishPolygon(const ros::Time& stamp)
00241 {
00242 polygons_.header.stamp = stamp;
00243 for (size_t i = 0; i < polygons_.polygons.size(); i++) {
00244 polygons_.polygons[i].header.stamp = stamp;
00245 }
00246
00247 coefficients_.header.stamp = stamp;
00248 for (size_t i = 0; i < coefficients_.coefficients.size(); i++) {
00249 coefficients_.coefficients[i].header.stamp = stamp;
00250 }
00251
00252 polygon_pub_.publish(polygons_);
00253 coefficients_pub_.publish(coefficients_);
00254 }
00255
00256 }
00257
00258 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::StaticPolygonArrayPublisher,
00259 nodelet::Nodelet);
00260