diff --git a/CMakeLists.txt b/CMakeLists.txt index b0348e875b9cd57472090631d9b134a5fa64cf93..ffb02fcdf3e6aa24fc0368551d390f6d668a085b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,11 +27,10 @@ ENDIF (PCL_FOUND) # Add topic, service and action definition here # ******************************************************************** ## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) +add_message_files( + FILES + segmented_objects.msg +) ## Generate services in the 'srv' folder add_service_files( diff --git a/msg/segmented_objects.msg b/msg/segmented_objects.msg new file mode 100644 index 0000000000000000000000000000000000000000..474846f47bf0dd1b299f0a320f0f22a42fd86e4e --- /dev/null +++ b/msg/segmented_objects.msg @@ -0,0 +1 @@ +sensor_msgs/PointCloud2[] objects diff --git a/src/tos_supervoxels_alg_client_node.cpp b/src/tos_supervoxels_alg_client_node.cpp index 1ad657996b62776fc62b0e8abd1e4a0b8b0680b0..92e8b39d2b9dbf49711ffc10edec523ad5155dc3 100644 --- a/src/tos_supervoxels_alg_client_node.cpp +++ b/src/tos_supervoxels_alg_client_node.cpp @@ -11,8 +11,11 @@ // srv #include "iri_tos_supervoxels/object_segmentation.h" +// msg +#include "iri_tos_supervoxels/segmented_objects.h" + ros::ServiceClient client; -ros::Publisher pub; +ros::Publisher pub,pub_segmented_objs; std::string service_name; void callback(const sensor_msgs::PointCloud2 cloud_msg) @@ -27,6 +30,9 @@ void callback(const sensor_msgs::PointCloud2 cloud_msg) //creating new point cloud for debugging with random color for each segmented object sensor_msgs::PointCloud2 detected_objects_msg; pcl::PointCloud<pcl::PointXYZRGB> detected_objects_cloud; + + iri_tos_supervoxels::segmented_objects segmented_objects_msg; + for (int i = 0; i < srv.response.objects.size(); ++i) { // convert it to pcl @@ -50,6 +56,8 @@ void callback(const sensor_msgs::PointCloud2 cloud_msg) temp_point.b = b*255; detected_objects_cloud.points.push_back(temp_point); } + + segmented_objects_msg.objects.push_back(srv.response.objects[i]); } detected_objects_cloud.width = detected_objects_cloud.points.size(); @@ -65,6 +73,8 @@ void callback(const sensor_msgs::PointCloud2 cloud_msg) detected_objects_msg.header.stamp = ros::Time::now(); //std::cout << "frame of point cloud: " << detected_objects_msg.header.frame_id << "\n"; pub.publish(detected_objects_msg); + + pub_segmented_objs.publish(segmented_objects_msg); } else ROS_ERROR("Failed to call service %s",service_name.c_str()); @@ -85,7 +95,8 @@ int main(int argc, char **argv) ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>(input_topic, 1, callback); - pub = n.advertise<sensor_msgs::PointCloud2>("/detected_objects/points", 1); + pub = n.advertise<sensor_msgs::PointCloud2>("/segmented_objects/points", 1); + pub_segmented_objs = n.advertise<iri_tos_supervoxels::segmented_objects>("/segmented_objects",1); client = n.serviceClient<iri_tos_supervoxels::object_segmentation>(service_name);