$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00044 #include <ros/ros.h> 00045 #include <planning_environment/models/collision_models.h> 00046 #include <planning_environment/util/construct_object.h> 00047 #include <planning_environment/monitors/monitor_utils.h> 00048 00049 #include <tf/message_filter.h> 00050 #include <message_filters/subscriber.h> 00051 #include <sensor_msgs/PointCloud2.h> 00052 #include <geometry_msgs/PoseStamped.h> 00053 #include <arm_navigation_msgs/AttachedCollisionObject.h> 00054 #include <robot_self_filter/self_mask.h> 00055 #include <pcl/ros/conversions.h> 00056 #include <pcl_ros/transforms.h> 00057 00058 class FilterAttachedObjects 00059 { 00060 public: 00061 00062 FilterAttachedObjects(void): priv_handle_("~") 00063 { 00064 cm_ = new planning_environment::CollisionModels("robot_description"); 00065 priv_handle_.param<std::string>("sensor_frame", sensor_frame_, std::string()); 00066 00067 cloud_publisher_ = root_handle_.advertise<sensor_msgs::PointCloud2>("cloud_out", 1); 00068 cloud_publisher_shadow_ = root_handle_.advertise<sensor_msgs::PointCloud2>("cloud_out_shadow", 1); 00069 attached_collision_object_subscriber_ = new message_filters::Subscriber<arm_navigation_msgs::AttachedCollisionObject>(root_handle_, "attached_collision_object", 1024); 00070 attached_collision_object_subscriber_->registerCallback(boost::bind(&FilterAttachedObjects::attachedObjectCallback, this, _1)); 00071 00072 collision_object_subscriber_ = new message_filters::Subscriber<arm_navigation_msgs::CollisionObject>(root_handle_, "collision_object", 1024); 00073 collision_object_subscriber_->registerCallback(boost::bind(&FilterAttachedObjects::objectCallback, this, _1)); 00074 00075 cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(root_handle_, "cloud_in", 1); 00076 cloud_filter_ = new tf::MessageFilter<sensor_msgs::PointCloud2>(*cloud_subscriber_, tf_, cm_->getWorldFrameId(), 1); 00077 cloud_filter_->registerCallback(boost::bind(&FilterAttachedObjects::cloudCallback, this, _1)); 00078 00079 attached_color_.a = 0.5; 00080 attached_color_.r = 0.6; 00081 attached_color_.g = 0.4; 00082 attached_color_.b = 0.3; 00083 00084 vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("filter_attached", 128); 00085 vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>(std::string("filter_attached")+"_array", 128); 00086 00087 } 00088 00089 ~FilterAttachedObjects(void) 00090 { 00091 delete cloud_filter_; 00092 delete cloud_subscriber_; 00093 delete attached_collision_object_subscriber_; 00094 delete collision_object_subscriber_; 00095 delete cm_; 00096 } 00097 00098 void cloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud) 00099 { 00100 ROS_DEBUG("Got pointcloud that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec()); 00101 00102 { 00103 planning_models::KinematicState state(cm_->getKinematicModel()); 00104 state.setKinematicStateToDefault(); 00105 00106 visualization_msgs::MarkerArray arr; 00107 planning_environment::updateAttachedObjectBodyPoses(cm_, 00108 state, 00109 tf_); 00110 00111 cm_->getAttachedCollisionObjectMarkers(state, 00112 arr, 00113 "filter_attached", 00114 attached_color_, 00115 ros::Duration(.2)); 00116 00117 std_msgs::ColorRGBA static_color; 00118 static_color.a = 0.5; 00119 static_color.r = 0.0; 00120 static_color.g = 1.0; 00121 static_color.b = 0.3; 00122 00123 cm_->getStaticCollisionObjectMarkers(arr, 00124 "filter_attached", 00125 static_color, 00126 ros::Duration(.2)); 00127 00128 vis_marker_array_publisher_.publish(arr); 00129 } 00130 00131 pcl::PointCloud<pcl::PointXYZ> pcl_cloud; 00132 pcl::fromROSMsg(*cloud, pcl_cloud); 00133 00134 std::vector<int> mask; 00135 00136 if(planning_environment::computeAttachedObjectPointCloudMask(pcl_cloud, 00137 sensor_frame_, 00138 cm_, 00139 tf_, 00140 mask)) { 00141 // publish new cloud 00142 const unsigned int np = pcl_cloud.size(); 00143 00144 pcl::PointCloud<pcl::PointXYZ> inside_masked_cloud; 00145 pcl::PointCloud<pcl::PointXYZ> shadow_cloud; 00146 00147 inside_masked_cloud.header = pcl_cloud.header; 00148 shadow_cloud.header = pcl_cloud.header; 00149 00150 inside_masked_cloud.points.reserve(np); 00151 shadow_cloud.points.reserve(np); 00152 00153 for (unsigned int i = 0; i < np; ++i) { 00154 if(mask[i] != robot_self_filter::INSIDE) { 00155 inside_masked_cloud.points.push_back(pcl_cloud.points[i]); 00156 } 00157 if(mask[i] == robot_self_filter::SHADOW) { 00158 shadow_cloud.points.push_back(pcl_cloud.points[i]); 00159 } 00160 } 00161 sensor_msgs::PointCloud2 out; 00162 pcl::toROSMsg(inside_masked_cloud, out); 00163 cloud_publisher_.publish(out); 00164 00165 sensor_msgs::PointCloud2 out_shadow; 00166 pcl::toROSMsg(shadow_cloud, out_shadow); 00167 cloud_publisher_shadow_.publish(out_shadow); 00168 } else { 00169 sensor_msgs::PointCloud2 out; 00170 pcl::toROSMsg(pcl_cloud, out); 00171 cloud_publisher_.publish(out); 00172 00173 pcl::PointCloud<pcl::PointXYZ> empty_cloud; 00174 empty_cloud.header = pcl_cloud.header; 00175 sensor_msgs::PointCloud2 out_shadow; 00176 pcl::toROSMsg(empty_cloud, out_shadow); 00177 cloud_publisher_shadow_.publish(out_shadow); 00178 } 00179 } 00180 00181 void objectCallback(const arm_navigation_msgs::CollisionObjectConstPtr& object) { 00182 planning_environment::processCollisionObjectMsg(object, tf_, cm_); 00183 } 00184 00185 void attachedObjectCallback(const arm_navigation_msgs::AttachedCollisionObjectConstPtr& attached_object) { 00186 planning_environment::processAttachedCollisionObjectMsg(attached_object, tf_, cm_); 00187 } 00188 00189 ros::NodeHandle priv_handle_; 00190 ros::NodeHandle root_handle_; 00191 tf::TransformListener tf_; 00192 planning_environment::CollisionModels *cm_; 00193 00194 message_filters::Subscriber<sensor_msgs::PointCloud2> *cloud_subscriber_; 00195 tf::MessageFilter<sensor_msgs::PointCloud2> *cloud_filter_; 00196 00197 message_filters::Subscriber<arm_navigation_msgs::AttachedCollisionObject> *attached_collision_object_subscriber_; 00198 message_filters::Subscriber<arm_navigation_msgs::CollisionObject> *collision_object_subscriber_; 00199 00200 ros::Publisher cloud_publisher_; 00201 ros::Publisher cloud_publisher_shadow_; 00202 00203 std::string sensor_frame_; 00204 00205 ros::Publisher vis_marker_publisher_; 00206 ros::Publisher vis_marker_array_publisher_; 00207 std_msgs::ColorRGBA attached_color_; 00208 }; 00209 00210 00211 int main(int argc, char **argv) 00212 { 00213 ros::init(argc, argv, "clear_known_objects"); 00214 00215 ros::AsyncSpinner spinner(1); // Use 2 threads 00216 spinner.start(); 00217 00218 FilterAttachedObjects cko; 00219 ros::waitForShutdown(); 00220 00221 return 0; 00222 }