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 #include <ros/ros.h>
00029 #include <tf/transform_listener.h>
00030
00031 #include <cob_interactive_teleop/teleop_cob_marker.h>
00032 #include <cob_interactive_teleop/topics_list.h>
00033
00034
00035
00036 using namespace interactive_markers;
00037 using namespace visualization_msgs;
00038
00039 namespace cob_interactive_teleop
00040 {
00041
00042 TeleopCOBMarker::TeleopCOBMarker() : pn_("~")
00043 {
00044 pn_.param(MAX_VEL_X_PARAM, params_.max_vel_x, params_.max_vel_x);
00045 pn_.param(MAX_VEL_Y_PARAM, params_.max_vel_y, params_.max_vel_y);
00046 pn_.param(MAX_VEL_TH_PARAM, params_.max_vel_th, params_.max_vel_th);
00047 pn_.param(SCALE_LINEAR_PARAM, params_.scale_linear, params_.scale_linear);
00048 pn_.param(SCALE_ANGULAR_PARAM, params_.scale_angular, params_.scale_angular);
00049 pn_.param(Z_POS_PARAM, params_.z_pos, params_.z_pos);
00050 pn_.param(DISABLE_DRIVER_PARAM, params_.disable_driver, params_.disable_driver);
00051
00052 ROS_INFO("max_vel_x = %f", params_.max_vel_x);
00053 ROS_INFO("max_vel_y = %f", params_.max_vel_y);
00054 ROS_INFO("max_vel_th = %f", params_.max_vel_th);
00055 ROS_INFO("scale_linear = %f", params_.scale_linear);
00056 ROS_INFO("scale_angular = %f", params_.scale_angular);
00057 ROS_INFO("z_pos = %f", params_.z_pos);
00058 ROS_INFO("disable_driver = %d", int(params_.disable_driver));
00059
00060 server_.reset(new InteractiveMarkerServer("cob_interactive_teleop", "", false));
00061 pub_ = n_.advertise<geometry_msgs::Twist>(BASE_CONTROLLER_COMMAND_TOPIC, 1);
00062
00063 initial_pose_ = geometry_msgs::Pose();
00064 initial_pose_.position.z = params_.z_pos;
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077 createMarkers();
00078 }
00079
00080 void TeleopCOBMarker::processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00081 {
00082 geometry_msgs::Twist twist;
00083
00084 if (feedback->marker_name == MARKER_DRIVER_NAME)
00085 {
00086 twist.linear.x = limitVel(params_.scale_linear * feedback->pose.position.x, params_.max_vel_x);
00087 twist.linear.y = limitVel(params_.scale_linear * feedback->pose.position.y, params_.max_vel_y);
00088 twist.angular.z = limitVel(params_.scale_angular * tf::getYaw(feedback->pose.orientation), params_.max_vel_th);
00089 }
00090 else if (feedback->marker_name == MARKER_NAVIGATOR_NAME)
00091 {
00092
00093 twist.linear.x = limitVel(params_.scale_linear * feedback->pose.position.x, params_.max_vel_x);
00094 twist.angular.z = sign(feedback->pose.position.x) * limitVel(params_.scale_linear * feedback->pose.position.y, params_.max_vel_th);
00095 }
00096 else
00097 return;
00098
00099 pub_.publish(twist);
00100
00101 reinitMarkers();
00102 }
00103
00104 void TeleopCOBMarker::reinitMarkers()
00105 {
00106 if( !params_.disable_driver )
00107 {
00108 server_->setPose(MARKER_DRIVER_NAME, initial_pose_);
00109 }
00110 server_->setPose(MARKER_NAVIGATOR_NAME, initial_pose_);
00111 server_->applyChanges();
00112 }
00113
00114 double TeleopCOBMarker::limitVel(double vel, double limit)
00115 {
00116 if (fabs(vel) >= limit)
00117 {
00118 return limit*sign(vel);
00119 }
00120 else
00121 {
00122 return vel;
00123 }
00124 }
00125
00126 int TeleopCOBMarker::sign(double value)
00127 {
00128 if (value > 0)
00129 return 1;
00130 else if (value <0)
00131 return -1;
00132 else
00133 return 0;
00134 }
00135
00136 void TeleopCOBMarker::createMarkers()
00137 {
00138 InteractiveMarker marker_driver;
00139 marker_driver.name = MARKER_DRIVER_NAME;
00140 marker_driver.header.frame_id = "/base_link";
00141
00142 marker_driver.header.stamp = ros::Time(0);
00143 marker_driver.pose = initial_pose_;
00144 marker_driver.scale = 1.5;
00145
00146 InteractiveMarkerControl control;
00147 control.name = CONTROL_ROTATE_NAME;
00148 control.orientation_mode = InteractiveMarkerControl::FIXED;
00149 control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
00150 control.orientation.x = 0;
00151 control.orientation.y = 1;
00152 control.orientation.z = 0;
00153 control.orientation.w = 1;
00154 marker_driver.controls.push_back(control);
00155
00156 control.name = CONTROL_MOVE_NAME;
00157 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00158 control.orientation.x = 1;
00159 control.orientation.y = 0;
00160 control.orientation.z = 0;
00161 control.orientation.w = 1;
00162 marker_driver.controls.push_back(control);
00163
00164 control.name = CONTROL_STRAFE_NAME;
00165 control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
00166 control.orientation.x = 0;
00167 control.orientation.y = 0;
00168 control.orientation.z = 1;
00169 control.orientation.w = 1;
00170 marker_driver.controls.push_back(control);
00171
00172 InteractiveMarker marker_navigator;
00173 marker_navigator.name = MARKER_NAVIGATOR_NAME;
00174 marker_navigator.header.frame_id = "/base_link";
00175
00176 marker_navigator.header.stamp = ros::Time(0);
00177 marker_navigator.pose = initial_pose_;
00178 marker_navigator.scale = 1.5;
00179
00180 InteractiveMarkerControl floorControl;
00181 floorControl.name = CONTROL_NAVIGATION_NAME;
00182 floorControl.orientation_mode = InteractiveMarkerControl::INHERIT;
00183 floorControl.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
00184 floorControl.orientation.x = 0;
00185 floorControl.orientation.y = 1;
00186 floorControl.orientation.z = 0;
00187 floorControl.orientation.w = 1;
00188 std_msgs::ColorRGBA c;
00189 c.r = 1.0;
00190 c.g = 1.0;
00191 c.b = 0.0;
00192 c.a = 0.7;
00193 makeCircle(floorControl, 0.1, 10.0, c);
00194 marker_navigator.controls.push_back(floorControl);
00195
00196 if( !params_.disable_driver )
00197 {
00198 server_->insert(marker_driver, boost::bind(&TeleopCOBMarker::processFeedback, this, _1));
00199 }
00200 server_->insert(marker_navigator, boost::bind(&TeleopCOBMarker::processFeedback, this, _1));
00201 server_->applyChanges();
00202 }
00203
00204 }
00205