1 #include <ros/console.h>
2 #include <ros/duration.h>
4 #include <ros/master.h>
6 #include <ros/node_handle.h>
13 #include <opencv2/highgui/highgui.hpp>
32 if ( event == cv::EVENT_LBUTTONDOWN )
36 else if ( event == cv::EVENT_RBUTTONDOWN )
40 else if ( event == cv::EVENT_MBUTTONDOWN )
44 else if ( event == cv::EVENT_MOUSEMOVE )
52 int main(
int argc,
char **argv)
54 ros::init(argc, argv,
"rgbd_multitool", ros::init_options::AnonymousName);
67 <<
" multitool --rgbd RGBD_TOPIC" <<
std::endl
72 for(
int i = 1; i < argc; i += 2)
80 client->initialize(ros::names::resolve(arg));
102 cv::namedWindow(window_name, 1);
105 cv::setMouseCallback(window_name,
CallBackFunc,
nullptr);
109 float max_view_distance = 10;
112 ros::NodeHandle nh_private(
"~");
115 nh_private.getParam(
"rate", rate);
117 ros::WallTime last_master_check = ros::WallTime::now();
122 if (ros::WallTime::now() >= last_master_check + ros::WallDuration(1))
124 last_master_check = ros::WallTime::now();
125 if (!ros::master::check())
127 ROS_FATAL(
"Lost connection to master");
131 if (!
PAUSE && client)
144 const cv::Mat& rgb = image->getRGBImage();
145 const cv::Mat& depth = image->getDepthImage();
149 cv::Mat depth_canvas(depth.rows, depth.cols, CV_8UC3, cv::Scalar(50, 0, 0));
150 for(
int y = 0; y < depth.rows; ++y)
152 for(
int x = 0; x < depth.cols; ++x)
154 float d = depth.at<
float>(y, x);
157 unsigned char v =
static_cast<unsigned char>(std::min<float>(max_view_distance, d / max_view_distance) * 255);
158 depth_canvas.at<cv::Vec3b>(y, x) = cv::Vec3b(v, v, v);
167 int rgb_height =
IMAGE_WIDTH * rgb.rows / rgb.cols;
168 int depth_height =
IMAGE_WIDTH * depth.rows / depth.cols;
174 cv::Mat rgb_roi = canvas(cv::Rect(cv::Point(0, 0), cv::Size(
IMAGE_WIDTH, rgb_height)));
177 cv::resize(rgb, rgb_roi, cv::Size(
IMAGE_WIDTH, rgb_height));
178 cv::resize(depth_canvas, depth_roi, cv::Size(
IMAGE_WIDTH, depth_height));
182 canvas = depth_canvas;
193 canvas = cv::Mat(480, 640, CV_8UC3, cv::Scalar(50, 50, 50));
194 cv::line(canvas, cv::Point(0, 0), cv::Point(640, 480), cv::Scalar(255, 255, 255), 5);
195 cv::line(canvas, cv::Point(0, 480), cv::Point(640, 0), cv::Scalar(255, 255, 255), 5);
205 cv::circle(canvas,
mouse_pos, 5, cv::Scalar(255, 0, 0), 1);
209 cv::putText(canvas,
MODE, cv::Point(10, 20), cv::FONT_HERSHEY_COMPLEX_SMALL, 1, cv::Scalar(255, 255, 255), 1);
212 cv::putText(canvas,
"PAUSED", cv::Point(10, canvas.rows - 25), cv::FONT_HERSHEY_COMPLEX_SMALL, 1, cv::Scalar(255, 255, 255), 1);
218 cv::circle(canvas,
mouse_points[i], 5, cv::Scalar(0, 0, 255), 2);
228 else if (
MODE ==
"MEASURE")
251 cv::imshow(window_name, canvas);
252 int i_key = cv::waitKey(3);
255 char key =
static_cast<char>(i_key);
261 case 'm':
MODE =
MODE ==
"MEASURE" ?
"" :
"MEASURE";
263 case 'q':
MODE=
"DONE";