|
| 1 | +/********************************************************************* |
| 2 | +* Software License Agreement (BSD License) |
| 3 | +* |
| 4 | +* Copyright (c) @TODO Please fill your name here |
| 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 Kei Okada 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 | + |
| 35 | +// @TODO Please add link to original sample program |
| 36 | +/** |
| 37 | + * This is a demo of @TODO |
| 38 | + */ |
| 39 | + |
| 40 | +#include <ros/ros.h> |
| 41 | +#include "opencv_apps/nodelet.h" |
| 42 | +#include <image_transport/image_transport.h> |
| 43 | +#include <sensor_msgs/image_encodings.h> |
| 44 | +#include <cv_bridge/cv_bridge.h> |
| 45 | +#include <sensor_msgs/image_encodings.h> |
| 46 | + |
| 47 | +#include <opencv2/highgui/highgui.hpp> |
| 48 | +#include <opencv2/imgproc/imgproc.hpp> |
| 49 | + |
| 50 | +// @TODO Fill more header files hHere |
| 51 | +#include <dynamic_reconfigure/server.h> |
| 52 | + |
| 53 | +// @TODO Note that please try to use existing opencv_apps/msg. They refrect opencv data classes. |
| 54 | +#include "opencv_apps/Point2DStamped.h" |
| 55 | + |
| 56 | +// |
| 57 | +#include "opencv_apps/SampleConfig.h" |
| 58 | +#include "opencv_apps/nodelet.h" |
| 59 | + |
| 60 | +namespace opencv_apps |
| 61 | +{ |
| 62 | +class SampleNodelet : public opencv_apps::Nodelet |
| 63 | +{ |
| 64 | + image_transport::Publisher img_pub_; |
| 65 | + image_transport::Subscriber img_sub_; |
| 66 | + image_transport::CameraSubscriber cam_sub_; |
| 67 | + ros::Publisher msg_pub_; |
| 68 | + |
| 69 | + boost::shared_ptr<image_transport::ImageTransport> it_; |
| 70 | + |
| 71 | + typedef opencv_apps::SampleConfig Config; |
| 72 | + typedef dynamic_reconfigure::Server<Config> ReconfigureServer; |
| 73 | + Config config_; |
| 74 | + boost::shared_ptr<ReconfigureServer> reconfigure_server_; |
| 75 | + |
| 76 | + int queue_size_; |
| 77 | + bool debug_view_; |
| 78 | + ros::Time prev_stamp_; |
| 79 | + |
| 80 | + std::string window_name_; |
| 81 | + static bool need_config_update_; |
| 82 | + |
| 83 | + bool needToInit; |
| 84 | + |
| 85 | + // @TODO Fill more vairables here |
| 86 | + static cv::Point2f point_; |
| 87 | + |
| 88 | + // @TODO Define callback functions as static |
| 89 | + static void onMouse(int event, int x, int y, int /*flags*/, void* /*param*/) |
| 90 | + { |
| 91 | + ROS_INFO("onMouse %d %d %d", event, x, y); // @TODO static funcion needs ROS_INFO, but others can use NODELET_INFO |
| 92 | + if (event == CV_EVENT_LBUTTONDOWN) |
| 93 | + { |
| 94 | + point_ = cv::Point2f((float)x, (float)y); |
| 95 | + } |
| 96 | + } |
| 97 | + |
| 98 | + void reconfigureCallback(Config& new_config, uint32_t level) |
| 99 | + { |
| 100 | + config_ = new_config; |
| 101 | + // @TODO Fill your code here |
| 102 | + } |
| 103 | + |
| 104 | + void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info) |
| 105 | + { |
| 106 | + doWork(msg, cam_info->header.frame_id); |
| 107 | + } |
| 108 | + |
| 109 | + void imageCallback(const sensor_msgs::ImageConstPtr& msg) |
| 110 | + { |
| 111 | + doWork(msg, msg->header.frame_id); |
| 112 | + } |
| 113 | + |
| 114 | + void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) |
| 115 | + { |
| 116 | + // Work on the image. |
| 117 | + try |
| 118 | + { |
| 119 | + // Convert the image into something opencv can handle. |
| 120 | + cv::Mat image = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image; |
| 121 | + |
| 122 | + // Messages |
| 123 | + opencv_apps::Point2DStamped output_msg; |
| 124 | + output_msg.header = msg->header; |
| 125 | + |
| 126 | + if (debug_view_) |
| 127 | + { |
| 128 | + // Create windows |
| 129 | + cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE); |
| 130 | + |
| 131 | + // @TODO Create other GUI tools |
| 132 | + cv::setMouseCallback(window_name_, onMouse, 0); |
| 133 | + |
| 134 | + if (need_config_update_) |
| 135 | + { |
| 136 | + reconfigure_server_->updateConfig(config_); |
| 137 | + // @TODO Update parameters |
| 138 | + need_config_update_ = false; |
| 139 | + } |
| 140 | + } |
| 141 | + |
| 142 | + // Do the work |
| 143 | + if (needToInit) |
| 144 | + { |
| 145 | + // @TODO Do initial process |
| 146 | + } |
| 147 | + |
| 148 | + // @TODO Do the work |
| 149 | + output_msg.point.x = point_.x; |
| 150 | + output_msg.point.y = point_.y; |
| 151 | + |
| 152 | + needToInit = false; |
| 153 | + if (debug_view_) |
| 154 | + { |
| 155 | + cv::imshow(window_name_, image); |
| 156 | + |
| 157 | + char c = (char)cv::waitKey(1); |
| 158 | + // if( c == 27 ) |
| 159 | + // break; |
| 160 | + switch (c) |
| 161 | + { |
| 162 | + case 'r': |
| 163 | + needToInit = true; |
| 164 | + break; |
| 165 | + } |
| 166 | + } |
| 167 | + |
| 168 | + // Publish the image. |
| 169 | + sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, image).toImageMsg(); |
| 170 | + img_pub_.publish(out_img); |
| 171 | + msg_pub_.publish(output_msg); |
| 172 | + } |
| 173 | + catch (cv::Exception& e) |
| 174 | + { |
| 175 | + NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line); |
| 176 | + } |
| 177 | + |
| 178 | + prev_stamp_ = msg->header.stamp; |
| 179 | + } |
| 180 | + |
| 181 | + void subscribe() // NOLINT(modernize-use-override) |
| 182 | + { |
| 183 | + NODELET_DEBUG("Subscribing to image topic."); |
| 184 | + if (config_.use_camera_info) |
| 185 | + cam_sub_ = it_->subscribeCamera("image", queue_size_, &SampleNodelet::imageCallbackWithInfo, this); |
| 186 | + else |
| 187 | + img_sub_ = it_->subscribe("image", queue_size_, &SampleNodelet::imageCallback, this); |
| 188 | + } |
| 189 | + |
| 190 | + void unsubscribe() // NOLINT(modernize-use-override) |
| 191 | + { |
| 192 | + NODELET_DEBUG("Unsubscribing from image topic."); |
| 193 | + img_sub_.shutdown(); |
| 194 | + cam_sub_.shutdown(); |
| 195 | + } |
| 196 | + |
| 197 | +public: |
| 198 | + virtual void onInit() // NOLINT(modernize-use-override) |
| 199 | + { |
| 200 | + Nodelet::onInit(); |
| 201 | + it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_)); |
| 202 | + |
| 203 | + pnh_->param("queue_size", queue_size_, 3); |
| 204 | + pnh_->param("debug_view", debug_view_, false); |
| 205 | + if (debug_view_) |
| 206 | + { |
| 207 | + always_subscribe_ = true; |
| 208 | + } |
| 209 | + prev_stamp_ = ros::Time(0, 0); |
| 210 | + |
| 211 | + window_name_ = "Sample"; // @TODO Add program name |
| 212 | + needToInit = true; |
| 213 | + |
| 214 | + reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_); |
| 215 | + dynamic_reconfigure::Server<Config>::CallbackType f = |
| 216 | + boost::bind(&SampleNodelet::reconfigureCallback, this, _1, _2); |
| 217 | + reconfigure_server_->setCallback(f); |
| 218 | + |
| 219 | + img_pub_ = advertiseImage(*pnh_, "image", 1); |
| 220 | + msg_pub_ = advertise<opencv_apps::Point2DStamped>(*pnh_, "output", 1); |
| 221 | + // @TODO add more advertise/services |
| 222 | + |
| 223 | + NODELET_INFO("Hot keys: "); |
| 224 | + NODELET_INFO("\tESC - quit the program"); |
| 225 | + NODELET_INFO("\tr - auto-initialize tracking"); |
| 226 | + // @ TODO Add more messages |
| 227 | + |
| 228 | + onInitPostProcess(); |
| 229 | + } |
| 230 | +}; |
| 231 | +bool SampleNodelet::need_config_update_ = false; |
| 232 | +// @TODO static variable must initialized here |
| 233 | +cv::Point2f SampleNodelet::point_ = cv::Point2f(0, 0); |
| 234 | +} // namespace opencv_apps |
| 235 | + |
| 236 | +#include <pluginlib/class_list_macros.h> |
| 237 | +PLUGINLIB_EXPORT_CLASS(opencv_apps::SampleNodelet, nodelet::Nodelet); |
0 commit comments