Added stereo_camera node

CMakeLists.txt

@@ -276,6 +276,9 @@ add_executable(camera src/CameraNode.cpp)
 add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_link_libraries(camera ${Libraries})
 
+add_executable(stereo_camera src/StereoCameraNode.cpp)
+target_link_libraries(stereo_camera rtabmap_ros ${Libraries})
+
 IF(RTABMAP_GUI)
     add_executable(rtabmapviz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp)
     target_link_libraries(rtabmapviz rtabmap_ros ${QT_LIBRARIES} ${Libraries})

src/StereoCameraNode.cpp

@@ -0,0 +1,80 @@
+
+#include "ros/ros.h"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/core/CameraStereo.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap_ros/MsgConversion.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <cv_bridge/cv_bridge.h>
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	ros::init(argc, argv, "uvc_stereo_camera");
+
+	ros::NodeHandle pnh("~");
+	double rate = 0.0;
+	std::string id = "camera";
+	std::string frameId = "camera_link";
+	double scale = 1.0;
+	pnh.param("rate", rate, rate);
+	pnh.param("camera_id", id, id);
+	pnh.param("frame_id", frameId, frameId);
+	pnh.param("scale", scale, scale);
+
+	rtabmap::CameraStereoVideo camera(0, false, rate);
+
+	if(camera.init(UDirectory::homeDir() + "/.ros/camera_info", id))
+	{
+		ros::NodeHandle nh;
+		ros::NodeHandle left_nh(nh, "left");
+		ros::NodeHandle right_nh(nh, "right");
+		image_transport::ImageTransport left_it(left_nh);
+		image_transport::ImageTransport right_it(right_nh);
+
+		image_transport::Publisher imageLeftPub = left_it.advertise(left_nh.resolveName("image_raw"), 1);
+		image_transport::Publisher imageRightPub = right_it.advertise(right_nh.resolveName("image_raw"), 1);
+		ros::Publisher infoLeftPub = left_nh.advertise<sensor_msgs::CameraInfo>(left_nh.resolveName("camera_info"), 1);
+		ros::Publisher infoRightPub = right_nh.advertise<sensor_msgs::CameraInfo>(right_nh.resolveName("camera_info"), 1);
+
+		while(ros::ok())
+		{
+			rtabmap::SensorData data = camera.takeImage();
+			
+			ros::Time currentTime = ros::Time::now();
+
+			cv_bridge::CvImage imageLeft;
+			imageLeft.header.frame_id = frameId;
+			imageLeft.header.stamp = currentTime;
+			imageLeft.encoding = "bgr8";
+			cv::resize(data.imageRaw(), imageLeft.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
+			imageLeftPub.publish(imageLeft.toImageMsg());
+
+			cv_bridge::CvImage imageRight;
+			imageRight.header = imageLeft.header;
+			imageRight.encoding = "mono8";
+			cv::resize(data.rightRaw(), imageRight.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
+			imageRightPub.publish(imageRight.toImageMsg());
+
+			sensor_msgs::CameraInfo infoLeft, infoRight;
+			rtabmap_ros::cameraModelToROS(data.stereoCameraModel().left().scaled(scale), infoLeft);
+			rtabmap_ros::cameraModelToROS(data.stereoCameraModel().right().scaled(scale), infoRight);
+			infoLeft.header = imageLeft.header;
+			infoRight.header = imageLeft.header;
+			infoLeftPub.publish(infoLeft);
+			infoRightPub.publish(infoRight);
+
+			ros::spinOnce();
+		}
+	}
+	else
+	{
+		ROS_ERROR("Could not initialize the camera!");
+	}
+
+	return 0;
+}