$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: teleop_cob_marker.h 2037 2012-11-30 16:28:17Z spanel $ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Tomas Lokaj (xlokaj03@stud.fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: 09/02/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00028 #ifndef TELEOPCOBMARKER_H_ 00029 #define TELEOPCOBMARKER_H_ 00030 00031 #include <ros/ros.h> 00032 #include <interactive_markers/interactive_marker_server.h> 00033 #include <geometry_msgs/Twist.h> 00034 #include <tf/transform_datatypes.h> 00035 00036 #include <cob_interactive_teleop/interactive_markers_tools.h> 00037 #include <cob_interactive_teleop/parameters_list.h> 00038 00039 namespace cob_interactive_teleop 00040 { 00041 00042 const std::string MARKER_DRIVER_NAME = "marker_driver"; 00043 const std::string MARKER_NAVIGATOR_NAME = "marker_navigator"; 00044 const std::string CONTROL_MOVE_NAME = "control_move"; 00045 const std::string CONTROL_STRAFE_NAME = "control_strafe"; 00046 const std::string CONTROL_ROTATE_NAME = "control_rotate"; 00047 const std::string CONTROL_NAVIGATION_NAME = "controle_navigation"; 00048 00049 typedef boost::shared_ptr<interactive_markers::InteractiveMarkerServer> InteractiveMarkerServerPtr; 00050 00051 00055 struct TeleopCOBParams 00056 { 00057 double max_vel_x, max_vel_y, max_vel_th; 00058 double scale_linear, scale_angular; 00059 double z_pos; 00060 bool disable_driver; 00061 00065 TeleopCOBParams() 00066 : max_vel_x(DEFAULT_MAX_VEL_X) 00067 , max_vel_y(DEFAULT_MAX_VEL_Y) 00068 , max_vel_th(DEFAULT_MAX_VEL_TH) 00069 , scale_linear(DEFAULT_SCALE_LINEAR) 00070 , scale_angular(DEFAULT_SCALE_ANGULAR) 00071 , z_pos(DEFAULT_Z_POS) 00072 , disable_driver(false) 00073 {} 00074 }; 00075 00076 00083 class TeleopCOBMarker 00084 { 00085 public: 00089 TeleopCOBMarker(); 00090 00094 virtual ~TeleopCOBMarker() 00095 { 00096 server_->erase(MARKER_DRIVER_NAME); 00097 server_->erase(MARKER_NAVIGATOR_NAME); 00098 server_->applyChanges(); 00099 } 00100 00104 TeleopCOBParams& getParams() { return params_; } 00105 00109 void reinitMarkers(); 00110 00111 private: 00115 void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 00116 00120 double limitVel(double vel, double limit); 00121 00125 int sign(double value); 00126 00130 void createMarkers(); 00131 00132 private: 00133 // Interactive Marker Server 00134 InteractiveMarkerServerPtr server_; 00135 00136 // Movement publisher 00137 ros::Publisher pub_; 00138 00139 // Node handles 00140 ros::NodeHandle n_, pn_; 00141 00142 // Initial position 00143 geometry_msgs::Pose initial_pose_; 00144 00145 // Teleop parameters 00146 TeleopCOBParams params_; 00147 }; 00148 00149 } 00150 00151 #endif /* TELEOPCOBMARKER_H_ */ 00152