Go to the documentation of this file.00001
00018 #include <asr_halcon_bridge/halcon_pointcloud.h>
00019 #include <boost/make_shared.hpp>
00020
00021 #include <sensor_msgs/PointCloud2.h>
00022 #include <sensor_msgs/PointField.h>
00023
00024 namespace halcon_bridge {
00025
00026 int getSizeFromDatatype(int datatype) {
00027 if (datatype == sensor_msgs::PointField::INT8) return 1;
00028 if (datatype == sensor_msgs::PointField::UINT8) return 1;
00029 if (datatype == sensor_msgs::PointField::INT16) return 2;
00030 if (datatype == sensor_msgs::PointField::UINT16) return 2;
00031 if (datatype == sensor_msgs::PointField::INT32) return 4;
00032 if (datatype == sensor_msgs::PointField::UINT32) return 4;
00033 if (datatype == sensor_msgs::PointField::FLOAT32) return 4;
00034 if (datatype == sensor_msgs::PointField::FLOAT64) return 8;
00035
00036 return -1;
00037 }
00038
00039 HalconPointcloud::~HalconPointcloud() {
00040 delete model;
00041 curvature.Clear();
00042 }
00043
00044 sensor_msgs::PointCloud2Ptr HalconPointcloud::toPointcloudMsg() const {
00045 sensor_msgs::PointCloud2Ptr ptr = boost::make_shared<sensor_msgs::PointCloud2>();
00046 toPointcloudMsg(*ptr);
00047 return ptr;
00048 }
00049
00050 void HalconPointcloud::toPointcloudMsg(sensor_msgs::PointCloud2& ros_pointcloud) const {
00051 ros_pointcloud.header = header;
00052 ros_pointcloud.height = 1;
00053 ros_pointcloud.width = (int)model->GetObjectModel3dParams("num_points")[0];
00054 ros_pointcloud.is_dense = false;
00055 ros_pointcloud.is_bigendian = false;
00056 bool has_normals = ((HalconCpp::HString)model->GetObjectModel3dParams("has_point_normals")) == HalconCpp::HString("true");
00057 std::vector<sensor_msgs::PointField> fields;
00058 if (has_normals) {
00059 ros_pointcloud.point_step = 28;
00060 fields = std::vector<sensor_msgs::PointField>(7);
00061 } else {
00062 ros_pointcloud.point_step = 12;
00063 fields = std::vector<sensor_msgs::PointField>(3);
00064 }
00065
00066 ros_pointcloud.row_step = ros_pointcloud.width * ros_pointcloud.point_step;
00067
00068
00069 sensor_msgs::PointField x_field;
00070 x_field.name = "x";
00071 x_field.count = 1;
00072 x_field.datatype = sensor_msgs::PointField::FLOAT32;
00073 x_field.offset = 0;
00074 fields[0] = x_field;
00075
00076 sensor_msgs::PointField y_field;
00077 y_field.name = "y";
00078 y_field.count = 1;
00079 y_field.datatype = sensor_msgs::PointField::FLOAT32;
00080 y_field.offset = 4;
00081 fields[1] = y_field;
00082
00083 sensor_msgs::PointField z_field;
00084 z_field.name = "z";
00085 z_field.count = 1;
00086 z_field.datatype = sensor_msgs::PointField::FLOAT32;
00087 z_field.offset = 8;
00088 fields[2] = z_field;
00089
00090 if (has_normals) {
00091 sensor_msgs::PointField x_normals_field;
00092 x_normals_field.name = "normal_x";
00093 x_normals_field.count = 1;
00094 x_normals_field.datatype = sensor_msgs::PointField::FLOAT32;
00095 x_normals_field.offset = 12;
00096 fields[3] = x_normals_field;
00097
00098 sensor_msgs::PointField y_normals_field;
00099 y_normals_field.name = "normal_y";
00100 y_normals_field.count = 1;
00101 y_normals_field.datatype = sensor_msgs::PointField::FLOAT32;
00102 y_normals_field.offset = 16;
00103 fields[4] = y_normals_field;
00104
00105 sensor_msgs::PointField z_normals_field;
00106 z_normals_field.name = "normal_z";
00107 z_normals_field.count = 1;
00108 z_normals_field.datatype = sensor_msgs::PointField::FLOAT32;
00109 z_normals_field.offset = 20;
00110 fields[5] = z_normals_field;
00111
00112 sensor_msgs::PointField curvature;
00113 curvature.name = "curvature";
00114 curvature.count = 1;
00115 curvature.datatype = sensor_msgs::PointField::FLOAT32;
00116 curvature.offset = 24;
00117 fields[6] = curvature;
00118 }
00119
00120
00121 ros_pointcloud.fields = fields;
00122
00123
00124 HalconCpp::HTuple x_values = model->GetObjectModel3dParams("point_coord_x");
00125 HalconCpp::HTuple y_values = model->GetObjectModel3dParams("point_coord_y");
00126 HalconCpp::HTuple z_values = model->GetObjectModel3dParams("point_coord_z");
00127
00128 HalconCpp::HTuple x_normal_values;
00129 HalconCpp::HTuple y_normal_values;
00130 HalconCpp::HTuple z_normal_values;
00131
00132
00133 float *interleavedPoints;
00134
00135 if (has_normals) {
00136 x_normal_values = model->GetObjectModel3dParams("point_normal_x");
00137 y_normal_values = model->GetObjectModel3dParams("point_normal_y");
00138 z_normal_values = model->GetObjectModel3dParams("point_normal_z");
00139 interleavedPoints = new float[ros_pointcloud.width * 7];
00140 } else {
00141 interleavedPoints = new float[ros_pointcloud.width * 3];
00142 }
00143
00144 for (size_t i = 0; i < ros_pointcloud.width; i++) {
00145 interleavedPoints[i * 3] = (float)x_values[i];
00146 interleavedPoints[i * 3 + 1] = (float)y_values[i];
00147 interleavedPoints[i * 3 + 2] = (float)z_values[i];
00148 if (has_normals) {
00149 interleavedPoints[i * 3 + 3] = (float)x_normal_values[i];
00150 interleavedPoints[i * 3 + 4] = (float)y_normal_values[i];
00151 interleavedPoints[i * 3 + 5] = (float)z_normal_values[i];
00152 interleavedPoints[i * 3 + 6] = (float)(curvature)[i];
00153 }
00154 }
00155
00156 size_t size;
00157 if (has_normals) {
00158 size = ros_pointcloud.width * 7 * 4;
00159 } else {
00160 size = ros_pointcloud.width * 3 * 4;
00161 }
00162 ros_pointcloud.data.resize(size);
00163
00164 memcpy((float*)(&ros_pointcloud.data[0]), &interleavedPoints[0], size);
00165
00166
00167 }
00168
00169
00170
00171 HalconPointcloudPtr toHalconCopy(const sensor_msgs::PointCloud2ConstPtr& source) {
00172 return toHalconCopy(*source);
00173 }
00174
00175 HalconPointcloudPtr toHalconCopy(const sensor_msgs::PointCloud2& source) {
00176 HalconPointcloudPtr ptr = boost::make_shared<HalconPointcloud>();
00177 ptr->header = source.header;
00178
00179
00180 int offset_x, offset_y, offset_z, offset_x_normal, offset_y_normal, offset_z_normal, offset_curvature;
00181 offset_x = offset_y = offset_z = offset_x_normal = offset_y_normal = offset_z_normal = offset_curvature = 0;
00182 int count_x_normal, count_y_normal, count_z_normal, count_curvature;
00183 count_x_normal = count_y_normal = count_z_normal = count_curvature = 0;
00184
00185 for (unsigned int i = 0; i < source.fields.size(); i++) {
00186 sensor_msgs::PointField field = source.fields[i];
00187 if (field.name == "x") {
00188 offset_x = field.offset;
00189 }
00190 if (field.name == "y") {
00191 offset_y = field.offset;
00192 }
00193 if (field.name == "z") {
00194 offset_z = field.offset;
00195 }
00196 if (field.name == "normal_x") {
00197 offset_x_normal = field.offset;
00198 count_x_normal = field.count;
00199 }
00200 if (field.name == "normal_y") {
00201 offset_y_normal = field.offset;
00202 count_y_normal = field.count;
00203 }
00204 if (field.name == "normal_z") {
00205 offset_z_normal = field.offset;
00206 count_z_normal = field.count;
00207 }
00208 if (field.name == "curvature") {
00209 offset_curvature = field.offset;
00210 count_curvature = field.count;
00211 }
00212
00213 }
00214
00215 HalconCpp::HTuple x_coords = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0);
00216 HalconCpp::HTuple y_coords = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0);
00217 HalconCpp::HTuple z_coords = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0);
00218
00219 HalconCpp::HTuple x_normals = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0);
00220 HalconCpp::HTuple y_normals = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0);
00221 HalconCpp::HTuple z_normals = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0);
00222
00223 HalconCpp::HTuple curvature = HalconCpp::HTuple::TupleGenConst((int)(source.width * source.height), 0);
00224
00225 for (int i = 0; i < x_coords.Length(); i++) {
00226 x_coords[i] = *(float*)&source.data[(i * source.point_step) + offset_x];
00227 y_coords[i] = *(float*)&source.data[(i * source.point_step) + offset_y];
00228 z_coords[i] = *(float*)&source.data[(i * source.point_step) + offset_z];
00229
00230 if ((count_x_normal > 0) && (count_y_normal > 0) && (count_z_normal > 0)) {
00231 x_normals[i] = *(float*)&source.data[(i * source.point_step) + offset_x_normal];
00232 y_normals[i] = *(float*)&source.data[(i * source.point_step) + offset_y_normal];
00233 z_normals[i] = *(float*)&source.data[(i * source.point_step) + offset_z_normal];
00234 }
00235
00236 }
00237 ptr->model = new HalconCpp::HObjectModel3D(x_coords, y_coords, z_coords);
00238
00239 if (source.fields.size() > 4) {
00240 HalconCpp::HTuple attrib_names("point_normal_x");
00241 attrib_names.Append("point_normal_y");
00242 attrib_names.Append("point_normal_z");
00243 HalconCpp::HTuple attrib_values = (x_normals.TupleConcat(y_normals)).TupleConcat(z_normals);
00244 HalconCpp::HObjectModel3D p_n_model = ptr->model->SetObjectModel3dAttrib(attrib_names, "", attrib_values);
00245 *ptr->model = p_n_model;
00246
00247 for (int i = 0; i < curvature.Length(); i++) {
00248 curvature[i] = *(float*)&source.data[(i * source.point_step) + offset_curvature];
00249 }
00250 ptr->curvature = curvature;
00251 }
00252
00253 return ptr;
00254 }
00255
00256 }
00257
00258
00259