ROS as an additional input to InfiniTAM

InfiniTAM/CMakeLists.txt → CMakeLists.txt

@@ -12,6 +12,32 @@ PROJECT(InfiniTAM)
 
 SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")
 
+####################################
+# Find catkin macros and libraries #
+####################################
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  message_filters
+)
+
+find_package(Boost REQUIRED
+  system
+)
+
+include_directories(
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES using_markers
+#  CATKIN_DEPENDS roscpp visualization_msgs
+#  DEPENDS system_lib
+)
+
 #################################
 # Add additional compiler flags #
 #################################
@@ -38,9 +64,9 @@ ENDIF()
 # Add subdirectories #
 ######################
 
-ADD_SUBDIRECTORY(Apps)
-ADD_SUBDIRECTORY(FernRelocLib)
-ADD_SUBDIRECTORY(InputSource)
-ADD_SUBDIRECTORY(ITMLib)
-ADD_SUBDIRECTORY(MiniSlamGraphLib)
-ADD_SUBDIRECTORY(ORUtils)
+ADD_SUBDIRECTORY(InfiniTAM/Apps)
+ADD_SUBDIRECTORY(InfiniTAM/FernRelocLib)
+ADD_SUBDIRECTORY(InfiniTAM/InputSource)
+ADD_SUBDIRECTORY(InfiniTAM/ITMLib)
+ADD_SUBDIRECTORY(InfiniTAM/MiniSlamGraphLib)
+ADD_SUBDIRECTORY(InfiniTAM/ORUtils)

InfiniTAM/Apps/InfiniTAM/CMakeLists.txt

@@ -52,7 +52,7 @@ INCLUDE(${PROJECT_SOURCE_DIR}/cmake/SetCUDAAppTarget.cmake)
 # Specify the libraries to link #
 #################################
 
-TARGET_LINK_LIBRARIES(${targetname} InputSource ITMLib MiniSlamGraphLib ORUtils FernRelocLib)
+TARGET_LINK_LIBRARIES(${targetname} InputSource ITMLib MiniSlamGraphLib ORUtils FernRelocLib ${catkin_LIBRARIES})
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkFFmpeg.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkGLUT.cmake)
 INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkLibRoyale.cmake)

InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp

@@ -3,8 +3,11 @@
 #include <cstdlib>
 #include <iostream>
 
+#include <ros/ros.h>
+
 #include "UIEngine.h"
 
+#include "../../InputSource/ROSEngine.h"
 #include "../../InputSource/OpenNIEngine.h"
 #include "../../InputSource/Kinect2Engine.h"
 #include "../../InputSource/LibUVCEngine.h"
@@ -78,6 +81,17 @@ static void CreateDefaultImageSource(ImageSourceEngine* & imageSource, IMUSource
 		}
 	}
 
