$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: teleop_cob_marker.cpp 665 2012-04-19 05:56:58Z 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 #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 //#include <iostream> 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 // wait for transform to the base_link 00067 /* tf::StampedTransform transform; 00068 tf::TransformListener listener; 00069 try { 00070 listener.waitForTransform("/base_link", "/base_footprint", ros::Time(0), ros::Duration(10.0) ); 00071 listener.lookupTransform("/base_link", "/base_footprint", ros::Time(0), transform); 00072 } 00073 catch (tf::TransformException ex) { 00074 ROS_INFO("Wait for transform failed..."); 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 // TODO: use /odom_combined frame to calculate moving direction. for now we use the y displacement for commanding the angular velocity 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 // marker_driver.header.stamp = ros::Time::now(); 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 // marker_navigator.header.stamp = ros::Time::now(); 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