Fixed error in function boost::math::round<d>(d): Value -inf can not be represented in the target integer type.

Author: progtologist

launch/k2_client.launch

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
-    <param name="serverNameOrIP" value="172.19.179.53" />
-    <group ns="/head/kinect2">
+    <param name="serverNameOrIP" value="192.168.1.3" />
+    <group ns="/kinect2">
         <param name="rgb_frame" value="/head/kinect2/rgb"/>
         <param name="depth_frame" value="/head/kinect2/depth"/>
         <param name="ir_frame" value="/head/kinect2/depth"/>

src/startRGB.cpp

@@ -27,10 +27,11 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 ***************************************************************************************/
 #include <k2_client/k2_client.h>
 
-int imageSize = 6220800;
+int imageSize = 8294400;
 int streamSize = imageSize + sizeof(double);
 std::string cameraName = "rgb";
 std::string imageTopicSubName = "image_color";
+std::string cameraInfoSubName = "camera_info";
 std::string cameraFrame = "";
 
 int main(int argC,char **argV)
@@ -40,35 +41,33 @@ int main(int argC,char **argV)
 	image_transport::ImageTransport imT(n);
 	std::string serverAddress;
 	n.getParam("/serverNameOrIP",serverAddress);
-    n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) +
-            "/rgb_frame", cameraFrame);
+    n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) + "/rgb_frame", cameraFrame);
     Socket mySocket(serverAddress.c_str(),const_cast<char*>("9000"),streamSize);
-    image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera(
-            imageTopicSubName, 1);
+    image_transport::Publisher imagePublisher = imT.advertise(imageTopicSubName, 1);
+    ros::Publisher cameraInfoPub = n.advertise<sensor_msgs::CameraInfo>(cameraInfoSubName,1);
 	camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName);
 	camInfoMgr.loadCameraInfo("");
 	cv::Mat frame;
 	cv_bridge::CvImage cvImage;
 	sensor_msgs::Image rosImage;
 	while(ros::ok())
 	{
-        printf("Got a frame.\n");
-
 		mySocket.readData();
-        printf("Creating mat.\n");
-        frame = cv::Mat(cv::Size(1920,1080), CV_8UC3, mySocket.mBuffer);
+        frame = cv::Mat(cv::Size(1920,1080), CV_8UC4, mySocket.mBuffer);
 		cv::flip(frame,frame,1);
-        printf("Getting time.\n");
 		double utcTime;
 		memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double));
+		cvImage.header.stamp = ros::Time(utcTime);
         cvImage.header.frame_id = cameraFrame.c_str();
-		cvImage.encoding = "bgr8";
+		cvImage.encoding = "bgra8";
 		cvImage.image = frame;
 		cvImage.toImageMsg(rosImage);
 		sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo();
+		camInfo.header.stamp = cvImage.header.stamp;
 		camInfo.header.frame_id = cvImage.header.frame_id;
+		cameraInfoPub.publish(camInfo);
         printf("Updating.\n");
-        cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime));
+        imagePublisher.publish(rosImage);
 		ros::spinOnce();
 	}
 	return 0;