stereo_offsetter.cpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/


#include "camera_offsetter/generic_offsetter.h"
#include "sensor_msgs/CameraInfo.h"
#include "sensor_msgs/Image.h"
#include <std_msgs/Float64.h>
#include <boost/thread.hpp>

using namespace std;

namespace camera_offsetter
{

class StereoOffsetter : public GenericOffsetter
{
public:
  StereoOffsetter()
  {
    string stereo_name = nh_.resolveName("stereo");

    left_image_sub_ = nh_.subscribe(stereo_name + "/left/image_raw",   1, &StereoOffsetter::leftImageCb, this);
    left_info_sub_  = nh_.subscribe(stereo_name + "/left/camera_info", 1, &StereoOffsetter::leftInfoCb, this);
    right_image_sub_ = nh_.subscribe(stereo_name + "/right/image_raw",    1, &StereoOffsetter::rightImageCb, this);
    right_info_sub_  = nh_.subscribe(stereo_name + "/right/camera_info", 1, &StereoOffsetter::rightInfoCb, this);

    baseline_factor_sub_ = nh_.subscribe("baseline_factor", 1, &StereoOffsetter::baselineCb, this);

    left_image_pub_  = nh_.advertise<sensor_msgs::Image>(stereo_name + frame_suffix_ + "/left/image_raw",  1);
    left_info_pub_   = nh_.advertise<sensor_msgs::CameraInfo>(stereo_name + frame_suffix_ + "/left/camera_info",  1);
    right_image_pub_ = nh_.advertise<sensor_msgs::Image>(stereo_name + frame_suffix_ + "/right/image_raw",  1);
    right_info_pub_  = nh_.advertise<sensor_msgs::CameraInfo>(stereo_name + frame_suffix_ + "/right/camera_info",  1);

    // baseline_factor_ = 0;
  }

  void leftImageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    left_image_ = *msg;
    left_image_.header.frame_id = msg->header.frame_id + frame_suffix_;
    left_image_pub_.publish(left_image_);

    conditionalPublishTransform(msg->header.stamp, left_image_.header.frame_id, msg->header.frame_id);
  }

  void rightImageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    right_image_ = *msg;
    right_image_.header.frame_id = msg->header.frame_id + frame_suffix_;
    right_image_pub_.publish(right_image_);

    conditionalPublishTransform(msg->header.stamp, right_image_.header.frame_id, msg->header.frame_id);
  }

  void leftInfoCb(const sensor_msgs::CameraInfoConstPtr& msg)
  {
    left_info_ = *msg;
    left_info_.header.frame_id = msg->header.frame_id + frame_suffix_;
    left_info_pub_.publish(left_info_);

    conditionalPublishTransform(msg->header.stamp, left_info_.header.frame_id, msg->header.frame_id);
  }

  void rightInfoCb(const sensor_msgs::CameraInfoConstPtr& msg)
  {
    right_info_ = *msg;
    right_info_.header.frame_id = msg->header.frame_id + frame_suffix_;

    {
      boost::mutex::scoped_lock lock(baseline_mutex_);
      right_info_.P[3] *= 1+baseline_factor_;
    }

    right_info_pub_.publish(right_info_);

    conditionalPublishTransform(msg->header.stamp, right_info_.header.frame_id, msg->header.frame_id);
  }

  void baselineCb(const std_msgs::Float64ConstPtr& msg)
  {
    boost::mutex::scoped_lock lock(baseline_mutex_);
    baseline_factor_ += msg->data;
    ROS_INFO("Updating baseline by a factor of %.6f", baseline_factor_);
    saveConfig();
  }

  void conditionalPublishTransform(const ros::Time& cur_time, const string& next_frame, const string& prev_frame)
  {
    bool publish = false;

    // Need to see if we've already published at this time.
    {
      boost::mutex::scoped_lock lock(time_mutex_);
      if (left_image_.header.stamp > latest_published_)
      {
        latest_published_ = left_image_.header.stamp;
        publish = true;
      }
    }

    // Do the actual transform publish
    if (publish)
      publishTransform(cur_time, next_frame, prev_frame);
  }

private:
  ros::NodeHandle nh_;
  ros::Subscriber left_image_sub_;
  ros::Subscriber left_info_sub_;
  ros::Subscriber right_image_sub_;
  ros::Subscriber right_info_sub_;
  ros::Subscriber baseline_factor_sub_;

  ros::Publisher  left_image_pub_;
  ros::Publisher  left_info_pub_;
  ros::Publisher  right_image_pub_;
  ros::Publisher  right_info_pub_;

  // Store the latest time we published TF data for
  boost::mutex time_mutex_;
  ros::Time latest_published_;

  // Need a lock for the baseline factor (valued actually stored in generic offsetter)
  boost::mutex baseline_mutex_;

  sensor_msgs::Image left_image_;
  sensor_msgs::Image right_image_;
  sensor_msgs::CameraInfo left_info_;
  sensor_msgs::CameraInfo right_info_;
} ;

}

using namespace camera_offsetter;

int main(int argc, char** argv)
{
  ros::init(argc, argv, "stereo_offsetter");

  StereoOffsetter offsetter;

  ros::spin();
}