stereo_image_proc.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <ros/ros.h>
00036 #include <nodelet/loader.h>
00037 #include <image_proc/advertisement_checker.h>
00038 
00039 void loadMonocularNodelets(nodelet::Loader& manager, const std::string& side,
00040                            const XmlRpc::XmlRpcValue& rectify_params,
00041                            const nodelet::V_string& my_argv)
00042 {
00043   nodelet::M_string remappings;
00044 
00045   // Explicitly resolve global remappings (wg-ros-pkg #5055).
00046   // Otherwise the internal remapping 'image_raw' -> 'left/image_raw' can hide a
00047   // global remapping from the launch file or command line.
00048   std::string image_raw_topic        = ros::names::resolve(side + "/image_raw");
00049   std::string image_mono_topic       = ros::names::resolve(side + "/image_mono");
00050   std::string image_color_topic      = ros::names::resolve(side + "/image_color");
00051   std::string image_rect_topic       = ros::names::resolve(side + "/image_rect");
00052   std::string image_rect_color_topic = ros::names::resolve(side + "/image_rect_color");
00053   std::string camera_info_topic      = ros::names::resolve(side + "/camera_info");
00054   
00055   // Debayer nodelet: image_raw -> image_mono, image_color
00056   remappings["image_raw"]   = image_raw_topic;
00057   remappings["image_mono"]  = image_mono_topic;
00058   remappings["image_color"] = image_color_topic;
00059   std::string debayer_name = ros::this_node::getName() + "_debayer_" + side;
00060   manager.load(debayer_name, "image_proc/debayer", remappings, my_argv);
00061 
00062   // Rectify nodelet: image_mono -> image_rect
00063   remappings.clear();
00064   remappings["image_mono"]  = image_mono_topic;
00065   remappings["camera_info"] = camera_info_topic;
00066   remappings["image_rect"]  = image_rect_topic;
00067   std::string rectify_mono_name = ros::this_node::getName() + "_rectify_mono_" + side;
00068   if (rectify_params.valid())
00069     ros::param::set(rectify_mono_name, rectify_params);
00070   manager.load(rectify_mono_name, "image_proc/rectify", remappings, my_argv);
00071 
00072   // Rectify nodelet: image_color -> image_rect_color
00073   remappings.clear();
00074   remappings["image_mono"]  = image_color_topic;
00075   remappings["camera_info"] = camera_info_topic;
00076   remappings["image_rect"]  = image_rect_color_topic;
00077   std::string rectify_color_name = ros::this_node::getName() + "_rectify_color_" + side;
00078   if (rectify_params.valid())
00079     ros::param::set(rectify_color_name, rectify_params);
00080   manager.load(rectify_color_name, "image_proc/rectify", remappings, my_argv);
00081 }
00082 
00083 int main(int argc, char **argv)
00084 {
00085   ros::init(argc, argv, "stereo_image_proc");
00086 
00087   // Check for common user errors
00088   if (ros::names::remap("camera") != "camera")
00089   {
00090     ROS_WARN("Remapping 'camera' has no effect! Start stereo_image_proc in the "
00091              "stereo namespace instead.\nExample command-line usage:\n"
00092              "\t$ ROS_NAMESPACE=%s rosrun stereo_image_proc stereo_image_proc",
00093              ros::names::remap("camera").c_str());
00094   }
00095   if (ros::this_node::getNamespace() == "/")
00096   {
00097     ROS_WARN("Started in the global namespace! This is probably wrong. Start "
00098              "stereo_image_proc in the stereo namespace.\nExample command-line usage:\n"
00099              "\t$ ROS_NAMESPACE=my_stereo rosrun stereo_image_proc stereo_image_proc");
00100   }
00101 
00102   // Shared parameters to be propagated to nodelet private namespaces
00103   ros::NodeHandle private_nh("~");
00104   XmlRpc::XmlRpcValue shared_params;
00105   int queue_size;
00106   if (private_nh.getParam("queue_size", queue_size))
00107     shared_params["queue_size"] = queue_size;
00108 
00109   nodelet::Loader manager(false); // Don't bring up the manager ROS API
00110   nodelet::M_string remappings;
00111   nodelet::V_string my_argv;
00112 
00113   // Load equivalents of image_proc for left and right cameras
00114   loadMonocularNodelets(manager, "left",  shared_params, my_argv);
00115   loadMonocularNodelets(manager, "right", shared_params, my_argv);
00116 
00117   // Stereo nodelets also need to know the synchronization policy
00118   bool approx_sync;
00119   if (private_nh.getParam("approximate_sync", approx_sync))
00120     shared_params["approximate_sync"] = XmlRpc::XmlRpcValue(approx_sync);
00121 
00122   // Disparity nodelet
00123   // Inputs: left/image_rect, left/camera_info, right/image_rect, right/camera_info
00124   // Outputs: disparity
00125   // NOTE: Using node name for the disparity nodelet because it is the only one using
00126   // dynamic_reconfigure so far, and this makes us backwards-compatible with cturtle.
00127   std::string disparity_name = ros::this_node::getName();
00128   manager.load(disparity_name, "stereo_image_proc/disparity", remappings, my_argv);
00129 
00130   // PointCloud2 nodelet
00131   // Inputs: left/image_rect_color, left/camera_info, right/camera_info, disparity
00132   // Outputs: points2
00133   std::string point_cloud2_name = ros::this_node::getName() + "_point_cloud2";
00134   if (shared_params.valid())
00135     ros::param::set(point_cloud2_name, shared_params);
00136   manager.load(point_cloud2_name, "stereo_image_proc/point_cloud2", remappings, my_argv);
00137 
00138   // PointCloud (deprecated) nodelet
00139   // Inputs: left/image_rect_color, left/camera_info, right/camera_info, disparity
00140   // Outputs: points
00141   std::string point_cloud_name = ros::this_node::getName() + "_point_cloud";
00142   if (shared_params.valid())
00143     ros::param::set(point_cloud_name, shared_params);
00144   manager.load(point_cloud_name, "stereo_image_proc/point_cloud", remappings, my_argv);
00145 
00146   // Check for only the original camera topics
00147   ros::V_string topics;
00148   topics.push_back(ros::names::resolve("left/image_raw"));
00149   topics.push_back(ros::names::resolve("left/camera_info"));
00150   topics.push_back(ros::names::resolve("right/image_raw"));
00151   topics.push_back(ros::names::resolve("right/camera_info"));
00152   image_proc::AdvertisementChecker check_inputs(ros::NodeHandle(), ros::this_node::getName());
00153   check_inputs.start(topics, 60.0);
00154 
00155   ros::spin();
00156   return 0;
00157 }


stereo_image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Wed Aug 26 2015 11:57:43