20150624 homework

20150422/CMakeLists.txt

@@ -7,6 +7,9 @@ find_package(catkin REQUIRED COMPONENTS
   turtlesim
   cv_camera
   cv_bridge
+  usb_cam
+  pcl_ros
+  sensor_msgs
   roseus)
 
 find_package(OpenCV REQUIRED)
@@ -17,3 +20,14 @@ catkin_package(
 #  CATKIN_DEPENDS other_catkin_pkg
 #  DEPENDS system_lib
 )
+
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+ add_executable(pub_pseudo_organized_image_points2 src/pub_pseudo_organized_image_points2.cpp)
+
+target_link_libraries(pub_pseudo_organized_image_points2
+  ${catkin_LIBRARIES}
+)

20150422/README.md

@@ -136,3 +136,13 @@ roslaunch homework20150422 face_detect.launch
       - original camera image
       - processed image
     - add `README` which describes your node
+
+hoge.launch
+
+input sensor_msgs::image
+
+output sensor_msgs::PointCloud2
+
+take width and height of image, and publish pointcloud2 which depth is directed.
+
+

20150422/launch/hoge.launch

@@ -0,0 +1,25 @@
+<launch>
+  <arg name="camera_image" default="/image" />
+  <arg name="camera_info" default="/info" />
+  <arg name="camera_points2" default="/points2" />
+  <arg name="launch_view" default="false" />
+
+  <node pkg="usb_cam" type="usb_cam_node" name="camera">
+    <!-- rosrun usb_cam usb_cam_node _video_device:="/dev/video1" -->
+    <!-- <remap from="~video_device" to="/dev/video0" /> -->
+  </node>
+  <node pkg="mycamera" type="pub_pseudo_organized_image_points2" name="pub_pseudo_points2" output="screen">
+    <remap from="image" to="/camera/image_raw" />
+    <remap from="output" to="pub_points/output" />
+  </node>
+  <node pkg="topic_tools" type="relay" name="camera_image"
+        args="/camera/image_raw $(arg camera_image)" />
+  <node pkg="topic_tools" type="relay" name="camera_info"
+        args="/camera/camera_info $(arg camera_info)" />
+  <node pkg="topic_tools" type="relay" name="camera_points2"
+        args="pub_points/output $(arg camera_points2)" />
+
+  <node pkg="image_view" type="image_view" name="image_view" if="$(arg launch_view)">
+    <remap from="image" to="$(arg camera_image)" />
+  </node>
+</launch>

20150422/src/pub_pseudo_organized_image_points2.cpp

@@ -0,0 +1,52 @@
+#include <iostream>
+#include "ros/ros.h"
+#include "sensor_msgs/PointCloud2.h"
+#include "sensor_msgs/Image.h"
+#include "pcl/point_types.h"
+#include "pcl/point_cloud.h"
+#include "std_msgs/Header.h"
+#include "pcl_conversions/pcl_conversions.h"
+
+ros::Publisher pub_;
+void image_callback(const sensor_msgs::Image::ConstPtr& msg);
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "pub_pointcloud2");
+  ros::NodeHandle n;
+  ros::Subscriber sub_=n.subscribe("image", 1000, image_callback);
+  pub_=n.advertise<sensor_msgs::PointCloud2> ("output", 1000);
+  std::cout<<"a"<<std::endl;
+  ros::Rate r(1);
+  ros::spin();
+  return 0;
+}
+
+void image_callback(const sensor_msgs::Image::ConstPtr& msg)
+{
+  pcl::PointCloud<pcl::PointXYZ>::Ptr
+    cloud (new pcl::PointCloud<pcl::PointXYZ> );
+  cloud->width=msg->width;
+  cloud->height=msg->height;
+  cloud->points.resize(msg->width* msg->height);
+  cloud->is_dense=false;
+  //  ROS_INFO("msg frame:%s", msg->header.frame_id.c_str());  //frame: head_camera
+
+  //  ROS_INFO("msg stamp.sec:%d stamp.nsec:%d", msg->header.stamp.sec, msg->header.stamp.nsec);
+  pcl_conversions::toPCL(msg->header, cloud->header);
+  //  ROS_INFO("msg->height:%d msg->width:%d", msg->height, msg->width);
+  for (size_t i=0; i< msg->height; i++){
+    for (size_t j=0; j< msg->width; j++){
+      pcl::PointXYZ tmppnt;
+      tmppnt.x=j;
+      tmppnt.y=i;
+      tmppnt.z=100*(i+j);
+      cloud->points[i* msg->width + j]=tmppnt;
+    }
+  }
+  sensor_msgs::PointCloud2 ros_cloud;
+  pcl::toROSMsg(*cloud, ros_cloud);
+  ros_cloud.header=msg->header;
+  pub_.publish(ros_cloud);
+}
+