33 #include <datamatrix_detection/DatamatrixMsg.h>
35 #include <image_transport/image_transport.h>
36 #include <cv_bridge/cv_bridge.h>
37 #include <sensor_msgs/image_encodings.h>
39 #include <opencv2/imgproc/imgproc.hpp>
40 #include <opencv2/highgui/highgui.hpp>
49 image_transport::ImageTransport
it_;
61 image_sub_ =
it_.subscribe(
"usb_cam_reader/image_rect_color", 1,
63 datamatrix_pub =
nh_.advertise<datamatrix_detection::DatamatrixMsg>(
"datamatrix_detection/datamatrix_msg",1);
69 double sum_time=0, max_time = 0.0, min_time = 1000;
74 if (elapsed_time[i] < min_time)
75 min_time = elapsed_time[i];
76 if (elapsed_time[i] > max_time)
77 max_time = elapsed_time[i];
80 std::cout <<
"Detection: " << std::endl;
81 std::cout <<
"max time: " << max_time*1000 <<
" min time: " << min_time*1000 <<
" mean time: " << sum_time*1000/
elapsed_time.size() << std::endl;
101 cv_bridge::CvImagePtr cv_ptr;
104 cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
106 catch (cv_bridge::Exception& e)
108 ROS_ERROR(
"cv_bridge exception: %s", e.what());
123 ros::Time start = ros::Time::now();
135 cv::Mat rot_mat = getRotationMatrix2D(cv::Point(cv_ori_img.cols/2, cv_ori_img.rows/2), 180, 1);
136 cv::warpAffine(cv_ori_img, rot_img, rot_mat, cv_ori_img.size());
138 cv::Mat cv_img = rot_img.rowRange(rot_img.rows/4, 3 * rot_img.rows/4);
139 int height = cv_img.rows-1 + rot_img.rows/4;
154 datamatrix_detection::DatamatrixMsg datamatrix_msg;
155 datamatrix_detection::DatamatrixData datamatrix_data;
156 datamatrix_msg.header = image_header;
158 img = dmtxImageCreate(cv_img.data, cv_img.cols, cv_img.rows, DmtxPack24bppRGB);
161 dec = dmtxDecodeCreate(img, 1);
166 DmtxTime timeout = dmtxTimeAdd(dmtxTimeNow(),delay);
167 reg = dmtxRegionFindNext(dec, &timeout);
169 msg = dmtxDecodeMatrixRegion(dec, reg, DmtxUndefined);
171 datamatrix_data.code = std::string((
char*)(msg->output));
173 datamatrix_data.left.x = reg->leftLoc.X;
174 datamatrix_data.left.y = height - reg->leftLoc.Y;
175 datamatrix_data.left.z = 0;
176 datamatrix_data.right.x = reg->rightLoc.X;
177 datamatrix_data.right.y = height - reg->rightLoc.Y;
178 datamatrix_data.right.z = 0;
179 datamatrix_data.top.x = reg->topLoc.X;
180 datamatrix_data.top.y = height - reg->topLoc.Y;
181 datamatrix_data.top.z = 0;
182 datamatrix_data.bottom.x = reg->bottomLoc.X;
183 datamatrix_data.bottom.y = height - reg->bottomLoc.Y;
184 datamatrix_data.bottom.z = 0;
186 datamatrix_msg.decoded_matrices.push_back(datamatrix_data);
188 dmtxMessageDestroy(&msg);
191 timeout = dmtxTimeAdd(dmtxTimeNow(),delay);
192 reg = dmtxRegionFindNext(dec, &timeout);
199 dmtxDecodeDestroy(&dec);
200 dmtxImageDestroy(&img);
201 dmtxRegionDestroy(®);
204 elapsed_time.push_back( fabs((ros::Time::now() - start).toSec()) );
210 int main(
int argc,
char** argv)
212 ros::init(argc, argv,
"datamatrix_detection");
void DatamatrixDecode(cv::Mat &cv_img)
int main(int argc, char **argv)
ros::Publisher datamatrix_pub
void DatamatrixDecode(cv::Mat &cv_ori_img, std_msgs::Header image_header)
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
image_transport::ImageTransport it_
image_transport::Subscriber image_sub_
std::vector< double > elapsed_time