Go to the documentation of this file.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
00035
00036 #include <ros/ros.h>
00037
00038 #include <tf/transform_listener.h>
00039
00040 #include "tabletop_collision_map_processing/collision_map_interface.h"
00041 #include "tabletop_collision_map_processing/TabletopCollisionMapProcessing.h"
00042
00043 namespace tabletop_collision_map_processing {
00044
00045 static const std::string COLLISION_SERVICE_NAME = "tabletop_collision_map_processing";
00046
00047 class CollisionMapProcessor
00048 {
00049 private:
00050 ros::NodeHandle priv_nh_;
00051 ros::NodeHandle root_nh_;
00052 CollisionMapInterface collision_map_;
00053 ros::ServiceServer processing_srv_;
00054 tf::TransformListener listener_;
00055
00056 bool serviceCallback(TabletopCollisionMapProcessing::Request &request,
00057 TabletopCollisionMapProcessing::Response &response)
00058 {
00059
00060 try
00061 {
00062
00063 if (request.reset_collision_models) collision_map_.resetCollisionModels();
00064 if (request.reset_attached_models) collision_map_.resetAttachedModels();
00065 if (request.reset_static_map) collision_map_.resetStaticMap();
00066
00067
00068
00069
00070
00071 tabletop_object_detector::Table table = request.detection_result.table;
00072 if (table.x_min!=0 || table.x_max!=0 || table.y_min!=0 || table.y_max!=0)
00073 {
00074 collision_map_.processCollisionGeometryForTable(table, "table");
00075 response.collision_support_surface_name = "table";
00076 }
00077
00078
00079
00080 for (size_t c=0; c<request.detection_result.clusters.size(); c++)
00081 {
00082 object_manipulation_msgs::GraspableObject object;
00083
00084
00085 object.cluster = request.detection_result.clusters[c];
00086
00087 if (!request.desired_frame.empty())
00088 {
00089
00090 object.cluster.header.stamp = ros::Time(0);
00091 try
00092 {
00093 listener_.transformPointCloud(request.desired_frame, object.cluster, object.cluster);
00094 }
00095 catch (tf::TransformException ex)
00096 {
00097 ROS_ERROR("Failed to transform cluster to %s frame; exception: %s",
00098 request.desired_frame.c_str(), ex.what());
00099 return false;
00100 }
00101
00102 object.reference_frame_id = request.desired_frame;
00103 }
00104 else
00105 {
00106
00107 object.reference_frame_id = object.cluster.header.frame_id;
00108 }
00109
00110 object_manipulation_msgs::ClusterBoundingBox bbox;
00111 collision_map_.getClusterBoundingBox(object.cluster, bbox.pose_stamped, bbox.dimensions);
00112
00113 if ( !collision_map_.extendBoundingBoxZToTable(request.detection_result.table,
00114 bbox.pose_stamped, bbox.dimensions) )
00115 {
00116 ROS_WARN("Failed to extend bbox to table; using original dimensions");
00117 }
00118
00119
00120 std::string collision_name;
00121 collision_map_.processCollisionGeometryForBoundingBox(bbox, collision_name);
00122
00123 response.collision_object_names.push_back(collision_name);
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162 response.graspable_objects.push_back(object);
00163 }
00164
00165 if (request.take_static_collision_map)
00166 {
00167
00168 collision_map_.takeStaticMap();
00169
00170 }
00171 return true;
00172 }
00173 catch (CollisionMapException &ex)
00174 {
00175 ROS_ERROR("Collision map processing error; exception: %s", ex.what());
00176 return false;
00177 }
00178 }
00179
00180 public:
00181 CollisionMapProcessor() : priv_nh_("~"), root_nh_("")
00182 {
00183
00184 collision_map_.connectionsEstablished(ros::Duration(-1.0));
00185
00186 processing_srv_ = priv_nh_.advertiseService(COLLISION_SERVICE_NAME,
00187 &CollisionMapProcessor::serviceCallback, this);
00188 }
00189
00190 ~CollisionMapProcessor() {}
00191 };
00192
00193 }
00194
00195 int main(int argc, char **argv)
00196 {
00197 ros::init(argc, argv, "tabletop_collision_map_processing_node");
00198 tabletop_collision_map_processing::CollisionMapProcessor node;
00199
00200 ros::spin();
00201 return 0;
00202 }