+	if (imageSource == NULL)
+	{
+		printf("trying ROS input: /camera/depth/image_raw, /camera/rgb/image_color \n");
+		imageSource = new ROSEngine(calibFile);
+		if (imageSource->getDepthImageSize().x == 0)
+		{
+			delete imageSource;
+			imageSource = NULL;
+		}
+	}
+
 	if (imageSource == NULL)
 	{
 		// If no calibration file specified, use the factory default calibration
@@ -142,6 +156,10 @@ static void CreateDefaultImageSource(ImageSourceEngine* & imageSource, IMUSource
 int main(int argc, char** argv)
 try
 {
+	// Start ROS
+	ros::init(argc, argv, "infinitam_ros");
+	ros::start();
+
 	const char *arg1 = "";
 	const char *arg2 = NULL;
 	const char *arg3 = NULL;
@@ -207,6 +225,8 @@ try
 	delete internalSettings;
 	delete imageSource;
 	if (imuSource != NULL) delete imuSource;
+
+	ros::shutdown();
 	return 0;
 }
 catch(std::exception& e)

InfiniTAM/InputSource/CMakeLists.txt

@@ -44,6 +44,7 @@ LibUVCEngine.cpp
 OpenNIEngine.cpp
 PicoFlexxEngine.cpp
 RealSenseEngine.cpp
+ROSEngine.cpp
 )
 
 SET(headers
@@ -57,6 +58,7 @@ LibUVCEngine.h
 OpenNIEngine.h
 PicoFlexxEngine.h
 RealSenseEngine.h
+ROSEngine.h
 )
 
 #############################

InfiniTAM/InputSource/ROSEngine.cpp

@@ -0,0 +1,83 @@
+// authored by https://github.com/ravich2-7183
+
+#include "ROSEngine.h"
+
+using namespace std;
+using namespace ORUtils;
+using namespace InputSource;
+using namespace sensor_msgs;
+using namespace message_filters;
+
+void ROSEngine::processMessage(const ImageConstPtr& rgb_image_msg, const ImageConstPtr& depth_image_msg)
+{
+	std::lock_guard<std::mutex> process_message_lock(images_mutex_);
+
+	// copy rgb_image_msg into rgb_image_
+	Vector4u *rgb = rgb_image_.GetData(MEMORYDEVICE_CPU);
+	for(int i = 0; i < rgb_image_.noDims.x * rgb_image_.noDims.y; i++) {
+		Vector4u newPix;
+		newPix.x = (rgb_image_msg->data)[i*3+2];
+		newPix.y = (rgb_image_msg->data)[i*3+1];
+		newPix.z = (rgb_image_msg->data)[i*3+0];
+		newPix.w = 255;
+
+		rgb[i] = newPix;
+	}
+
+	// copy depth_image_msg into depth_image_
+	short *depth = depth_image_.GetData(MEMORYDEVICE_CPU);
+	const short *depth_msg_data  = reinterpret_cast<const short*>(depth_image_msg->data.data());
+	for(int i = 0; i < depth_image_.noDims.x * depth_image_.noDims.y; i++) {
+		depth[i] = depth_msg_data[i];
+	}
+}
+
+void ROSEngine::topicListenerThread()
+{
+	// subscribe to rgb and depth topics
+    message_filters::Subscriber<sensor_msgs::Image> rgb_sub_(nh_, "/camera/rgb/image_color", 1);
+	message_filters::Subscriber<sensor_msgs::Image> depth_sub_(nh_, "/camera/depth/image_raw", 1); 
+	typedef sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> ITAMSyncPolicy;
+	Synchronizer<ITAMSyncPolicy> sync(ITAMSyncPolicy(10), rgb_sub_, depth_sub_);
+	sync.registerCallback(boost::bind(&ROSEngine::processMessage, this, _1, _2));
+
+	ros::spin();
+}
+
+ROSEngine::ROSEngine(const char *calibFilename,
+					 Vector2i requested_imageSize_rgb,
+					 Vector2i requested_imageSize_d) :
+			BaseImageSourceEngine(calibFilename),
+			nh_(),
+			rgb_image_(requested_imageSize_rgb, MEMORYDEVICE_CPU),
+			depth_image_(requested_imageSize_d, MEMORYDEVICE_CPU),
+			topic_listener_thread(&ROSEngine::topicListenerThread, this) // Starts up topicListenerThread
+{
+	this->calib.disparityCalib.SetStandard(); // assumes depth is in millimeters
+}
+
+void ROSEngine::getImages(ITMUChar4Image *rgbImage, ITMShortImage *rawDepthImage)
+{
+	std::lock_guard<std::mutex> get_images_lock(images_mutex_);
+
+	rgbImage->SetFrom(&rgb_image_,        MemoryBlock<Vector4u>::CPU_TO_CPU); 
+	rawDepthImage->SetFrom(&depth_image_, MemoryBlock<short>::CPU_TO_CPU);
+}
+
+bool ROSEngine::hasMoreImages(void) const
+{
+	return ros::ok();
+}
+
+Vector2i ROSEngine::getDepthImageSize(void) const
+{
+	return Vector2i(depth_image_.noDims.x , depth_image_.noDims.y);
+}
+
+Vector2i ROSEngine::getRGBImageSize(void) const
+{
+	return Vector2i(rgb_image_.noDims.x , rgb_image_.noDims.y);
+}
+
+ROSEngine::~ROSEngine()
+{}

InfiniTAM/InputSource/ROSEngine.h

@@ -0,0 +1,59 @@
+// Copyright 2014-2017 Oxford University Innovation Limited and the authors of InfiniTAM
+
+#pragma once
+
+#include "../ORUtils/FileUtils.h"
+#include "ImageSourceEngine.h"
+
+#include <iostream>
+#include <stdexcept>
+#include <string>
+#include <thread>
+#include <mutex>
+
+#include <ros/ros.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <sensor_msgs/Image.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+
+#if (!defined USING_CMAKE) && (defined _MSC_VER)
+#pragma comment(lib, "OpenNI2")
+#endif
+
+namespace InputSource {
+
+class ROSEngine : public BaseImageSourceEngine
+{
+private:
+	ros::NodeHandle nh_;
+	ros::Subscriber rgb_sub_, depth_sub_;
+	ITMUChar4Image rgb_image_;
+	ITMShortImage depth_image_;
+	std::thread topic_listener_thread;
+	std::mutex images_mutex_;
+
+public:
+	ROSEngine(const char *calibFilename, 
+			  Vector2i imageSize_rgb = Vector2i(640, 480),
+			  Vector2i imageSize_d = Vector2i(640, 480));
+	~ROSEngine();
+
+	/**
+	   @param rgb_image_msg must have an encoding "bgr8"
+	   @param depth_image_msg must have an encoding "mono16" or "16UC1" which is the depth in mm. 
+	*/
+	void processMessage(const sensor_msgs::ImageConstPtr& rgb_image_msg, const sensor_msgs::ImageConstPtr& depth_image_msg);
+
+	void topicListenerThread();
+
+	bool hasMoreImages(void) const;
+	void getImages(ITMUChar4Image *rgb, ITMShortImage *rawDepth);
+	Vector2i getDepthImageSize(void) const;
+	Vector2i getRGBImageSize(void) const;
+};
+
+}