$search
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 00036 00037 #include "camera_offsetter/generic_offsetter.h" 00038 #include "sensor_msgs/CameraInfo.h" 00039 #include "sensor_msgs/Image.h" 00040 #include <std_msgs/Float64.h> 00041 #include <boost/thread.hpp> 00042 00043 using namespace std; 00044 00045 namespace camera_offsetter 00046 { 00047 00048 class StereoOffsetter : public GenericOffsetter 00049 { 00050 public: 00051 StereoOffsetter() 00052 { 00053 string stereo_name = nh_.resolveName("stereo"); 00054 00055 left_image_sub_ = nh_.subscribe(stereo_name + "/left/image_raw", 1, &StereoOffsetter::leftImageCb, this); 00056 left_info_sub_ = nh_.subscribe(stereo_name + "/left/camera_info", 1, &StereoOffsetter::leftInfoCb, this); 00057 right_image_sub_ = nh_.subscribe(stereo_name + "/right/image_raw", 1, &StereoOffsetter::rightImageCb, this); 00058 right_info_sub_ = nh_.subscribe(stereo_name + "/right/camera_info", 1, &StereoOffsetter::rightInfoCb, this); 00059 00060 baseline_factor_sub_ = nh_.subscribe("baseline_factor", 1, &StereoOffsetter::baselineCb, this); 00061 00062 left_image_pub_ = nh_.advertise<sensor_msgs::Image>(stereo_name + frame_suffix_ + "/left/image_raw", 1); 00063 left_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(stereo_name + frame_suffix_ + "/left/camera_info", 1); 00064 right_image_pub_ = nh_.advertise<sensor_msgs::Image>(stereo_name + frame_suffix_ + "/right/image_raw", 1); 00065 right_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(stereo_name + frame_suffix_ + "/right/camera_info", 1); 00066 00067 // baseline_factor_ = 0; 00068 } 00069 00070 void leftImageCb(const sensor_msgs::ImageConstPtr& msg) 00071 { 00072 left_image_ = *msg; 00073 left_image_.header.frame_id = msg->header.frame_id + frame_suffix_; 00074 left_image_pub_.publish(left_image_); 00075 00076 conditionalPublishTransform(msg->header.stamp, left_image_.header.frame_id, msg->header.frame_id); 00077 } 00078 00079 void rightImageCb(const sensor_msgs::ImageConstPtr& msg) 00080 { 00081 right_image_ = *msg; 00082 right_image_.header.frame_id = msg->header.frame_id + frame_suffix_; 00083 right_image_pub_.publish(right_image_); 00084 00085 conditionalPublishTransform(msg->header.stamp, right_image_.header.frame_id, msg->header.frame_id); 00086 } 00087 00088 void leftInfoCb(const sensor_msgs::CameraInfoConstPtr& msg) 00089 { 00090 left_info_ = *msg; 00091 left_info_.header.frame_id = msg->header.frame_id + frame_suffix_; 00092 left_info_pub_.publish(left_info_); 00093 00094 conditionalPublishTransform(msg->header.stamp, left_info_.header.frame_id, msg->header.frame_id); 00095 } 00096 00097 void rightInfoCb(const sensor_msgs::CameraInfoConstPtr& msg) 00098 { 00099 right_info_ = *msg; 00100 right_info_.header.frame_id = msg->header.frame_id + frame_suffix_; 00101 00102 { 00103 boost::mutex::scoped_lock lock(baseline_mutex_); 00104 right_info_.P[3] *= 1+baseline_factor_; 00105 } 00106 00107 right_info_pub_.publish(right_info_); 00108 00109 conditionalPublishTransform(msg->header.stamp, right_info_.header.frame_id, msg->header.frame_id); 00110 } 00111 00112 void baselineCb(const std_msgs::Float64ConstPtr& msg) 00113 { 00114 boost::mutex::scoped_lock lock(baseline_mutex_); 00115 baseline_factor_ += msg->data; 00116 ROS_INFO("Updating baseline by a factor of %.6f", baseline_factor_); 00117 saveConfig(); 00118 } 00119 00120 void conditionalPublishTransform(const ros::Time& cur_time, const string& next_frame, const string& prev_frame) 00121 { 00122 bool publish = false; 00123 00124 // Need to see if we've already published at this time. 00125 { 00126 boost::mutex::scoped_lock lock(time_mutex_); 00127 if (left_image_.header.stamp > latest_published_) 00128 { 00129 latest_published_ = left_image_.header.stamp; 00130 publish = true; 00131 } 00132 } 00133 00134 // Do the actual transform publish 00135 if (publish) 00136 publishTransform(cur_time, next_frame, prev_frame); 00137 } 00138 00139 private: 00140 ros::NodeHandle nh_; 00141 ros::Subscriber left_image_sub_; 00142 ros::Subscriber left_info_sub_; 00143 ros::Subscriber right_image_sub_; 00144 ros::Subscriber right_info_sub_; 00145 ros::Subscriber baseline_factor_sub_; 00146 00147 ros::Publisher left_image_pub_; 00148 ros::Publisher left_info_pub_; 00149 ros::Publisher right_image_pub_; 00150 ros::Publisher right_info_pub_; 00151 00152 // Store the latest time we published TF data for 00153 boost::mutex time_mutex_; 00154 ros::Time latest_published_; 00155 00156 // Need a lock for the baseline factor (valued actually stored in generic offsetter) 00157 boost::mutex baseline_mutex_; 00158 00159 sensor_msgs::Image left_image_; 00160 sensor_msgs::Image right_image_; 00161 sensor_msgs::CameraInfo left_info_; 00162 sensor_msgs::CameraInfo right_info_; 00163 } ; 00164 00165 } 00166 00167 using namespace camera_offsetter; 00168 00169 int main(int argc, char** argv) 00170 { 00171 ros::init(argc, argv, "stereo_offsetter"); 00172 00173 StereoOffsetter offsetter; 00174 00175 ros::spin(); 00176 }