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