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 #include <interactive_markers/interactive_marker_server.h>
00049 #include "srs_user_tests/SetFloat.h"
00050 
00051 namespace srs_user_tests {
00052 
00053 typedef struct {
00054 
00055         geometry_msgs::Vector3 lwh;
00056         geometry_msgs::Pose pose;
00057 
00058 } tbb;
00059 
00060 typedef Eigen::Vector3f tpoint;
00061 typedef std::vector<tpoint> tpoints;
00062 
00063 
00064 class BBOverlap {
00065 
00066         public:
00067 
00068                 BBOverlap();
00069 
00070 
00071                 ~BBOverlap();
00072 
00073                 tf::TransformListener tfl_;
00074 
00075                 ros::Time last_log_out_;
00076 
00077                 bool bb_suc_;
00078                 bool gr_suc_;
00079 
00080                 double points_volume(const tpoints &p);
00081 
00082                 double rmin(double val1, double val2);
00083                 double rmax(double val1, double val2);
00084 
00085                 bool moveX(SetFloat::Request& req, SetFloat::Response& res);
00086 
00087         private:
00088 
00089         protected:
00090 
00091                 ros::ServiceServer srv_move_;
00092 
00093                 geometry_msgs::Pose gripper_pose_;
00094                 geometry_msgs::Pose gripper_pose_curr_;
00095                 ros::Publisher gripper_pub_;
00096                 visualization_msgs::Marker gripper_marker_;
00097                 bool gripper_pose_rec_;
00098                 ros::Subscriber sub_gripper_update_;
00099                 double gr_success_val_;
00100 
00101                 ros::Subscriber sub_arm_state_;
00102 
00103                 bool publish_debug_markers_;
00104 
00105                 double bb_success_val_;
00106 
00107                 ros::Duration bb_success_min_dur_;
00108                 ros::Duration gr_success_min_dur_;
00109 
00110                 ros::Time bb_success_first_;
00111                 ros::Time bb_success_last_;
00112                 ros::Time bb_success_tmp_;
00113 
00114                 ros::Time gr_success_first_;
00115                 ros::Time gr_success_last_;
00116                 ros::Time gr_success_tmp_;
00117 
00118                 ros::Subscriber sub_im_feedback_;
00119                 ros::Subscriber sub_im_scale_;
00120 
00121                 void im_feedback_cb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& msg);
00122                 void im_scale_cb(const srs_interaction_primitives::ScaleChangedConstPtr& msg);
00123                 void gripper_im_cb(const visualization_msgs::InteractiveMarkerUpdateConstPtr& msg);
00124                 void arm_nav_state_cb(const srs_assisted_arm_navigation_msgs::AssistedArmNavigationStateConstPtr& msg);
00125 
00126                 bool arm_state_ok_;
00127 
00128                 void timer_cb(const ros::TimerEvent&);
00129                 ros::Timer timer_;
00130 
00131                 void update_points(tbb bb, tpoints &p);
00132 
00133                 struct {
00134 
00135                         boost::mutex mutex;
00136                         visualization_msgs::Marker marker;
00137                         tbb bb;
00138                         ros::Publisher pub;
00139                         double vol;
00140                         tpoints points;
00141 
00142                         ros::Publisher points_pub;
00143 
00144                 } id_;
00145 
00146                 struct {
00147 
00148                         boost::mutex mutex;
00149                         /*geometry_msgs::Vector3 lwh;
00150                         geometry_msgs::Pose pose;*/
00151                         tbb bb;
00152                         tpoints points;
00153 
00154                         ros::Publisher points_pub;
00155 
00156                         bool scale_rec;
00157                         bool pose_rec;
00158 
00159                 } im_;
00160 
00161                 ros::Publisher points_proc_pub_;
00162 
00163                 void publish_points(tpoints points, ros::Publisher &pub, std_msgs::ColorRGBA c);
00164 
00165 };
00166 
00167 } // namespace
00168 
00169 #endif


srs_user_tests
Author(s): SRS team
autogenerated on Sun Jan 5 2014 12:14:04