move_to_table_node.h
Go to the documentation of this file.00001 00056 #ifndef MOVE_TO_TABLE_NODE_H_ 00057 #define MOVE_TO_TABLE_NODE_H_ 00058 00059 //################## 00060 //#### includes #### 00061 // standard includes 00062 #include <stdio.h> 00063 #include <sstream> 00064 #include <Eigen/Core> 00065 #include <Eigen/Dense> 00066 #include <Eigen/LU> 00067 // ROS includes 00068 #include <ros/ros.h> 00069 #include <cob_3d_mapping_msgs/MoveToTable.h> 00070 #include <cob_3d_mapping_msgs/GetTables.h> 00071 #include <tf/transform_broadcaster.h> 00072 00073 #include <visualization_msgs/Marker.h> 00074 #include <visualization_msgs/InteractiveMarker.h> 00075 #include <visualization_msgs/InteractiveMarkerControl.h> 00076 #include <interactive_markers/interactive_marker_server.h> 00077 //#include <cob_script_server/ScriptAction.h> 00078 #include <tf/transform_listener.h> 00079 00080 00081 class MoveToTableNode 00082 { 00083 public: 00084 // Constructor 00085 MoveToTableNode() ; 00086 // Destructor 00087 ~MoveToTableNode() 00088 { 00090 } 00098 bool moveToTableService (cob_3d_mapping_msgs::MoveToTable::Request &req,cob_3d_mapping_msgs::MoveToTable::Response &res) ; 00108 geometry_msgs::Pose transformToTableCoordinateSystem(cob_3d_mapping_msgs::Table &table,geometry_msgs::Pose &Pose); 00114 bool doIntersect(float line) ; 00121 geometry_msgs::Pose findIntersectionPoint() ; 00128 geometry_msgs::Pose findSafeTargetPoint() ; 00133 void addMarkerForFinalPose(geometry_msgs::Pose finalPose) ; 00142 Eigen::Quaternionf faceTable (geometry_msgs::Pose finalPose); 00143 00144 ros::NodeHandle n_; 00145 cob_3d_mapping_msgs::Table table_; 00146 geometry_msgs::Pose robotPoseInTableCoordinateSys_; 00147 geometry_msgs::Pose robotPose_; 00148 00149 Eigen::Matrix4f transformToTableCoordinateSys_ ; 00150 float safe_dist_ ; 00151 00152 protected: 00153 ros::ServiceServer move_to_table_server_ ; 00154 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> table_im_server_; 00155 ros::Publisher navigation_goal_pub_ ; 00156 00157 }; 00158 00159 00160 00161 00162 00163 #endif /* MOVE_TO_TABLE_NODE_H_ */