bb_overlap.h
Go to the documentation of this file.00001 /****************************************************************************** 00002 * \file 00003 * \brief 00004 * \author Zdenek Materna (imaterna@fit.vutbr.cz) 00005 * 00006 * $Id:$ 00007 * 00008 * Copyright (C) Brno University of Technology 00009 * 00010 * This file is part of software developed by dcgm-robotics@FIT group. 00011 * 00012 * Author: Zdenek Materna (imaterna@fit.vutbr.cz) 00013 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00014 * Date: dd/mm/2012 00015 * 00016 * This file is free software: you can redistribute it and/or modify 00017 * it under the terms of the GNU Lesser General Public License as published by 00018 * the Free Software Foundation, either version 3 of the License, or 00019 * (at your option) any later version. 00020 * 00021 * This file is distributed in the hope that it will be useful, 00022 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00023 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00024 * GNU Lesser General Public License for more details. 00025 * 00026 * You should have received a copy of the GNU Lesser General Public License 00027 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00028 */ 00029 00030 #pragma once 00031 #ifndef BUT_BBOVERLAP_NODE_H 00032 #define BUT_BBOVERLAP_NODE_H 00033 00034 #include <ros/ros.h> 00035 #include "geometry_msgs/Vector3.h" 00036 #include "geometry_msgs/Pose.h" 00037 #include "geometry_msgs/PoseStamped.h" 00038 #include "visualization_msgs/InteractiveMarkerFeedback.h" 00039 #include "visualization_msgs/InteractiveMarkerUpdate.h" 00040 #include "visualization_msgs/Marker.h" 00041 #include "srs_interaction_primitives/ScaleChanged.h" 00042 #include "srs_assisted_arm_navigation_msgs/AssistedArmNavigationState.h" 00043 #include <boost/thread.hpp> 00044 #include "tf/transform_listener.h" 00045 #include <Eigen/Core> 00046 #include <Eigen/Geometry> 00047 #include "boost/date_time/posix_time/posix_time.hpp" 00048 00049 namespace srs_user_tests { 00050 00051 typedef struct { 00052 00053 geometry_msgs::Vector3 lwh; 00054 geometry_msgs::Pose pose; 00055 00056 } tbb; 00057 00058 typedef Eigen::Vector3f tpoint; 00059 typedef std::vector<tpoint> tpoints; 00060 00061 00062 class BBOverlap { 00063 00064 public: 00065 00066 BBOverlap(); 00067 00068 00069 ~BBOverlap(); 00070 00071 tf::TransformListener tfl_; 00072 00073 ros::WallTime last_log_out_; 00074 00075 double points_volume(const tpoints &p); 00076 00077 double rmin(double val1, double val2); 00078 double rmax(double val1, double val2); 00079 00080 private: 00081 00082 protected: 00083 00084 geometry_msgs::Pose gripper_pose_; 00085 geometry_msgs::Pose gripper_pose_curr_; 00086 ros::Publisher gripper_pub_; 00087 visualization_msgs::Marker gripper_marker_; 00088 bool gripper_pose_rec_; 00089 ros::Subscriber sub_gripper_update_; 00090 double gr_success_val_; 00091 00092 ros::Subscriber sub_arm_state_; 00093 00094 bool publish_debug_markers_; 00095 00096 double bb_success_val_; 00097 00098 ros::WallDuration bb_success_min_dur_; 00099 ros::WallDuration gr_success_min_dur_; 00100 00101 ros::WallTime bb_success_first_; 00102 ros::WallTime bb_success_last_; 00103 ros::WallTime bb_success_tmp_; 00104 00105 ros::WallTime gr_success_first_; 00106 ros::WallTime gr_success_last_; 00107 ros::WallTime gr_success_tmp_; 00108 00109 ros::Subscriber sub_im_feedback_; 00110 ros::Subscriber sub_im_scale_; 00111 00112 void im_feedback_cb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& msg); 00113 void im_scale_cb(const srs_interaction_primitives::ScaleChangedConstPtr& msg); 00114 void gripper_im_cb(const visualization_msgs::InteractiveMarkerUpdateConstPtr& msg); 00115 void arm_nav_state_cb(const srs_assisted_arm_navigation_msgs::AssistedArmNavigationStateConstPtr& msg); 00116 00117 bool arm_state_ok_; 00118 00119 void timer_cb(const ros::TimerEvent&); 00120 ros::Timer timer_; 00121 00122 void update_points(tbb bb, tpoints &p); 00123 00124 struct { 00125 00126 boost::mutex mutex; 00127 visualization_msgs::Marker marker; 00128 //geometry_msgs::Vector3 lwh; 00129 tbb bb; 00130 ros::Publisher pub; 00131 double vol; 00132 tpoints points; 00133 00134 ros::Publisher points_pub; 00135 00136 } id_; 00137 00138 struct { 00139 00140 boost::mutex mutex; 00141 /*geometry_msgs::Vector3 lwh; 00142 geometry_msgs::Pose pose;*/ 00143 tbb bb; 00144 tpoints points; 00145 00146 ros::Publisher points_pub; 00147 00148 bool scale_rec; 00149 bool pose_rec; 00150 00151 } im_; 00152 00153 ros::Publisher points_proc_pub_; 00154 00155 void publish_points(tpoints points, ros::Publisher &pub, std_msgs::ColorRGBA c); 00156 00157 }; 00158 00159 } // namespace 00160 00161 #endif