semantic_extraction_test_server.cpp
Go to the documentation of this file.00001 #include "ros/ros.h" 00002 00003 #include <cob_3d_mapping_msgs/GetObjectsOfClass.h> 00004 #include <sensor_msgs/PointCloud2.h> 00005 #include <pcl/point_cloud.h> 00006 #include <pcl/point_types.h> 00007 #include <pcl/ros/conversions.h> 00008 00009 00010 bool serviceCb(cob_3d_mapping_msgs::GetObjectsOfClass::Request &req, 00011 cob_3d_mapping_msgs::GetObjectsOfClass::Response &res ) 00012 { 00013 if(req.class_id.data != 1) 00014 { 00015 ROS_WARN("Object class not supported"); 00016 return false; 00017 } 00018 cob_3d_mapping_msgs::Shape table1; 00019 table1.type = cob_3d_mapping_msgs::Shape::POLYGON; 00020 table1.params.push_back(0); 00021 table1.params.push_back(0); 00022 table1.params.push_back(1); 00023 table1.params.push_back(0.8); 00024 table1.params.push_back(0); 00025 table1.params.push_back(0); 00026 table1.params.push_back(0); 00027 pcl::PointCloud<pcl::PointXYZ> cloud; 00028 pcl::PointXYZ p; 00029 p.x = -0.5; 00030 p.y = -0.5; 00031 p.z = 0.8; 00032 cloud.points.push_back(p); 00033 p.x = 0.5; 00034 cloud.points.push_back(p); 00035 p.y = 0.5; 00036 cloud.points.push_back(p); 00037 p.x = -0.5; 00038 cloud.points.push_back(p); 00039 sensor_msgs::PointCloud2 cloud_msg; 00040 pcl::toROSMsg(cloud, cloud_msg); 00041 table1.points.push_back(cloud_msg); 00042 00043 cob_3d_mapping_msgs::Shape table2; 00044 table2.type = cob_3d_mapping_msgs::Shape::POLYGON; 00045 table2.params.push_back(0); 00046 table2.params.push_back(0); 00047 table2.params.push_back(1); 00048 table2.params.push_back(0.8); 00049 table2.params.push_back(5); 00050 table2.params.push_back(5); 00051 table2.params.push_back(0); 00052 p.x = 4.5; 00053 p.y = 4.5; 00054 p.z = 0.8; 00055 cloud.points.push_back(p); 00056 p.x = 5.5; 00057 cloud.points.push_back(p); 00058 p.y = 5.5; 00059 cloud.points.push_back(p); 00060 p.x = 4.5; 00061 cloud.points.push_back(p); 00062 pcl::toROSMsg(cloud, cloud_msg); 00063 table2.points.push_back(cloud_msg); 00064 00065 res.objects.shapes.push_back(table1); 00066 res.objects.shapes.push_back(table2); 00067 00068 ROS_INFO("sending back response"); 00069 return true; 00070 } 00071 00072 int main(int argc, char **argv) 00073 { 00074 ros::init(argc, argv, "get_objects_server"); 00075 ros::NodeHandle n; 00076 00077 ros::ServiceServer service = n.advertiseService("get_objects_of_class", serviceCb); 00078 ROS_INFO("Ready to return objects."); 00079 ros::spin(); 00080 00081 return 0; 00082 } 00083