Merge pull request #793 from 708yamaguchi/camera-info-pub-yaml

doc/jsk_interactive_marker/nodes/camera_info_publisher.md

@@ -0,0 +1,64 @@
+# camera_info_publisher
+
+`camera_info_publisher` provides camera info topics to an image or pointcloud without camera info.
+
+
+## Parameters
+
+* `~yaml_filename` (String, default: ``)
+
+   Path to yaml file which has camera info information.
+
+* `~frame_id` (String, default: `camera`)
+
+   Frame id of camera info.
+
+* `~parent_frame_id_` (Bool, default: `base_link`)
+
+   Frame id of interactive marker.
+
+* `~sync_pointcloud` (String, default: `false`)
+
+    Synchronize camera info to pointcloud.
+    If both `~sync_pointcloud` and `~sync_image` are not specified, camera info is published at a static rate.
+
+* `~sync_image` (String, default: `false`)
+
+    Synchronize camera info to image.
+
+* `~static_rate` (Double, default: `30.0`)
+
+    Static rate at which camera info is published.
+    If both `~sync_pointcloud` and `~sync_image` are not specified, camera info is published at a static rate.
+
+* `~width` (Double, default: `640`)
+
+    With of published camera info. This parameter is enabled when `~yaml_filename` is not speficied. This parameter can be changed by dynamic reconfigure.
+
+* `~height` (Double, default: `480`)
+
+    Height of published camera info. This parameter is enabled when `~yaml_filename` is not speficied. This parameter can be changed by dynamic reconfigure.
+
+* `~f` (Double, default: `525`)
+
+    F of published camera_info. This parameter is enabled when `~yaml_filename` is not speficied. This parameter can be changed by dynamic reconfigure.
+
+
+## Subscribing Topics
+
+* `~input` (`sensor_msgs/Image` or `sensor_msgs/Pointcloud2`)
+
+   Image or pointcloud whose camera info is published.
+
+
+## Publishing Topics
+
+* `~camera_info` (`sensor_msgs/CameraInfo`)
+
+   Camera info which has the same timestamp as the input topic.
+
+## Sample
+
+```
+roslaunch jsk_interactive_marker sample_camera_info_publisher.launch
+```

jsk_interactive_markers/jsk_interactive_marker/CMakeLists.txt

@@ -112,7 +112,7 @@ target_link_libraries(interactive_marker_interface ${catkin_LIBRARIES} ${orocos_
 add_dependencies(interactive_marker_interface ${PROJECT_NAME}_generate_messages_cpp ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS})
 
 add_executable(camera_info_publisher src/camera_info_publisher.cpp src/interactive_marker_utils.cpp)
-target_link_libraries(camera_info_publisher ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ${Boost_LIBRARIES} jsk_interactive_marker)
+target_link_libraries(camera_info_publisher ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ${Boost_LIBRARIES} jsk_interactive_marker yaml-cpp)
 add_dependencies(camera_info_publisher ${PROJECT_NAME}_generate_messages_cpp ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS})
 
 add_executable(urdf_model_marker src/urdf_model_marker.cpp src/urdf_model_marker_main.cpp src/interactive_marker_utils.cpp)

jsk_interactive_markers/jsk_interactive_marker/config/sample_camera_info.yaml

@@ -0,0 +1,29 @@
+# This file is generated by
+# $ rosrun camera_calibration cameracalibrator.py
+
+image_width: 1504
+image_height: 1504
+camera_name: narrow_stereo
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [753.86903,   0.     , 829.59748,
+           0.     , 703.23599, 918.33654,
+           0.     ,   0.     ,   1.     ]
+camera_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.287251, 0.041911, -0.039052, -0.022004, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1., 0., 0.,
+         0., 1., 0.,
+         0., 0., 1.]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [678.98792,   0.     , 847.68605,   0.     ,
+           0.     , 621.01874, 874.74736,   0.     ,
+           0.     ,   0.     ,   1.     ,   0.     ]

jsk_interactive_markers/jsk_interactive_marker/include/jsk_interactive_marker/camera_info_publisher.h

@@ -43,6 +43,7 @@
 #include "jsk_interactive_marker/interactive_marker_helpers.h"
 #include <dynamic_reconfigure/server.h>
 #include "jsk_interactive_marker/CameraInfoPublisherConfig.h"
