36 #ifndef JSK_PCL_ROS_SNAPIT_H_ 37 #define JSK_PCL_ROS_SNAPIT_H_ 41 #include <pcl/point_types.h> 43 #include <jsk_recognition_msgs/PolygonArray.h> 44 #include <geometry_msgs/PolygonStamped.h> 45 #include <geometry_msgs/PoseArray.h> 46 #include <jsk_recognition_msgs/ModelCoefficientsArray.h> 47 #include "jsk_recognition_msgs/CallSnapIt.h" 54 #include <jsk_recognition_msgs/SnapFootstep.h> 62 jsk_recognition_msgs::PolygonArray,
74 const jsk_recognition_msgs::PolygonArray::ConstPtr&
polygon_msg,
75 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg);
77 const geometry_msgs::PoseStamped::ConstPtr& pose_msg);
79 const geometry_msgs::PoseStamped::ConstPtr& pose_msg);
81 const geometry_msgs::PolygonStamped::ConstPtr& poly_msg);
82 virtual std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>
createConvexes(
84 jsk_recognition_msgs::PolygonArray::ConstPtr
polygons);
86 const Eigen::Vector3f& pose_point,
87 const std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& convexes);
88 virtual geometry_msgs::PoseStamped
alignPose(
91 jsk_recognition_msgs::SnapFootstep::Request&
req,
92 jsk_recognition_msgs::SnapFootstep::Response&
res);
message_filters::sync_policies::ExactTime< jsk_recognition_msgs::PolygonArray, jsk_recognition_msgs::ModelCoefficientsArray > SyncPolygonPolicy
ros::ServiceServer align_footstep_srv_
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_polygons_
virtual void unsubscribe()
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)
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)
jsk_recognition_msgs::PolygonArray::ConstPtr polygon_msg
virtual bool footstepAlignServiceCallback(jsk_recognition_msgs::SnapFootstep::Request &req, jsk_recognition_msgs::SnapFootstep::Response &res)
ros::Publisher convex_aligned_pose_array_marker_pub_
ros::Subscriber polygon_align_sub_
virtual int findNearestConvex(const Eigen::Vector3f &pose_point, const std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > &convexes)
boost::mutex mutex
global mutex.
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_
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_