camera_info_publisher.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_interactive_marker/camera_info_publisher.h"
00037 #include <sensor_msgs/distortion_models.h>
00038 #include <tf/transform_broadcaster.h>
00039 
00040 namespace jsk_interactive_marker
00041 {
00042   CameraInfoPublisher::CameraInfoPublisher()
00043   {
00044     ros::NodeHandle nh, pnh("~");
00045     
00046     latest_pose_.orientation.w = 1.0;
00047     tf_listener_.reset(new tf::TransformListener());
00048     pub_camera_info_ = pnh.advertise<sensor_msgs::CameraInfo>("camera_info", 1);
00049 
00050     // setup dynamic reconfigure
00051     srv_ = std::make_shared <dynamic_reconfigure::Server<Config> > (pnh);
00052     dynamic_reconfigure::Server<Config>::CallbackType f =
00053       boost::bind (
00054         &CameraInfoPublisher::configCallback, this, _1, _2);
00055     srv_->setCallback (f);
00056 
00057     // read parameters
00058     if (!pnh.getParam("frame_id", frame_id_)) {
00059       ROS_WARN("~frame_id is not specified, use camera as frame_id");
00060       frame_id_ = "camera";
00061     }
00062     if (!pnh.getParam("parent_frame_id", parent_frame_id_)) {
00063       ROS_WARN("~parent_frame_id is not specified, use base_link as parent_frame_id");
00064       parent_frame_id_ = "base_link";
00065     }
00066 
00067     // interactive marker
00068     server_.reset(new interactive_markers::InteractiveMarkerServer(
00069                     ros::this_node::getName()));
00070     initializeInteractiveMarker();
00071     bool sync_pointcloud;
00072     bool sync_image;
00073     
00074     if (!pnh.getParam("sync_pointcloud", sync_pointcloud)) {
00075       sync_pointcloud = false;
00076     }
00077     if (sync_pointcloud) {
00078       ROS_INFO("~sync_pointcloud is specified, synchronize ~camera_info to pointcloud");
00079       sub_sync_ = pnh.subscribe(
00080         "input", 1, &CameraInfoPublisher::pointcloudCallback, this);
00081     }
00082     else {
00083       if (!pnh.getParam("sync_image", sync_image)) {
00084         sync_image = false;
00085       }
00086       if (sync_image) {
00087         ROS_INFO("~sync_image is specified, synchronize ~camera_info to image");
00088         sub_sync_ = pnh.subscribe(
00089           "input", 1, &CameraInfoPublisher::imageCallback, this);
00090       }
00091       else {
00092         ROS_INFO("~sync_image or ~sync_pointcloud are not specified, use static_rate");
00093         double static_rate;
00094         pnh.param("static_rate", static_rate, 30.0); // defaults to 30 Hz
00095         timer_ = nh.createTimer(
00096           ros::Duration( 1 / static_rate ),
00097           boost::bind(&CameraInfoPublisher::staticRateCallback,
00098                       this, _1));
00099       }
00100     }
00101   }
00102 
00103   CameraInfoPublisher::~CameraInfoPublisher()
00104   {
00105 
00106   }
00107   
00108   void CameraInfoPublisher::initializeInteractiveMarker()
00109   {
00110     visualization_msgs::InteractiveMarker int_marker;
00111     int_marker.header.frame_id = parent_frame_id_;
00112     int_marker.name = "camera info";
00113     im_helpers::add6DofControl(int_marker, false);
00114     server_->insert(int_marker,
00115                     boost::bind(&CameraInfoPublisher::processFeedback, this, _1));
00116     server_->applyChanges();
00117   }
00118 
00119   void CameraInfoPublisher::processFeedback(
00120     const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00121   {
00122     boost::mutex::scoped_lock lock(mutex_);
00123     geometry_msgs::PoseStamped new_pose, transformed_pose;
00124     new_pose.pose = feedback->pose;
00125     new_pose.header = feedback->header;
00126     try {
00127       tf_listener_->transformPose(
00128         parent_frame_id_,
00129         new_pose, transformed_pose);
00130       latest_pose_ = transformed_pose.pose;
00131     }
00132     catch (...) {
00133       ROS_FATAL("tf exception");
00134       return;
00135     }
00136   }
00137 
00138 
00139   void CameraInfoPublisher::configCallback(Config &config, uint32_t level)
00140   {
00141     boost::mutex::scoped_lock lock(mutex_);
00142     width_ = config.width;
00143     height_ = config.height;
00144     f_ = config.f;
00145   }
00146 
00147   void CameraInfoPublisher::publishCameraInfo(const ros::Time& stamp)
00148   {
00149     boost::mutex::scoped_lock lock(mutex_);
00150     sensor_msgs::CameraInfo camera_info;
00151     camera_info.header.stamp = stamp;
00152     camera_info.header.frame_id = frame_id_;
00153     camera_info.height = height_;
00154     camera_info.width = width_;
00155     camera_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00156     camera_info.D.resize(5, 0);
00157     camera_info.K.assign(0.0);
00158     camera_info.R.assign(0.0);
00159     camera_info.P.assign(0.0);
00160     camera_info.K[0] = camera_info.K[4] = f_;
00161     
00162     camera_info.K[0] = camera_info.P[0] = camera_info.K[4] = camera_info.P[5] = f_;
00163     camera_info.K[2] = camera_info.P[2] = width_ / 2.0;
00164     camera_info.K[5] = camera_info.P[6] = height_ / 2.0;
00165     camera_info.K[8] = camera_info.P[10] = 1.0;
00166     camera_info.R[0] = camera_info.R[4] = camera_info.R[8] = 1.0;
00167     pub_camera_info_.publish(camera_info);
00168     static tf::TransformBroadcaster br;
00169     tf::Transform transform;
00170     transform.setOrigin(tf::Vector3(latest_pose_.position.x,
00171                                     latest_pose_.position.y,
00172                                     latest_pose_.position.z));
00173     tf::Quaternion q(latest_pose_.orientation.x,
00174                      latest_pose_.orientation.y,
00175                      latest_pose_.orientation.z,
00176                      latest_pose_.orientation.w);
00177     transform.setRotation(q);
00178     br.sendTransform(tf::StampedTransform(transform, stamp,
00179                                           parent_frame_id_, frame_id_));
00180   }
00181 
00182   void CameraInfoPublisher::pointcloudCallback(
00183     const sensor_msgs::PointCloud2::ConstPtr& msg)
00184   {
00185     publishCameraInfo(msg->header.stamp);
00186   }
00187   
00188   void CameraInfoPublisher::imageCallback(
00189     const sensor_msgs::Image::ConstPtr& msg)
00190   {
00191     publishCameraInfo(msg->header.stamp);
00192   }
00193   
00194   void CameraInfoPublisher::staticRateCallback(
00195     const ros::TimerEvent& event)
00196   {
00197     publishCameraInfo(event.current_real);
00198   }
00199   
00200 }
00201 
00202 
00203 int main(int argc, char** argv)
00204 {
00205   ros::init(argc, argv, "camera_info_publisher");
00206   jsk_interactive_marker::CameraInfoPublisher publisher;
00207   ros::spin();
00208   return 0;
00209 }


jsk_interactive_marker
Author(s): furuta
autogenerated on Wed May 1 2019 02:40:31