Add camera info parameter

rocon_rtsp_camera_relay/CMakeLists.txt

@@ -7,6 +7,7 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   std_msgs
   sensor_msgs
+  camera_info_manager
 )
 
 find_package(OpenCV)

rocon_rtsp_camera_relay/include/rocon_rtsp_camera_relay/rocon_rtsp_camera_relay.hpp

@@ -6,15 +6,16 @@
 #ifndef ROCON_RTSP_CAMERA_RELAY
 #define ROCON_RTSP_CAMERA_RELAY
 
-#include<ros/ros.h>
-#include<opencv2/opencv.hpp>
-#include<cv_bridge/cv_bridge.h>
+#include <ros/ros.h>
+#include <opencv2/opencv.hpp>
+#include <cv_bridge/cv_bridge.h>
 
-#include<std_msgs/String.h>
-#include<image_transport/image_transport.h>
-#include<sensor_msgs/image_encodings.h>
-#include<sensor_msgs/Image.h>
-#include<sensor_msgs/CameraInfo.h>
+#include <std_msgs/String.h>
+#include <image_transport/image_transport.h>
+#include <camera_info_manager/camera_info_manager.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
 
 namespace rocon {
 
@@ -23,10 +24,10 @@ class RoconRtspCameraRelay {
     RoconRtspCameraRelay(ros::NodeHandle& n);
     ~RoconRtspCameraRelay();
 
-    bool init(const std::string video_stream_url);
-    bool reset(const std::string video_stream_url);
-
+    bool init(const std::string video_stream_url, const std::string camera_name , const std::string camera_info_url);
+    bool reset(const std::string video_stream_url, const std::string camera_name , const std::string camera_info_url);
     void spin();
+    boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;
 
   protected:
     void convertCvToRosImg(const cv::Mat& mat, sensor_msgs::Image& ros_img, sensor_msgs::CameraInfo& ci);
@@ -35,6 +36,8 @@ class RoconRtspCameraRelay {
     cv::VideoCapture                vcap_;
     std::string                     video_stream_address_;
     std::string                     status_;
+    std::string                     camera_info_url_;
+    std::string                     camera_name_;
 
     image_transport::Publisher pub_video_;
     ros::Publisher pub_camera_info_;

rocon_rtsp_camera_relay/package.xml

@@ -15,11 +15,14 @@
   <build_depend>image_transport</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>camera_info_manager</build_depend>
+
   <run_depend>roscpp</run_depend>
   <run_depend>cv_bridge</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>camera_info_manager</run_depend>
 
   <export>
     <rocon_app>rapps/image_stream/image_stream.rapp</rocon_app>

rocon_rtsp_camera_relay/src/main.cpp

@@ -11,12 +11,15 @@ int main (int argc, char** argv)
   ros::init(argc, argv, "rtsp_camera_relay");
   ros::NodeHandle pnh("~");
   std::string video_stream_url, user, password;
+  std::string camera_name, camera_info_url;
 
   pnh.getParam("video_stream_url", video_stream_url);
+  pnh.getParam("camera_info_url", camera_info_url);
+  pnh.getParam("camera_name", camera_name);
 
   rocon::RoconRtspCameraRelay rtsp(pnh);
   ROS_INFO("Rtsp Camera : Initialising..");
-  if(!rtsp.init(video_stream_url))
+  if(!rtsp.init(video_stream_url, camera_name, camera_info_url))
   {
     ROS_ERROR("Rtsp Camera : Failed to initialise stream");
     return -1;

rocon_rtsp_camera_relay/src/rocon_rtsp_camera_relay.cpp

@@ -20,19 +20,24 @@ RoconRtspCameraRelay::~RoconRtspCameraRelay()
   vcap_.release();
 }
 
-bool RoconRtspCameraRelay::init(const std::string video_stream_url) {
+bool RoconRtspCameraRelay::init(const std::string video_stream_url, const std::string camera_name , const std::string camera_info_url)
+{
   video_stream_address_ = video_stream_url;
+  camera_name_ = camera_name;
+  camera_info_url_ = camera_info_url;
+  cinfo_.reset(new camera_info_manager::CameraInfoManager(nh_, camera_name_, camera_info_url_));
+  cinfo_->loadCameraInfo(camera_info_url_);
 
   if (!vcap_.open(video_stream_address_)) 
     return false; 
   else
     return true;
 }
 
-bool RoconRtspCameraRelay::reset(const std::string video_stream_url)
+bool RoconRtspCameraRelay::reset(const std::string video_stream_url, const std::string camera_name , const std::string camera_info_url)
 {
   vcap_.release();
-  return init(video_stream_url);
+  return init(video_stream_url, camera_name, camera_info_url);
 }
 
 /*
@@ -46,9 +51,18 @@ void RoconRtspCameraRelay::convertCvToRosImg(const cv::Mat& mat, sensor_msgs::Im
   cv_img.image = mat;
   cv_img.toImageMsg(ros_img);
   ros_img.header.stamp = ros::Time::now();
+  // check for default camera info
+  if (!cinfo_->isCalibrated())
+  {
+    cinfo_->setCameraName(camera_name_);
+    sensor_msgs::CameraInfo camera_info;
+    camera_info.header = ros_img.header;
+    camera_info.width = ros_img.width;
+    camera_info.height = ros_img.height;
+    cinfo_->setCameraInfo(camera_info);
+  }
+  ci = cinfo_->getCameraInfo();
   ci.header = ros_img.header;
-  ci.width = ros_img.width;
-  ci.height = ros_img.height;
 
   return;
 }