laser_cb_detector.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008-2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 
38 #include <ros/console.h>
39 #include <cv_bridge/cv_bridge.h>
40 //#include <highgui.h>
41 
42 using namespace std;
43 using namespace laser_cb_detector;
44 
45 LaserCbDetector::LaserCbDetector() : configured_(false) {}
46 
47 bool LaserCbDetector::configure(const ConfigGoal& config)
48 {
49  config_ = config;
50  image_cb_detector::ConfigGoal image_cfg;
51  // TODO: setup message
52 
53  image_cfg.num_x = config.num_x;
54  image_cfg.num_y = config.num_y;
55  image_cfg.spacing_x = config.spacing_x;
56  image_cfg.spacing_y = config.spacing_y;
57 
58  image_cfg.width_scaling = config.width_scaling;
59  image_cfg.height_scaling = config.height_scaling;
60 
61  image_cfg.subpixel_window = config.subpixel_window;
62  image_cfg.subpixel_zero_zone = config.subpixel_zero_zone;
63 
64  detector_.configure(image_cfg);
65  return true;
66 }
67 
68 bool LaserCbDetector::detect(const calibration_msgs::DenseLaserSnapshot& snapshot,
69  calibration_msgs::CalibrationPattern& result)
70 {
71  // ***** Convert the snapshot into an image, based on intensity window in config *****
72  if(!bridge_.fromIntensity(snapshot, config_.min_intensity, config_.max_intensity))
73  return false;
74  cv::Mat image = bridge_.toCvMat();
75 
76  if (config_.flip_horizontal)
77  {
78  ROS_DEBUG("Flipping image");
79  cv::flip(image, image, 1);
80  }
81  else
82  ROS_DEBUG("Not flipping image");
83 
84  cv_bridge::CvImage cv_image(snapshot.header, "mono8", image);
85  sensor_msgs::ImagePtr ros_image = cv_image.toImageMsg();
86  if(detector_.detect(ros_image, result)){
87  if (config_.flip_horizontal){
88  for(int i=0; i < result.image_points.size(); i++)
89  result.image_points[i].x = image.cols - result.image_points[i].x - 1;
90  }
91  return true;
92  }else
93  return false;
94 }
95 
96 bool LaserCbDetector::getImage(const calibration_msgs::DenseLaserSnapshot& snapshot, sensor_msgs::Image& ros_image)
97 {
98  if(!bridge_.fromIntensity(snapshot, config_.min_intensity, config_.max_intensity))
99  {
100  ROS_ERROR("Error building cv::Mat from DenseLaserSnapshot's intensity data");
101  return false;
102  }
103  cv::Mat image = bridge_.toCvMat();
104 
105  cv_bridge::CvImage(snapshot.header, "mono8", image).toImageMsg(ros_image);
106 
107  return true;
108 }
109 
cv_bridge::CvImage::toImageMsg
sensor_msgs::ImagePtr toImageMsg() const
image_cb_detector::ImageCbDetector::detect
bool detect(const sensor_msgs::ImageConstPtr &image, calibration_msgs::CalibrationPattern &result)
laser_cb_detector.h
laser_cb_detector::CvLaserBridge::fromIntensity
bool fromIntensity(const calibration_msgs::DenseLaserSnapshot &snapshot, float min_val, float max_val)
Definition: cv_laser_bridge.cpp:42
laser_cb_detector
Definition: cv_laser_bridge.h:43
console.h
ROS_DEBUG
#define ROS_DEBUG(...)
laser_cb_detector::LaserCbDetector::configure
bool configure(const ConfigGoal &config)
Definition: laser_cb_detector.cpp:47
laser_cb_detector::LaserCbDetector::config_
ConfigGoal config_
Definition: laser_cb_detector.h:129
laser_cb_detector::LaserCbDetector::detect
bool detect(const calibration_msgs::DenseLaserSnapshot &snapshot, calibration_msgs::CalibrationPattern &result)
Definition: laser_cb_detector.cpp:68
image_cb_detector::ImageCbDetector::configure
bool configure(const ConfigGoal &config)
laser_cb_detector::CvLaserBridge::toCvMat
cv::Mat toCvMat()
Definition: cv_laser_bridge.h:119
laser_cb_detector::LaserCbDetector::bridge_
CvLaserBridge bridge_
Definition: laser_cb_detector.h:130
std
cv_bridge.h
ROS_ERROR
#define ROS_ERROR(...)
cv_bridge::CvImage
laser_cb_detector::LaserCbDetector::detector_
image_cb_detector::ImageCbDetector detector_
Definition: laser_cb_detector.h:131
laser_cb_detector::LaserCbDetector::getImage
bool getImage(const calibration_msgs::DenseLaserSnapshot &snapshot, sensor_msgs::Image &ros_image)
Definition: laser_cb_detector.cpp:96


laser_cb_detector
Author(s): Vijay Pradeep
autogenerated on Tue Mar 1 2022 23:58:55