Changes to add CameraInfo publisher.

CMakeLists.txt

@@ -20,6 +20,7 @@ if( catkin_FOUND )
 		image_transport
 		roscpp
 		sensor_msgs
+		camera_info_manager
 		vision_msgs
 		std_msgs
 	)

launch/video_source.ros1.launch

@@ -1,15 +1,17 @@
 <launch>
 	<arg name="input" default="csi://0"/>
-	<arg name="input_width" default="0"/>
-	<arg name="input_height" default="0"/>
+	<arg name="input_width" default="800"/>
+	<arg name="input_height" default="600"/>
 	<arg name="input_codec" default="unknown"/>
 	<arg name="input_loop" default="0"/>
+	<arg name="framerate" default="30"/>
 
 	<node pkg="ros_deep_learning" type="video_source" name="video_source" output="screen">
 		<param name="resource" value="$(arg input)"/>
 		<param name="width" value="$(arg input_width)"/>
 		<param name="height" value="$(arg input_height)"/>
 		<param name="loop" value="$(arg input_loop)"/>
+		<param name="framerate" value="$(arg framerate)"/>
 	</node>
 
 </launch>

package.ros1.xml

@@ -7,12 +7,14 @@
   <license>Open</license>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>camera_info_manager</build_depend>
   <build_depend>cv_bridge</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>nodelet</build_depend>
+  <run_depend>camera_info_manager</run_depend>
   <run_depend>cv_bridge</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>roscpp</run_depend>

package.xml

@@ -7,12 +7,14 @@
   <license>Open</license>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>camera_info_manager</build_depend>
   <build_depend>cv_bridge</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>nodelet</build_depend>
+  <run_depend>camera_info_manager</run_depend>
   <run_depend>cv_bridge</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>roscpp</run_depend>

src/node_video_source.cpp

@@ -25,13 +25,15 @@
 
 #include <jetson-utils/videoSource.h>
 
-
+#include <image_transport/image_transport.h>
 
 // globals	
 videoSource* stream = NULL;
 imageConverter* image_cvt = NULL;
 Publisher<sensor_msgs::Image> image_pub = NULL;
 
+sensor_msgs::CameraInfo c_info;
+Publisher<sensor_msgs::CameraInfo> camera_info_pub = NULL;
 
 // aquire and publish camera frame
 bool aquireFrame()
@@ -68,6 +70,11 @@ bool aquireFrame()
 	image_pub->publish(msg);
 	ROS_DEBUG("published %ux%u video frame", stream->GetWidth(), stream->GetHeight());
 
+    // c_info.header.seq = pData->frame;
+    c_info.header.stamp = msg.header.stamp;
+    c_info.header.frame_id = msg.header.frame_id;
+    camera_info_pub->publish(c_info);
+
 	return true;
 }
 
@@ -78,7 +85,9 @@ int main(int argc, char **argv)
 	/*
 	 * create node instance
 	 */
-	ROS_CREATE_NODE("video_source");
+	ROS_CREATE_NODE("camera");
+
+	ros::NodeHandle nh_params("~");
 
 	/*
 	 * declare parameters
@@ -98,6 +107,7 @@ int main(int argc, char **argv)
 	ROS_DECLARE_PARAMETER("framerate", video_options.frameRate);
 	ROS_DECLARE_PARAMETER("loop", video_options.loop);
 
+
 	/*
 	 * retrieve parameters
 	 */
@@ -108,6 +118,31 @@ int main(int argc, char **argv)
 	ROS_GET_PARAMETER("framerate", video_options.frameRate);
 	ROS_GET_PARAMETER("loop", video_options.loop);
 
+	std::string camera_info_url;
+	std::string camera_name;
+
+	ROS_GET_PARAMETER_OR("camera_info_url", camera_info_url, std::string("package://ros_deep_learning/camera_info/camera.yaml"));
+	ROS_GET_PARAMETER_OR("camera_name", camera_name, std::string("camera"));
+
+	// I'm not sure how to write this lines on ros_compact.h
+	camera_info_manager::CameraInfoManager c_info_man(nh_params, camera_name, camera_info_url);
+	if (!c_info_man.loadCameraInfo(camera_info_url)) {
+		ROS_INFO("Calibration file missing. Camera not calibrated");
+	} else {
+		c_info = c_info_man.getCameraInfo();
+		ROS_INFO("Camera successfully calibrated from default file");
+	}
+
+	if (!c_info_man.loadCameraInfo("")) {
+		ROS_INFO("No device specifc calibration found");
+	} else {
+		c_info = c_info_man.getCameraInfo();
+		ROS_INFO("Camera successfully calibrated from device specifc file");
+	}
+	// Create a Camera_Info publisher
+	ROS_CREATE_PUBLISHER(sensor_msgs::CameraInfo, "camera_info", 1, camera_info_pub);
+
+
 	if( resource_str.size() == 0 )
 	{
 		ROS_ERROR("resource param wasn't set - please set the node's resource parameter to the input device/filename/URL");
@@ -149,7 +184,7 @@ int main(int argc, char **argv)
 	/*
 	 * advertise publisher topics
 	 */
-	ROS_CREATE_PUBLISHER(sensor_msgs::Image, "raw", 2, image_pub);
+	ROS_CREATE_PUBLISHER(sensor_msgs::Image, "image_raw", 2, image_pub);
 
 
 	/*

src/ros_compat.h

@@ -25,12 +25,16 @@
 
 #ifdef ROS1
 
+
 #include <ros/ros.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/image_encodings.h>
 #include <vision_msgs/Classification2D.h>
 #include <vision_msgs/Detection2DArray.h>
 #include <vision_msgs/VisionInfo.h>
+#include <camera_info_manager/camera_info_manager.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/SetCameraInfo.h>
 
 #define ROS_CREATE_NODE(name)				\
 		ros::init(argc, argv, name); 		\