diff --git a/20150422/CMakeLists.txt b/20150422/CMakeLists.txt
index e690005..240f2e2 100644
--- a/20150422/CMakeLists.txt
+++ b/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}
+)
diff --git a/20150422/README.md b/20150422/README.md
index 30f3a37..93a87f9 100644
--- a/20150422/README.md
+++ b/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.
+
+
diff --git a/20150422/launch/hoge.launch b/20150422/launch/hoge.launch
new file mode 100644
index 0000000..3c2bf3c
--- /dev/null
+++ b/20150422/launch/hoge.launch
@@ -0,0 +1,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/20150422/src/pub_pseudo_organized_image_points2.cpp b/20150422/src/pub_pseudo_organized_image_points2.cpp
new file mode 100644
index 0000000..c94f5b0
--- /dev/null
+++ b/20150422/src/pub_pseudo_organized_image_points2.cpp
@@ -0,0 +1,52 @@
+#include
+#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 ("output", 1000);
+ std::cout<<"a"<::Ptr
+ cloud (new pcl::PointCloud );
+ 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);
+}
+
diff --git a/20150624/gazebo-48156606.png b/20150624/gazebo-48156606.png
new file mode 100644
index 0000000..0d7101a
Binary files /dev/null and b/20150624/gazebo-48156606.png differ
diff --git a/20150624/rviz-48156606.png b/20150624/rviz-48156606.png
new file mode 100644
index 0000000..3f0e06d
Binary files /dev/null and b/20150624/rviz-48156606.png differ