{"action":"created","comment":{"url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls/comments/54440098","id":54440098,"diff_hunk":"@@ -53,57 +54,119 @@\n #include <opencv2/imgproc/imgproc.hpp>\n #include <opencv2/calib3d/calib3d.hpp>\n \n+//PCL includes\n+#include <sensor_msgs/PointCloud2.h>\n+#include <pcl_conversions/pcl_conversions.h>\n+#include <pcl/point_cloud.h>\n+#include <pcl/point_types.h>\n+\n //ZED Includes\n #include <zed/Camera.hpp>\n \n using namespace sl::zed;\n using namespace std;\n \n-int computeDepth;\n int confidence;\n+bool computeDepth;\n+bool pointCloudThreadRunning;\n+\n+\n+/* \\brief Publish a cv::Mat image with a ros Publisher\n+ * \\param img : the image to publish\n+ * \\param pub_img : the publisher object to use\n+ * \\param img_frame_id : the id of the reference frame of the image\n+ * \\param t : the ros::Time to stamp the image\n+ */\n+void publishImage(cv::Mat img, image_transport::Publisher &pub_img, string img_frame_id, ros::Time t) {\n+    cv_bridge::CvImage img_im;\n+    img_im.image = img;\n+    img_im.encoding = sensor_msgs::image_encodings::BGR8;\n+    img_im.header.frame_id = img_frame_id;\n+    img_im.header.stamp = t;\n+    pub_img.publish(img_im.toImageMsg());\n+}\n \n-// Function to publish left and depth/right images\n-\n-void publishImages(cv::Mat left, cv::Mat second, image_transport::Publisher &pub_left, image_transport::Publisher &pub_second,\n-        string left_frame_id, string second_frame_id, ros::Time t) {\n \n-    // Publish left image\n-    cv_bridge::CvImage left_im;\n-    left_im.image = left;\n-    left_im.encoding = sensor_msgs::image_encodings::BGR8;\n-    left_im.header.frame_id = left_frame_id;\n-    left_im.header.stamp = t;\n-    pub_left.publish(left_im.toImageMsg());\n+/* \\brief Publish a cv::Mat depth image with a ros Publisher\n+ * \\param depth : the depth image to publish\n+ * \\param pub_depth : the publisher object to use\n+ * \\param depth_frame_id : the id of the reference frame of the depth image\n+ * \\param t : the ros::Time to stamp the depth image\n+ */\n+void publishDepth(cv::Mat depth, image_transport::Publisher &pub_depth, string depth_frame_id, ros::Time t) {\n+    cv_bridge::CvImage depth_im;\n+    depth_im.image = depth;\n+    depth_im.encoding = sensor_msgs::image_encodings::TYPE_16UC1;\n+    depth_im.header.frame_id = depth_frame_id;\n+    depth_im.header.stamp = t;\n+    pub_depth.publish(depth_im.toImageMsg());\n+}\n \n-    // Publish second image\n-    cv_bridge::CvImage second_im;\n-    second_im.image = second;\n-    second_im.encoding = (computeDepth) ? sensor_msgs::image_encodings::TYPE_16UC1 : sensor_msgs::image_encodings::BGR8;\n-    second_im.header.frame_id = second_frame_id;\n-    second_im.header.stamp = t;\n-    pub_second.publish(second_im.toImageMsg());\n \n+/* \\brief Publish a pointCloud with a ros Publisher\n+ * \\param p_could : the float pointer to point cloud datas\n+ * \\param width : the width of the point cloud\n+ * \\param height : the height of the point cloud\n+ * \\param pub_cloud : the publisher object to use\n+ * \\param cloud_frame_id : the id of the reference frame of the point cloud\n+ * \\param t : the ros::Time to stamp the point cloud\n+ */\n+void publishPointCloud(float* p_cloud, int width, int height, ros::Publisher &pub_cloud, string cloud_frame_id, ros::Time t) {\n+    pcl::PointCloud<pcl::PointXYZRGB> point_cloud;\n+    point_cloud.width = width;\n+    point_cloud.height = height;\n+    int size = width*height;\n+    point_cloud.points.resize(size);\n+    int index4 = 0;\n+    float color;\n+    for (int i = 0; i < size; i++) {\n+        if(p_cloud[index4+2] < 0) { // Check if it's an unvalid point, the depth is lower than 0 \n+            index4 += 4;\n+            continue; \n+        }\n+        pcl::PointXYZRGB point;\n+        point.y = -p_cloud[index4++] * 0.001;\n+        point.z = -p_cloud[index4++] * 0.001;\n+        point.x = p_cloud[index4++] * 0.001;\n+        color = p_cloud[index4++];\n+        uint32_t color_uint = *(uint32_t*) & color; // Convert the color\n+        unsigned char* color_uchar = (unsigned char*) &color_uint;\n+        color_uint = ((uint32_t) color_uchar[0] << 16 | (uint32_t) color_uchar[1] << 8 | (uint32_t) color_uchar[2]);\n+        point.rgb = *reinterpret_cast<float*> (&color_uint);\n+        point_cloud.points[i] = point;\n+    }\n+    sensor_msgs::PointCloud2 output;\n+    pcl::toROSMsg(point_cloud, output); // Convert the point cloud to a ROS message\n+    output.header.frame_id = cloud_frame_id;// Set the header values of the ROS message\n+    output.header.stamp = t;\n+    pub_cloud.publish(output);","path":"src/zed_wrapper_node.cpp","position":119,"original_position":119,"commit_id":"3841ffb758fd7e7466f6dcdca10f74fff9a57d5d","original_commit_id":"3841ffb758fd7e7466f6dcdca10f74fff9a57d5d","user":{"login":"eric-wieser","id":425260,"avatar_url":"https://avatars.githubusercontent.com/u/425260?v=3","gravatar_id":"","url":"https://api.github.com/users/eric-wieser","html_url":"https://github.com/eric-wieser","followers_url":"https://api.github.com/users/eric-wieser/followers","following_url":"https://api.github.com/users/eric-wieser/following{/other_user}","gists_url":"https://api.github.com/users/eric-wieser/gists{/gist_id}","starred_url":"https://api.github.com/users/eric-wieser/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/eric-wieser/subscriptions","organizations_url":"https://api.github.com/users/eric-wieser/orgs","repos_url":"https://api.github.com/users/eric-wieser/repos","events_url":"https://api.github.com/users/eric-wieser/events{/privacy}","received_events_url":"https://api.github.com/users/eric-wieser/received_events","type":"User","site_admin":false},"body":"Wow, I'm blind. Apologies","created_at":"2016-02-29T16:58:06Z","updated_at":"2016-02-29T16:58:06Z","html_url":"https://github.com/stereolabs/zed-ros-wrapper/pull/19#discussion_r54440098","pull_request_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls/19","_links":{"self":{"href":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls/comments/54440098"},"html":{"href":"https://github.com/stereolabs/zed-ros-wrapper/pull/19#discussion_r54440098"},"pull_request":{"href":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls/19"}}},"pull_request":{"url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls/19","id":60855519,"html_url":"https://github.com/stereolabs/zed-ros-wrapper/pull/19","diff_url":"https://github.com/stereolabs/zed-ros-wrapper/pull/19.diff","patch_url":"https://github.com/stereolabs/zed-ros-wrapper/pull/19.patch","issue_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/issues/19","number":19,"state":"closed","locked":false,"title":"Point cloud integration","user":{"login":"nesnes","id":9192862,"avatar_url":"https://avatars.githubusercontent.com/u/9192862?v=3","gravatar_id":"","url":"https://api.github.com/users/nesnes","html_url":"https://github.com/nesnes","followers_url":"https://api.github.com/users/nesnes/followers","following_url":"https://api.github.com/users/nesnes/following{/other_user}","gists_url":"https://api.github.com/users/nesnes/gists{/gist_id}","starred_url":"https://api.github.com/users/nesnes/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/nesnes/subscriptions","organizations_url":"https://api.github.com/users/nesnes/orgs","repos_url":"https://api.github.com/users/nesnes/repos","events_url":"https://api.github.com/users/nesnes/events{/privacy}","received_events_url":"https://api.github.com/users/nesnes/received_events","type":"User","site_admin":false},"body":"Now the wrapper is able to provide the 3D point cloud, the depth map and the camera images at the same time. There is only one zed.launch now, the wrapper automatically adapts on subscribers change.","created_at":"2016-02-26T15:13:09Z","updated_at":"2016-02-29T16:58:06Z","closed_at":"2016-02-26T18:06:14Z","merged_at":"2016-02-26T18:06:14Z","merge_commit_sha":"1e37e21bc94dd1911f3531929b8e13b758a9772e","assignee":null,"milestone":null,"commits_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls/19/commits","review_comments_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls/19/comments","review_comment_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls/comments{/number}","comments_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/issues/19/comments","statuses_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/statuses/3841ffb758fd7e7466f6dcdca10f74fff9a57d5d","head":{"label":"nesnes:master","ref":"master","sha":"3841ffb758fd7e7466f6dcdca10f74fff9a57d5d","user":{"login":"nesnes","id":9192862,"avatar_url":"https://avatars.githubusercontent.com/u/9192862?v=3","gravatar_id":"","url":"https://api.github.com/users/nesnes","html_url":"https://github.com/nesnes","followers_url":"https://api.github.com/users/nesnes/followers","following_url":"https://api.github.com/users/nesnes/following{/other_user}","gists_url":"https://api.github.com/users/nesnes/gists{/gist_id}","starred_url":"https://api.github.com/users/nesnes/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/nesnes/subscriptions","organizations_url":"https://api.github.com/users/nesnes/orgs","repos_url":"https://api.github.com/users/nesnes/repos","events_url":"https://api.github.com/users/nesnes/events{/privacy}","received_events_url":"https://api.github.com/users/nesnes/received_events","type":"User","site_admin":false},"repo":{"id":52608172,"name":"zed-ros-wrapper","full_name":"nesnes/zed-ros-wrapper","owner":{"login":"nesnes","id":9192862,"avatar_url":"https://avatars.githubusercontent.com/u/9192862?v=3","gravatar_id":"","url":"https://api.github.com/users/nesnes","html_url":"https://github.com/nesnes","followers_url":"https://api.github.com/users/nesnes/followers","following_url":"https://api.github.com/users/nesnes/following{/other_user}","gists_url":"https://api.github.com/users/nesnes/gists{/gist_id}","starred_url":"https://api.github.com/users/nesnes/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/nesnes/subscriptions","organizations_url":"https://api.github.com/users/nesnes/orgs","repos_url":"https://api.github.com/users/nesnes/repos","events_url":"https://api.github.com/users/nesnes/events{/privacy}","received_events_url":"https://api.github.com/users/nesnes/received_events","type":"User","site_admin":false},"private":false,"html_url":"https://github.com/nesnes/zed-ros-wrapper","description":"ROS wrapper for the ZED SDK","fork":true,"url":"https://api.github.com/repos/nesnes/zed-ros-wrapper","forks_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/forks","keys_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/keys{/key_id}","collaborators_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/teams","hooks_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/hooks","issue_events_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/issues/events{/number}","events_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/events","assignees_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/assignees{/user}","branches_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/branches{/branch}","tags_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/tags","blobs_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/git/refs{/sha}","trees_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/git/trees{/sha}","statuses_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/statuses/{sha}","languages_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/languages","stargazers_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/stargazers","contributors_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/contributors","subscribers_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/subscribers","subscription_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/subscription","commits_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/commits{/sha}","git_commits_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/git/commits{/sha}","comments_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/comments{/number}","issue_comment_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/issues/comments{/number}","contents_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/contents/{+path}","compare_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/compare/{base}...{head}","merges_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/merges","archive_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/downloads","issues_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/issues{/number}","pulls_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/pulls{/number}","milestones_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/milestones{/number}","notifications_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/labels{/name}","releases_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/releases{/id}","deployments_url":"https://api.github.com/repos/nesnes/zed-ros-wrapper/deployments","created_at":"2016-02-26T14:03:23Z","updated_at":"2016-02-26T14:03:24Z","pushed_at":"2016-02-26T17:46:20Z","git_url":"git://github.com/nesnes/zed-ros-wrapper.git","ssh_url":"git@github.com:nesnes/zed-ros-wrapper.git","clone_url":"https://github.com/nesnes/zed-ros-wrapper.git","svn_url":"https://github.com/nesnes/zed-ros-wrapper","homepage":"","size":34,"stargazers_count":0,"watchers_count":0,"language":"C++","has_issues":false,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":0,"mirror_url":null,"open_issues_count":0,"forks":0,"open_issues":0,"watchers":0,"default_branch":"master"}},"base":{"label":"stereolabs:master","ref":"master","sha":"fd1188a126cf7cf5e8786583f6e8d9cea893944c","user":{"login":"stereolabs","id":14199582,"avatar_url":"https://avatars.githubusercontent.com/u/14199582?v=3","gravatar_id":"","url":"https://api.github.com/users/stereolabs","html_url":"https://github.com/stereolabs","followers_url":"https://api.github.com/users/stereolabs/followers","following_url":"https://api.github.com/users/stereolabs/following{/other_user}","gists_url":"https://api.github.com/users/stereolabs/gists{/gist_id}","starred_url":"https://api.github.com/users/stereolabs/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/stereolabs/subscriptions","organizations_url":"https://api.github.com/users/stereolabs/orgs","repos_url":"https://api.github.com/users/stereolabs/repos","events_url":"https://api.github.com/users/stereolabs/events{/privacy}","received_events_url":"https://api.github.com/users/stereolabs/received_events","type":"Organization","site_admin":false},"repo":{"id":42183658,"name":"zed-ros-wrapper","full_name":"stereolabs/zed-ros-wrapper","owner":{"login":"stereolabs","id":14199582,"avatar_url":"https://avatars.githubusercontent.com/u/14199582?v=3","gravatar_id":"","url":"https://api.github.com/users/stereolabs","html_url":"https://github.com/stereolabs","followers_url":"https://api.github.com/users/stereolabs/followers","following_url":"https://api.github.com/users/stereolabs/following{/other_user}","gists_url":"https://api.github.com/users/stereolabs/gists{/gist_id}","starred_url":"https://api.github.com/users/stereolabs/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/stereolabs/subscriptions","organizations_url":"https://api.github.com/users/stereolabs/orgs","repos_url":"https://api.github.com/users/stereolabs/repos","events_url":"https://api.github.com/users/stereolabs/events{/privacy}","received_events_url":"https://api.github.com/users/stereolabs/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/stereolabs/zed-ros-wrapper","description":"ROS wrapper for the ZED SDK","fork":false,"url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper","forks_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/forks","keys_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/keys{/key_id}","collaborators_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/teams","hooks_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/hooks","issue_events_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/issues/events{/number}","events_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/events","assignees_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/assignees{/user}","branches_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/branches{/branch}","tags_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/tags","blobs_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/git/refs{/sha}","trees_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/git/trees{/sha}","statuses_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/statuses/{sha}","languages_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/languages","stargazers_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/stargazers","contributors_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/contributors","subscribers_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/subscribers","subscription_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/subscription","commits_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/commits{/sha}","git_commits_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/git/commits{/sha}","comments_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/comments{/number}","issue_comment_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/issues/comments{/number}","contents_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/contents/{+path}","compare_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/compare/{base}...{head}","merges_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/merges","archive_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/downloads","issues_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/issues{/number}","pulls_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls{/number}","milestones_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/milestones{/number}","notifications_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/labels{/name}","releases_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/releases{/id}","deployments_url":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/deployments","created_at":"2015-09-09T14:29:32Z","updated_at":"2016-02-24T03:14:44Z","pushed_at":"2016-02-26T18:06:20Z","git_url":"git://github.com/stereolabs/zed-ros-wrapper.git","ssh_url":"git@github.com:stereolabs/zed-ros-wrapper.git","clone_url":"https://github.com/stereolabs/zed-ros-wrapper.git","svn_url":"https://github.com/stereolabs/zed-ros-wrapper","homepage":"","size":35,"stargazers_count":13,"watchers_count":13,"language":"C++","has_issues":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":11,"mirror_url":null,"open_issues_count":3,"forks":11,"open_issues":3,"watchers":13,"default_branch":"master"}},"_links":{"self":{"href":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls/19"},"html":{"href":"https://github.com/stereolabs/zed-ros-wrapper/pull/19"},"issue":{"href":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/issues/19"},"comments":{"href":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/issues/19/comments"},"review_comments":{"href":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls/19/comments"},"review_comment":{"href":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls/comments{/number}"},"commits":{"href":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/pulls/19/commits"},"statuses":{"href":"https://api.github.com/repos/stereolabs/zed-ros-wrapper/statuses/3841ffb758fd7e7466f6dcdca10f74fff9a57d5d"}}}}