diff --git a/jsk_pcl_ros_utils/CMakeLists.txt b/jsk_pcl_ros_utils/CMakeLists.txt index c214d87dbc..fe5a008790 100644 --- a/jsk_pcl_ros_utils/CMakeLists.txt +++ b/jsk_pcl_ros_utils/CMakeLists.txt @@ -270,6 +270,8 @@ jsk_pcl_util_nodelet(src/pointcloud_relative_from_pose_stamped_nodelet.cpp "jsk_pcl_utils/PointCloudRelativeFromPoseStamped" "pointcloud_relative_from_pose_stamped") jsk_pcl_util_nodelet(src/pointcloud_to_pcd_nodelet.cpp "jsk_pcl_utils/PointCloudToPCD" "pointcloud_to_pcd") +jsk_pcl_util_nodelet(src/marker_array_voxel_to_pointcloud_nodelet.cpp + "jsk_pcl_utils/MarkerArrayVoxelToPointCloud" "marker_array_voxel_to_pointcloud") add_library(jsk_pcl_ros_utils SHARED ${jsk_pcl_util_nodelet_sources}) add_dependencies(jsk_pcl_ros_utils ${PROJECT_NAME}_gencpp ${PROJECT_NAME}_gencfg) target_link_libraries(jsk_pcl_ros_utils diff --git a/jsk_pcl_ros_utils/include/jsk_pcl_ros_utils/marker_array_voxel_to_pointcloud.h b/jsk_pcl_ros_utils/include/jsk_pcl_ros_utils/marker_array_voxel_to_pointcloud.h new file mode 100644 index 0000000000..5a6a41f18b --- /dev/null +++ b/jsk_pcl_ros_utils/include/jsk_pcl_ros_utils/marker_array_voxel_to_pointcloud.h @@ -0,0 +1,63 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + + +#ifndef JSK_PCL_ROS_UTILS_MARKER_ARRAY_VOXEL_TO_POINTCLOUD_H_ +#define JSK_PCL_ROS_UTILS_MARKER_ARRAY_VOXEL_TO_POINTCLOUD_H_ + +#include +#include + +namespace jsk_pcl_ros_utils +{ + +class MarkerArrayVoxelToPointCloud: public jsk_topic_tools::DiagnosticNodelet +{ +public: + MarkerArrayVoxelToPointCloud(): DiagnosticNodelet("MarkerArrayVoxelToPointCloud") { } +protected: + virtual void onInit(); + virtual void subscribe(); + virtual void unsubscribe(); + virtual void convert(const visualization_msgs::MarkerArray::ConstPtr& marker_array_msg); + + ros::Subscriber sub_; + ros::Publisher pub_; +private: +}; + +} // namespace jsk_pcl_ros_utils + +#endif // JSK_PCL_ROS_UTILS_MARKER_ARRAY_VOXEL_TO_POINTCLOUD_H_ diff --git a/jsk_pcl_ros_utils/jsk_pcl_nodelets.xml b/jsk_pcl_ros_utils/jsk_pcl_nodelets.xml index 83028437cf..dd3d79fd83 100644 --- a/jsk_pcl_ros_utils/jsk_pcl_nodelets.xml +++ b/jsk_pcl_ros_utils/jsk_pcl_nodelets.xml @@ -212,4 +212,10 @@ + + + Convert voxel represented by visualization_msgs::MarkerArray to PointCloud. + + diff --git a/jsk_pcl_ros_utils/src/marker_array_voxel_to_pointcloud_nodelet.cpp b/jsk_pcl_ros_utils/src/marker_array_voxel_to_pointcloud_nodelet.cpp new file mode 100644 index 0000000000..4293ae64b1 --- /dev/null +++ b/jsk_pcl_ros_utils/src/marker_array_voxel_to_pointcloud_nodelet.cpp @@ -0,0 +1,91 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#define BOOST_PARAMETER_MAX_ARITY 7 + +#include +#include "jsk_pcl_ros_utils/marker_array_voxel_to_pointcloud.h" + +namespace jsk_pcl_ros_utils +{ + + void MarkerArrayVoxelToPointCloud::onInit() + { + DiagnosticNodelet::onInit(); + + pub_ = advertise(*pnh_, "output", 1); + onInitPostProcess(); + } + + void MarkerArrayVoxelToPointCloud::subscribe() + { + sub_ = pnh_->subscribe("input", 1, &MarkerArrayVoxelToPointCloud::convert, this); + } + + void MarkerArrayVoxelToPointCloud::unsubscribe() + { + sub_.shutdown(); + } + + void MarkerArrayVoxelToPointCloud::convert( + const visualization_msgs::MarkerArray::ConstPtr& marker_array_msg) + { + vital_checker_->poke(); + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + cloud->is_dense = true; + for (size_t i=0; imarkers.size(); i++) { + visualization_msgs::Marker marker = marker_array_msg->markers[i]; + for (size_t j=0; jpoints.push_back(pt); + } + } + + sensor_msgs::PointCloud2 ros_cloud; + pcl::toROSMsg(*cloud, ros_cloud); + ros_cloud.header = marker_array_msg->markers[0].header; + pub_.publish(ros_cloud); + } + +} // namespace jsk_pcl_ros_utils + +#include +PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::MarkerArrayVoxelToPointCloud, nodelet::Nodelet);