+#include <yaml-cpp/yaml.h>
 
 namespace jsk_interactive_marker
 {
@@ -87,6 +88,8 @@ namespace jsk_interactive_marker
     double width_;
     double height_;
     double f_;
+    std::string yaml_filename_;
+    YAML::Node camera_info_yaml_;
     geometry_msgs::Pose latest_pose_;
 
   private:

jsk_interactive_markers/jsk_interactive_marker/launch/sample_camera_info_publisher.launch

@@ -0,0 +1,12 @@
+<launch>
+
+  <node pkg="jsk_interactive_marker" name="camera_info_publisher" type="camera_info_publisher" output="screen">
+  </node>
+
+  <node pkg="jsk_interactive_marker" name="camera_info_publisher_with_yaml" type="camera_info_publisher" output="screen">
+    <rosparam subst_value="true">
+      yaml_filename: $(find jsk_interactive_marker)/config/sample_camera_info.yaml
+    </rosparam>
+  </node>
+
+</launch>

jsk_interactive_markers/jsk_interactive_marker/src/camera_info_publisher.cpp

@@ -46,6 +46,13 @@ namespace jsk_interactive_marker
     latest_pose_.orientation.w = 1.0;
     tf_listener_.reset(new tf::TransformListener());
     pub_camera_info_ = pnh.advertise<sensor_msgs::CameraInfo>("camera_info", 1);
+    if (!pnh.getParam("yaml_filename", yaml_filename_)) {
+      yaml_filename_ = "";
+      ROS_WARN("~yaml_fliename is not specified, use default camera info parameters");
+    }
+    else {
+      camera_info_yaml_ = YAML::LoadFile(yaml_filename_);
+    }
 
     // setup dynamic reconfigure
     srv_ = std::make_shared <dynamic_reconfigure::Server<Config> > (pnh);
@@ -150,20 +157,42 @@ namespace jsk_interactive_marker
     sensor_msgs::CameraInfo camera_info;
     camera_info.header.stamp = stamp;
     camera_info.header.frame_id = frame_id_;
-    camera_info.height = height_;
-    camera_info.width = width_;
-    camera_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
-    camera_info.D.resize(5, 0);
-    camera_info.K.assign(0.0);
-    camera_info.R.assign(0.0);
-    camera_info.P.assign(0.0);
-    camera_info.K[0] = camera_info.K[4] = f_;
-
-    camera_info.K[0] = camera_info.P[0] = camera_info.K[4] = camera_info.P[5] = f_;
-    camera_info.K[2] = camera_info.P[2] = width_ / 2.0;
-    camera_info.K[5] = camera_info.P[6] = height_ / 2.0;
-    camera_info.K[8] = camera_info.P[10] = 1.0;
-    camera_info.R[0] = camera_info.R[4] = camera_info.R[8] = 1.0;
+    if (yaml_filename_ != "") {
+      camera_info.height = camera_info_yaml_["image_height"].as<uint32_t>();
+      camera_info.width = camera_info_yaml_["image_width"].as<uint32_t>();
+      camera_info.distortion_model =
+        camera_info_yaml_["camera_model"].as<std::string>();
+      std::vector<double> D, K, R, P;
+      boost::array<double, 9ul> Kl, Rl;
+      boost::array<double, 12ul> Pl;
+      D = camera_info_yaml_["distortion_coefficients"]["data"].as<std::vector<double>>();
+      K = camera_info_yaml_["camera_matrix"]["data"].as<std::vector<double>>();
+      std::memcpy(&Kl[0], &K[0], sizeof(double)*9);
+      R = camera_info_yaml_["rectification_matrix"]["data"].as<std::vector<double>>();
+      std::memcpy(&Rl[0], &R[0], sizeof(double)*9);
+      P = camera_info_yaml_["projection_matrix"]["data"].as<std::vector<double>>();
+      std::memcpy(&Pl[0], &P[0], sizeof(double)*12);
+      camera_info.D = D;
+      camera_info.K = Kl;
+      camera_info.R = Rl;
+      camera_info.P = Pl;
+    }
+    else {
+      camera_info.height = height_;
+      camera_info.width = width_;
+      camera_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
+      camera_info.D.resize(5, 0);
+      camera_info.K.assign(0.0);
+      camera_info.R.assign(0.0);
+      camera_info.P.assign(0.0);
+      camera_info.K[0] = camera_info.K[4] = f_;
+
+      camera_info.K[0] = camera_info.P[0] = camera_info.K[4] = camera_info.P[5] = f_;
+      camera_info.K[2] = camera_info.P[2] = width_ / 2.0;
+      camera_info.K[5] = camera_info.P[6] = height_ / 2.0;
+      camera_info.K[8] = camera_info.P[10] = 1.0;
+      camera_info.R[0] = camera_info.R[4] = camera_info.R[8] = 1.0;
+    }
     pub_camera_info_.publish(camera_info);
     static tf::TransformBroadcaster br;
     tf::Transform transform;