00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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_conversions/pcl_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
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);
00216 spinner.start();
00217
00218 FilterAttachedObjects cko;
00219 ros::waitForShutdown();
00220
00221 return 0;
00222 }