37 #include <geometry_msgs/PolygonStamped.h> 38 #include <Eigen/StdVector> 39 #include <pcl/ModelCoefficients.h> 40 #include <pcl/filters/project_inliers.h> 41 #include <pcl/filters/extract_indices.h> 42 #include <pcl/sample_consensus/method_types.h> 43 #include <pcl/sample_consensus/model_types.h> 44 #include <pcl/segmentation/sac_segmentation.h> 45 #include <pcl/common/distances.h> 46 #include <pcl/features/normal_3d.h> 47 #include <pcl/surface/concave_hull.h> 48 #include <pcl/common/centroid.h> 49 #include <visualization_msgs/Marker.h> 54 #include <visualization_msgs/MarkerArray.h> 61 DiagnosticNodelet::onInit();
65 *
pnh_,
"output/plane_aligned", 1);
67 *
pnh_,
"output/convex_aligned", 1);
69 *
pnh_,
"output/convex_aligned_pose_array", 1);
83 = boost::make_shared<message_filters::Synchronizer<SyncPolygonPolicy> >(100);
92 "input/convex_align_polygon", 1,
108 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
109 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg)
117 const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
125 std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
127 pose_msg->header.stamp,
129 Eigen::Affine3d pose_eigend;
130 Eigen::Affine3f pose_eigen;
133 Eigen::Vector3f pose_point(pose_eigen.translation());
134 double min_distance = DBL_MAX;
136 for (
size_t i = 0; i < convexes.size(); i++) {
138 double d = convex->distanceToPoint(pose_point);
139 if (d < min_distance) {
146 geometry_msgs::PoseStamped aligned_pose =
alignPose(pose_eigen, min_convex);
147 aligned_pose.header = pose_msg->header;
156 jsk_recognition_msgs::SnapFootstep::Request&
req,
157 jsk_recognition_msgs::SnapFootstep::Response&
res)
160 jsk_footstep_msgs::FootstepArray input_footsteps = req.input;
161 res.output.header = input_footsteps.header;
162 std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
164 input_footsteps.header.stamp,
166 for (
size_t i = 0; i < input_footsteps.footsteps.size(); i++) {
167 jsk_footstep_msgs::Footstep footstep = input_footsteps.footsteps[i];
168 Eigen::Affine3d pose_eigend;
169 Eigen::Affine3f pose_eigen;
171 Eigen::Vector3f pose_point(pose_eigen.translation());
173 jsk_footstep_msgs::Footstep aligned_footstep;
174 if (min_index != -1) {
176 geometry_msgs::PoseStamped aligned_pose =
alignPose(pose_eigen, min_convex);
177 aligned_footstep.pose = aligned_pose.pose;
180 aligned_footstep.pose = footstep.pose;
183 aligned_footstep.leg = footstep.leg;
184 aligned_footstep.dimensions = footstep.dimensions;
185 aligned_footstep.duration = footstep.duration;
186 aligned_footstep.footstep_group = footstep.footstep_group;
187 res.output.footsteps.push_back(aligned_footstep);
193 const geometry_msgs::PolygonStamped::ConstPtr& poly_msg)
196 geometry_msgs::PoseArray pose_array;
197 pose_array.header = poly_msg->header;
202 std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
204 poly_msg->header.stamp,
206 for (
size_t i = 0; i < poly_msg->polygon.points.size(); i++) {
207 geometry_msgs::Point32
p = poly_msg->polygon.points[i];
208 Eigen::Vector3f pose_point(p.x, p.y, p.z);
210 if (min_index == -1) {
216 Eigen::Affine3f pose_eigen = Eigen::Affine3f::Identity();
217 pose_eigen.translate(pose_point);
218 geometry_msgs::PoseStamped aligned_pose =
alignPose(pose_eigen, min_convex);
219 aligned_pose.header = poly_msg->header;
220 pose_array.poses.push_back(aligned_pose.pose);
227 const Eigen::Vector3f& pose_point,
228 const std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& convexes)
231 double min_distance = DBL_MAX;
233 for (
size_t i = 0; i < convexes.size(); i++) {
235 if (convex->isProjectableInside(pose_point)) {
236 double d = convex->distanceToPoint(pose_point);
237 if (d < min_distance) {
248 const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
256 std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
258 pose_msg->header.stamp,
260 Eigen::Affine3d pose_eigend;
261 Eigen::Affine3f pose_eigen;
264 Eigen::Vector3f pose_point(pose_eigen.translation());
266 if (min_index != -1) {
268 geometry_msgs::PoseStamped aligned_pose =
alignPose(pose_eigen, min_convex);
269 aligned_pose.header = pose_msg->header;
280 Eigen::Affine3f aligned_pose(pose);
281 Eigen::Vector3f original_point(pose.translation());
282 Eigen::Vector3f projected_point;
283 convex->project(original_point, projected_point);
285 Eigen::Vector3f normal = convex->getNormal();
286 Eigen::Vector3f old_normal;
287 old_normal[0] = pose(0, 2);
288 old_normal[1] = pose(1, 2);
289 old_normal[2] = pose(2, 2);
290 Eigen::Quaternionf
rot;
291 if (normal.dot(old_normal) < 0) {
294 rot.setFromTwoVectors(old_normal, normal);
295 aligned_pose = aligned_pose * rot;
296 aligned_pose.translation() = projected_point;
297 Eigen::Affine3d aligned_posed;
299 geometry_msgs::PoseStamped ret;
306 jsk_recognition_msgs::PolygonArray::ConstPtr polygons)
308 std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>
result;
311 for (
size_t i = 0; i < polygons->polygons.size(); i++) {
312 geometry_msgs::PolygonStamped
polygon = polygons->polygons[i];
317 polygon.header.frame_id, frame_id, stamp,
ros::Duration(5.0));
318 for (
size_t j = 0; j < polygon.polygon.points.size(); j++) {
320 p[0] = polygon.polygon.points[j].x;
321 p[1] = polygon.polygon.points[j].y;
322 p[2] = polygon.polygon.points[j].z;
324 Eigen::Affine3d eigen_transform;
326 Eigen::Vector4d transformed_pointd = eigen_transform.inverse() * p;
327 Eigen::Vector3f transformed_point;
328 transformed_point[0] = transformed_pointd[0];
329 transformed_point[1] = transformed_pointd[1];
330 transformed_point[2] = transformed_pointd[2];
331 vertices.push_back(transformed_point);
333 std::reverse(vertices.begin(), vertices.end());
335 result.push_back(convex);
ros::ServiceServer align_footstep_srv_
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_polygons_
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
virtual void unsubscribe()
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
tf::StampedTransform lookupTransformWithDuration(tf::TransformListener *listener, const std::string &to_frame, const std::string &from_frame, const ros::Time &stamp, ros::Duration duration)
virtual geometry_msgs::PoseStamped alignPose(Eigen::Affine3f &pose, jsk_recognition_utils::ConvexPolygon::Ptr convex)
virtual void polygonCallback(const jsk_recognition_msgs::PolygonArray::ConstPtr &polygon_msg, const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &coefficients_msg)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::SnapIt, nodelet::Nodelet)
tf::TransformListener * tf_listener_
virtual void convexAlignPolygonCallback(const geometry_msgs::PolygonStamped::ConstPtr &poly_msg)
jsk_recognition_msgs::PolygonArray::ConstPtr polygons_
ros::Publisher convex_aligned_pose_array_pub_
virtual void polygonAlignCallback(const geometry_msgs::PoseStamped::ConstPtr &pose_msg)
void convertEigenAffine3(const Eigen::Affine3d &from, Eigen::Affine3f &to)
virtual bool footstepAlignServiceCallback(jsk_recognition_msgs::SnapFootstep::Request &req, jsk_recognition_msgs::SnapFootstep::Response &res)
ros::Subscriber polygon_align_sub_
virtual int findNearestConvex(const Eigen::Vector3f &pose_point, const std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > &convexes)
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
ros::Publisher polygon_aligned_pub_
ros::Subscriber convex_align_sub_
ros::Subscriber convex_align_polygon_sub_
message_filters::Subscriber< jsk_recognition_msgs::ModelCoefficientsArray > sub_coefficients_
static tf::TransformListener * getInstance()
virtual std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > createConvexes(const std::string &frame_id, const ros::Time &stamp, jsk_recognition_msgs::PolygonArray::ConstPtr polygons)
virtual void convexAlignCallback(const geometry_msgs::PoseStamped::ConstPtr &pose_msg)
ros::Publisher convex_aligned_pub_
boost::shared_ptr< message_filters::Synchronizer< SyncPolygonPolicy > > sync_polygon_