test.cpp
Go to the documentation of this file.00001 /* 00002 * test.cpp 00003 * 00004 * Created on: 16.03.2012 00005 * Author: josh 00006 */ 00007 00008 #include "ros/ros.h" 00009 #include <pcl/point_types.h> 00010 #include <pcl/point_cloud.h> 00011 #include <pcl/ros/conversions.h> 00012 00013 #include <sensor_msgs/PointCloud2.h> 00014 00015 #include <cob_3d_mapping_msgs/ShapeArray.h> 00016 #include <cob_3d_mapping_msgs/Shape.h> 00017 00018 #include <ros/time.h> 00019 00020 #include <boost/shared_ptr.hpp> 00021 00022 00023 int main(int argc, char **argv) { 00024 00025 ros::init(argc, argv, "talker"); 00026 ros::NodeHandle n; 00027 ros::Publisher pub = n.advertise<cob_3d_mapping_msgs::ShapeArray>("shapes_array",1); 00028 00029 ros::Rate loop_rate(1); 00030 00031 while(ros::ok()) { 00032 cob_3d_mapping_msgs::ShapeArray sa; 00033 sa.header.frame_id="/map"; 00034 00035 cob_3d_mapping_msgs::Shape s; 00036 00037 s.params.push_back(0); 00038 s.params.push_back(0); 00039 s.params.push_back(1); 00040 s.params.push_back(0); 00041 s.centroid.x = 1; 00042 s.centroid.y = 1; 00043 s.centroid.z = 1; 00044 s.header.frame_id="/map"; 00045 00046 s.color.r = 0; 00047 s.color.g = 0.71; 00048 s.color.b = 0; 00049 s.color.a = 1; 00050 00051 pcl::PointCloud<pcl::PointXYZ> pc; 00052 pcl::PointXYZ pt; 00053 00054 pt.x=0; 00055 pt.y=0; 00056 pt.z=0; 00057 pc.push_back(pt); 00058 00059 pt.x=1; 00060 pt.y=0; 00061 pt.z=0; 00062 pc.push_back(pt); 00063 00064 pt.x=1; 00065 pt.y=2; 00066 pt.z=0; 00067 pc.push_back(pt); 00068 00069 pt.x=0; 00070 pt.y=2; 00071 pt.z=0; 00072 pc.push_back(pt); 00073 00074 pt.x=0.5; 00075 pt.y=1; 00076 pt.z=0; 00077 pc.push_back(pt); 00078 00079 sensor_msgs::PointCloud2 pc2; 00080 pcl::toROSMsg(pc,pc2); 00081 s.points.push_back(pc2); 00082 s.holes.push_back(false); 00083 00084 sa.shapes.push_back(s); 00085 00086 s.params[0]=1; 00087 s.params[1]=0; 00088 sa.shapes.push_back(s); 00089 00090 s.params[0]=0; 00091 s.params[1]=1; 00092 sa.shapes.push_back(s); 00093 00094 pub.publish(sa); 00095 00096 ros::spinOnce(); 00097 loop_rate.sleep(); 00098 } 00099 00100 return 0; 00101 } 00102