New package to advertise virtual RGBD camera

it publishes:

- camera_info with calibration_file.
- image with image_file.
- points with pcd_file.

virtual_camera/CMakeLists.txt

@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(virtual_camera)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+  camera_info_manager
+  jsk_topic_tools
+  nodelet
+  sensor_msgs
+)
+
+find_package(Boost REQUIRED)
+
+
+###################################
+## catkin specific configuration ##
+###################################
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS
+  camera_info_manager
+  jsk_topic_tools
+  nodelet
+  #
+  DEPENDS
+  Boost
+)
+
+###########
+## Build ##
+###########
+
+include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
+
+jsk_nodelet(
+  src/virtual_camera_driver.cpp
+  "${PROJECT_NAME}/driver"
+  "driver"
+  ${PROJECT_NAME}_sources
+  ${PROJECT_NAME}_nodes
+)
+
+add_library(${PROJECT_NAME} ${${PROJECT_NAME}_sources})
+
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+#############
+## Install ##
+#############
+
+install(TARGETS
+  ${PROJECT_NAME}
+  ${${PROJECT_NAME}_nodes}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY
+  include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(FILES
+  nodelets.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(catkin REQUIRED COMPONENTS rostest)
+  add_rostest(test/driver.test)
+endif()

virtual_camera/include/virtual_camera/virtual_camera_driver.h

@@ -0,0 +1,76 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, Kentaro Wada
+ *  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/o2r other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Kentaro Wada 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.
+ *********************************************************************/
+
+#ifndef VIRTUAL_CAMERA_VIRTUAL_CAMERA_DRIVER_H_
+#define VIRTUAL_CAMERA_VIRTUAL_CAMERA_DRIVER_H_
+
+#include <boost/shared_ptr.hpp>
+
+#include <nodelet/nodelet.h>
+#include <camera_info_manager/camera_info_manager.h>
+
+namespace virtual_camera
+{
+
+class VirtualCameraDriver: public nodelet::Nodelet
+{
+public:
+  VirtualCameraDriver():
+    publish_info_(false),
+    publish_image_(false),
+    publish_cloud_(false) {}
+protected:
+  virtual void onInit();
+  void publishCb(const ros::TimerEvent& event, const std::string target);
+
+  ros::NodeHandle* nh_;
+  ros::NodeHandle* pnh_;
+
+  boost::shared_ptr<camera_info_manager::CameraInfoManager> info_manager;
+
+  ros::Publisher pub_info_;
+  ros::Publisher pub_image_;
+  ros::Publisher pub_cloud_;
+  ros::Timer timer_pub_info_;
+  ros::Timer timer_pub_image_;
+  ros::Timer timer_pub_cloud_;
+private:
+  bool publish_info_;
+  bool publish_image_;
+  bool publish_cloud_;
+};
+
+}  // namespace virtual_camera
+
+#endif  // VIRTUAL_CAMERA_VIRTUAL_CAMERA_DRIVER_H_

virtual_camera/nodelets.xml

@@ -0,0 +1,11 @@
+<library path="lib/libvirtual_camera">
+
+  <class name="virtual_camera/driver"
+         type="virtual_camera::VirtualCameraDriver"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      Virtual camera driver nodelet.
+    </description>
+  </class>
+
+</library>

virtual_camera/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package>
+  <name>virtual_camera</name>
+  <version>0.3.25</version>
+  <description>A virtual ROS driver for RGB-D cameras.</description>
+  <maintainer email="[email protected]">Kentaro Wada</maintainer>
+  <license>MIT</license>
+  <url type="website">https://github.com/jsk-ros-pkg/jsk_recognition</url>
+  <author email="[email protected]">Kentaro Wada</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>camera_info_manager</build_depend>
+  <build_depend>jsk_topic_tools</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+
+  <run_depend>camera_info_manager</run_depend>
+  <run_depend>jsk_topic_tools</run_depend>
+  <run_depend>nodelet</run_depend>
+
+  <test_depend>jsk_tools</test_depend>
+  <test_depend>rostest</test_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelets.xml"/>
+  </export>
+</package>

virtual_camera/samples/camera_info/calibration_0001.yaml

@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 480
+camera_name: 08144361026320a0
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [1168.68, 0, 295.015, 0, 1169.01, 252.247, 0, 0, 1]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-1.04482, 1.59252, -0.0196308, 0.0287906, 0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [1168.68, 0, 295.015, 0, 0, 1169.01, 252.247, 0, 0, 0, 1, 0]

virtual_camera/samples/sample_driver.launch

@@ -0,0 +1,10 @@
+<launch>
+
+  <node name="virtual_camera"
+        pkg="virtual_camera" type="driver">
+    <rosparam subst_value="true">
+      camera_info_file: file://$(find virtual_camera)/samples/camera_info/calibration_0001.yaml
+    </rosparam>
+  </node>
+
+</launch>

virtual_camera/src/virtual_camera_driver.cpp

@@ -0,0 +1,92 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, Kentaro Wada.
+ *  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/o2r other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Kentaro Wada 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 "virtual_camera/virtual_camera_driver.h"
+
+#include <boost/bind.hpp>
+
+#include <camera_info_manager/camera_info_manager.h>
+#include <ros/ros.h>
+#include <sensor_msgs/CameraInfo.h>
+
+namespace virtual_camera
+{
+
+void VirtualCameraDriver::onInit()
+{
+  nh_ = new ros::NodeHandle(getNodeHandle());
+  pnh_ = new ros::NodeHandle(getPrivateNodeHandle());
+
+  if (pnh_->hasParam("camera_info_file"))
+  {
+    std::string camera_info_file;
+    pnh_->getParam("camera_info_file", camera_info_file);
+    info_manager.reset(new camera_info_manager::CameraInfoManager(
+      *nh_, "virtual_camera", camera_info_file));
+    pub_info_ = pnh_->advertise<sensor_msgs::CameraInfo>("camera_info", 1);
+
+    double camera_info_rate;
+    pnh_->param("camera_info_rate", camera_info_rate, 30.0);
+    timer_pub_info_ = nh_->createTimer(
+      ros::Duration(1.0 / camera_info_rate),
+      boost::bind(&VirtualCameraDriver::publishCb, this, _1, /*target=*/"camera_info"));
+    publish_info_ = true;
+  }
+
+  // TODO(wkentaro): publish image & cloud
+
+  if (!(publish_info_ || publish_image_ || publish_cloud_))
+  {
+    ROS_FATAL("Nothing to publish. You should set ~camera_info_file, ~image_file or ~pcd_file.");
+    exit(1);
+  }
+}
+
+void VirtualCameraDriver::publishCb(const ros::TimerEvent& event, std::string target)
+{
+  if (target == "camera_info" && publish_info_)
+  {
+    sensor_msgs::CameraInfo info_msg = info_manager->getCameraInfo();
+    info_msg.header.stamp = event.current_real;
+    pub_info_.publish(info_msg);
+  }
+
+  // TODO(wkentaro): publish image & cloud
+
+}
+
+}  // namespace virtual_camera
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(virtual_camera::VirtualCameraDriver, nodelet::Nodelet);

virtual_camera/test/driver.test

@@ -0,0 +1,14 @@
+<launch>
+
+  <include file="$(find virtual_camera)/samples/sample_driver.launch" />
+
+  <test test-name="test_driver"
+        name="test_driver"
+        pkg="jsk_tools" type="test_topic_published.py">
+    <rosparam>
+      topic_0: /virtual_camera/camera_info
+      timeout_0: 10
+    </rosparam>
+  </test>
+
+</launch>