Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
00134 InteractiveMarkerServerPtr server_;
00135
00136
00137 ros::Publisher pub_;
00138
00139
00140 ros::NodeHandle n_, pn_;
00141
00142
00143 geometry_msgs::Pose initial_pose_;
00144
00145
00146 TeleopCOBParams params_;
00147 };
00148
00149 }
00150
00151 #endif
00152