3 #include <opencv2/core/core.hpp>
4 #include <opencv2/highgui/highgui.hpp>
5 #include <opencv2/imgproc/imgproc.hpp>
8 #include "mtt/TargetList.h"
10 #include <image_transport/image_transport.h>
12 #include <cv_bridge/cv_bridge.h>
14 #include "sensor_msgs/LaserScan.h"
15 #include "sensor_msgs/PointCloud.h"
16 #include <sensor_msgs/image_encodings.h>
18 #include <pcl_conversions/pcl_conversions.h>
22 namespace enc = sensor_msgs::image_encodings;
27 image_transport::ImageTransport
it_;
53 ros::NodeHandle nh_local(
"~");
75 void imageCb(
const sensor_msgs::ImageConstPtr& cam_msg)
77 cv_bridge::CvImagePtr cv_ptr;
80 cv_ptr = cv_bridge::toCvCopy(cam_msg, enc::BGR8);
82 catch (cv_bridge::Exception& e)
84 ROS_ERROR(
"cv_bridge exception: %s", e.what());
92 Mat input = cv_ptr->image;
104 Mat grayscaleMat(input.size(), CV_8UC1);
108 cvtColor(input, grayscaleMat, CV_BGR2GRAY);
109 threshold(grayscaleMat, grayscaleMat, 170, 255, THRESH_BINARY);
116 vector<vector<Point> > contours;
118 Mat contourOutput = grayscaleMat.clone();
119 findContours( contourOutput, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE );
121 vector<vector<Point> > contours_poly( contours.size() );
123 Mat drawing = Mat::zeros( grayscaleMat.size(), CV_8UC3 );
125 int max_size_triangle=0;
126 for(
size_t i = 0; i < contours.size(); i++)
128 approxPolyDP(Mat(contours[i]), contours_poly[i], 5,
true);
133 drawContours(drawing, contours_poly, i, Scalar(0, 0, 255),CV_FILLED,8);
134 if (contours_poly[i].size() == 4)
141 if( arcLength(Mat(contours[i]),
true) > arcLength(Mat(contours[max_size_triangle]),
true))
143 max_size_triangle = i;
153 Point left_upper_point;
154 Point right_upper_point;
155 Point left_lower_point;
156 Point right_lower_point;
158 if (contours_poly[max_size_triangle][1].y < (contours_poly[max_size_triangle][0].y + 10) )
160 left_upper_point.x = contours_poly[max_size_triangle][1].x;
161 left_upper_point.y = contours_poly[max_size_triangle][1].y;
163 right_upper_point.x = contours_poly[max_size_triangle][0].x;
164 right_upper_point.y = contours_poly[max_size_triangle][0].y;
166 left_lower_point.x = contours_poly[max_size_triangle][2].x;
167 left_lower_point.y = contours_poly[max_size_triangle][2].y;
169 right_lower_point.x = contours_poly[max_size_triangle][3].x;
170 right_lower_point.y = contours_poly[max_size_triangle][3].y;
173 left_upper_point.x = contours_poly[max_size_triangle][0].x;
174 left_upper_point.y = contours_poly[max_size_triangle][0].y;
176 right_upper_point.x = contours_poly[max_size_triangle][3].x;
177 right_upper_point.y = contours_poly[max_size_triangle][3].y;
179 left_lower_point.x = contours_poly[max_size_triangle][1].x;
180 left_lower_point.y = contours_poly[max_size_triangle][1].y;
182 right_lower_point.x = contours_poly[max_size_triangle][2].x;
183 right_lower_point.y = contours_poly[max_size_triangle][2].y;
186 min_width = right_upper_point.x - left_upper_point.x;
187 max_width = right_lower_point.x - left_lower_point.x;
188 left_height = left_lower_point.y - left_upper_point.y;
189 right_height = right_lower_point.y - right_upper_point.y;
191 cout <<
"triangle pixel size: " << max_size_triangle <<
" " << contours_poly[max_size_triangle] <<
" " << arcLength(Mat(contours[max_size_triangle]),
true) << endl
192 <<
"max width: " << max_width << endl
193 <<
"min width: " << min_width << endl
194 <<
"left_height: " << left_height << endl
195 <<
"right_height: " << right_height << endl;
197 drawContours(drawing, contours_poly, max_size_triangle, Scalar(255, 255, 255),CV_FILLED,8);
201 double object_width_percent_laser_given = largura;
203 int laser_left_line_int = object_width_percent_laser_given * left_height;
204 double laser_left_line = object_width_percent_laser_given * left_height;
206 int laser_right_line_int = object_width_percent_laser_given * right_height;
207 double laser_right_line = object_width_percent_laser_given * right_height;
211 if(laser_left_line - laser_left_line_int >=0.5 )
212 laser_left_line_int++;
214 if(laser_right_line - laser_right_line_int >=0.5 )
215 laser_right_line_int++;
217 int laser_left_line_position = left_upper_point.y + laser_left_line_int;
218 int laser_right_line_position = right_upper_point.y + laser_right_line_int;
221 Mat grayscaleMat_2(drawing.size(), CV_8UC1);
224 cvtColor(drawing, grayscaleMat_2, CV_BGR2GRAY);
226 threshold(grayscaleMat_2, grayscaleMat_2, 245, 255, THRESH_BINARY);
232 for(
int col=(left_lower_point.x - 10); col<(right_lower_point.x + 10); col++)
233 if(grayscaleMat_2.at<uchar>(laser_left_line_position,col) != 0)
236 left_point.y=laser_left_line_position;
237 left_point.x=col - 2;
241 for(
int col=(right_lower_point.x + 10); col>(left_lower_point.x - 10); col--)
242 if(grayscaleMat_2.at<uchar>(laser_left_line_position,col) != 0)
245 right_point.y=laser_left_line_position;
246 right_point.x=col + 2;
252 cout <<
"points, left_point: " << left_point <<
" right_point: " << right_point << endl;
254 line(input, left_point, right_point, Scalar(0, 0, 255), 2, 8,0);
263 fs << left_point.x <<
" " << left_point.y <<
"\n";
264 fs << right_point.x <<
" " << right_point.y <<
"\n";
268 cout <<
"please change the object position, and then run the program again" << endl;
270 imshow(
"input_incial", input);
272 imshow(
"drawing", drawing);
273 imshow(
"drawing1", grayscaleMat_2);
276 image_sub_.shutdown();
283 double distance_to_target_from_center_bumper_;
287 cout <<
"laser_size: " << laser_msg.Targets.size() << endl;
289 for(
int i=0; i<laser_msg.Targets.size();i++)
291 distance_to_target_from_center_bumper_ = sqrt(laser_msg.Targets[i].pose.position.x * laser_msg.Targets[i].pose.position.x +
292 laser_msg.Targets[i].pose.position.y * laser_msg.Targets[i].pose.position.y);
294 theta_ = acos(laser_msg.Targets[i].pose.position.x / distance_to_target_from_center_bumper);
296 largura_ = sqrt(((laser_msg.Targets[i].finalpose.x - laser_msg.Targets[i].initialpose.x) * (laser_msg.Targets[i].finalpose.x - laser_msg.Targets[i].initialpose.x)) + ((laser_msg.Targets[i].finalpose.y - laser_msg.Targets[i].initialpose.y) * (laser_msg.Targets[i].finalpose.y - laser_msg.Targets[i].initialpose.y)));
298 if(laser_msg.Targets[i].pose.position.x > 0 && laser_msg.Targets[i].pose.position.y > -10 && laser_msg.Targets[i].pose.position.y < 10 ) {
301 distance_to_target_from_center_bumper = distance_to_target_from_center_bumper_;
303 cout <<
"largura"<< largura << endl;
305 if (largura_ > largura)
309 laser_left_point = Point3f(laser_msg.Targets[i].initialpose.x, laser_msg.Targets[i].initialpose.y, laser_msg.Targets[i].initialpose.z);
311 laser_right_point = Point3f(laser_msg.Targets[i].finalpose.x, laser_msg.Targets[i].finalpose.y, laser_msg.Targets[i].finalpose.z);
316 laser_done = laser_done + 1;
318 cout <<
"laser_done, width: " << largura << endl;
326 fs << laser_left_point.x <<
" " << laser_left_point.y <<
" " << 0 <<
"\n";
327 fs << laser_right_point.x <<
" " << laser_right_point.y <<
" " << 0 <<
"\n";
331 mtt_laser_sub_.shutdown();
337 int main(
int argc,
char** argv)
339 ros::init(argc, argv,
"calibrate_save_camera_laser_points_node");
std::string imagePoints1FileName
int main(int argc, char **argv)
void mttLaserGather(const mtt::TargetList &laser_msg)
std::string imagePoints1FileName
void imageCb(const sensor_msgs::ImageConstPtr &cam_msg)
double distance_to_target_from_center_bumper
Point3f laser_right_point
ros::Subscriber mtt_laser_sub_
std::string objectPoints1FileName
image_transport::ImageTransport it_
image_transport::Subscriber image_sub_
std::string objectPoints1FileName