$search
00001 /**************************************************************** 00002 * 00003 * Copyright (c) 2012 Brno University of Technology (BUT) 00004 * 00005 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00006 * 00007 * Project name: care-o-bot 00008 * ROS stack name: cob_navigation 00009 * ROS package name: cob_collision_velocity_filter 00010 * 00011 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00012 * 00013 * Author: Michal Spanel, email:spanel@fit.vutbr.cz 00014 * 00015 * Date of creation: November 2012 00016 * 00017 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00018 * 00019 * Redistribution and use in source and binary forms, with or without 00020 * modification, are permitted provided that the following conditions are met: 00021 * 00022 * * Redistributions of source code must retain the above copyright 00023 * notice, this list of conditions and the following disclaimer. 00024 * * Redistributions in binary form must reproduce the above copyright 00025 * notice, this list of conditions and the following disclaimer in the 00026 * documentation and/or other materials provided with the distribution. 00027 * * Neither the name of the Fraunhofer Institute for Manufacturing 00028 * Engineering and Automation (IPA) nor the names of its 00029 * contributors may be used to endorse or promote products derived from 00030 * this software without specific prior written permission. 00031 * 00032 * This program is free software: you can redistribute it and/or modify 00033 * it under the terms of the GNU Lesser General Public License LGPL as 00034 * published by the Free Software Foundation, either version 3 of the 00035 * License, or (at your option) any later version. 00036 * 00037 * This program is distributed in the hope that it will be useful, 00038 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00039 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00040 * GNU Lesser General Public License LGPL for more details. 00041 * 00042 * You should have received a copy of the GNU Lesser General Public 00043 * License LGPL along with this program. 00044 * If not, see <http://www.gnu.org/licenses/>. 00045 * 00046 ****************************************************************/ 00047 00048 #include <velocity_limited_marker.h> 00049 00050 #include <tf/tf.h> 00051 00052 00053 namespace cob_collision_velocity_filter 00054 { 00055 00056 const double DEFAULT_LIFETIME = 1.0; 00057 const double DEFAULT_Z_POSITION = 0.25; 00058 00059 const double MARKER_SCALE_DIR = 1.0; 00060 const double MARKER_RADIUS_DIR = 0.5; 00061 const double MARKER_WIDTH_DIR = 0.2; 00062 00063 const double MARKER_SCALE_ROT = 1.0; 00064 const double MARKER_RADIUS_ROT = 0.6; 00065 //const double MARKER_WIDTH_ROT = 0.065; 00066 const double MARKER_WIDTH_ROT = 0.15; 00067 00068 const double MAX_VELOCITY = 0.2; 00069 const double VELOCITY_COEFF = 1.0 / MAX_VELOCITY; 00070 00071 00072 VelocityLimitedMarker::VelocityLimitedMarker() 00073 { 00074 // a handle for this node, initialize node 00075 nh_ = ros::NodeHandle("~"); 00076 00077 // read parameters from parameter server 00078 nh_.param("marker_frame", base_frame_, std::string("/base_link")); 00079 nh_.param("marker_topic_name", topic_name_, std::string("velocity_limited_marker")); 00080 nh_.param("marker_lifetime", lifetime_, DEFAULT_LIFETIME); 00081 nh_.param("z_pos", z_pos_, DEFAULT_Z_POSITION); 00082 nh_.param("marker_disabled", disabled_, false); 00083 00084 // Create the publisher 00085 marker_pub_ = nh_.advertise<visualization_msgs::Marker>(topic_name_, 1); 00086 00087 vx_last_ = 0.0; 00088 vy_last_ = 0.0; 00089 vtheta_last_ = 0.0; 00090 00091 // Create the markers 00092 createDirectionalMarkers(); 00093 createRotationalMarkers(); 00094 } 00095 00096 00097 VelocityLimitedMarker::~VelocityLimitedMarker() 00098 { 00099 } 00100 00101 00102 void VelocityLimitedMarker::createDirectionalMarkers() 00103 { 00104 // Message template 00105 visualization_msgs::Marker marker; 00106 marker.header.frame_id = base_frame_; 00107 marker.header.stamp = ros::Time::now(); 00108 marker.ns = "cob_velocity_limited_marker"; 00109 marker.id = 0; 00110 marker.type = visualization_msgs::Marker::TRIANGLE_LIST; 00111 marker.action = visualization_msgs::Marker::ADD; 00112 marker.lifetime = ros::Duration(lifetime_); 00113 marker.pose.orientation.x = 0; 00114 marker.pose.orientation.y = 0; 00115 marker.pose.orientation.z = 0; 00116 marker.pose.orientation.w = 1; 00117 marker.pose.position.x = 0; 00118 marker.pose.position.y = 0; 00119 marker.pose.position.z = z_pos_; 00120 marker.scale.x = MARKER_SCALE_DIR; 00121 marker.scale.y = MARKER_SCALE_DIR; 00122 marker.scale.z = 1.0; 00123 marker.color.r = 1.0; 00124 marker.color.g = 0.0; 00125 marker.color.b = 0.0; 00126 marker.color.a = 0.5; // adjust according to the velocity? 00127 00128 // Create the disc like geometry for the markers 00129 std::vector<geometry_msgs::Point> circle1, circle2; 00130 geometry_msgs::Point v1, v2; 00131 00132 static const int STEPS = 36; 00133 static const int FIRST = -4; 00134 static const int LAST = 4; 00135 for( int i = FIRST; i <= LAST; ++i ) 00136 { 00137 float a = float(i) / float(STEPS) * M_PI * 2.0; 00138 float cosa = cos(a); 00139 float sina = sin(a); 00140 00141 v1.x = MARKER_RADIUS_DIR * cosa; 00142 v1.y = MARKER_RADIUS_DIR * sina; 00143 v2.x = (MARKER_RADIUS_DIR + MARKER_WIDTH_DIR) * cosa; 00144 v2.y = (MARKER_RADIUS_DIR + MARKER_WIDTH_DIR) * sina; 00145 00146 circle1.push_back(v1); 00147 circle2.push_back(v2); 00148 } 00149 00150 marker.points.clear(); 00151 for( std::size_t i = 0; i < (circle1.size() - 1); ++i ) 00152 { 00153 std::size_t i1 = i; 00154 std::size_t i2 = (i + 1) % circle1.size(); 00155 00156 marker.points.push_back(circle1[i1]); 00157 marker.points.push_back(circle2[i1]); 00158 marker.points.push_back(circle1[i2]); 00159 00160 marker.points.push_back(circle1[i2]); 00161 marker.points.push_back(circle2[i2]); 00162 marker.points.push_back(circle2[i1]); 00163 } 00164 00165 // Particular messages for each axis 00166 x_pos_marker_ = marker; 00167 x_pos_marker_.id = 0; 00168 x_pos_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0); 00169 00170 x_neg_marker_ = marker; 00171 x_neg_marker_.id = 1; 00172 x_neg_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI); 00173 00174 y_pos_marker_ = marker; 00175 y_pos_marker_.id = 2; 00176 y_pos_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0.5 * M_PI); 00177 00178 y_neg_marker_ = marker; 00179 y_neg_marker_.id = 3; 00180 y_neg_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, -0.5 * M_PI); 00181 } 00182 00183 00184 void VelocityLimitedMarker::createRotationalMarkers() 00185 { 00186 // Message template 00187 visualization_msgs::Marker marker; 00188 marker.header.frame_id = base_frame_; 00189 marker.header.stamp = ros::Time::now(); 00190 marker.ns = "cob_velocity_limited_marker"; 00191 marker.id = 0; 00192 marker.type = visualization_msgs::Marker::TRIANGLE_LIST; 00193 marker.action = visualization_msgs::Marker::ADD; 00194 marker.lifetime = ros::Duration(lifetime_); 00195 marker.pose.orientation.x = 0; 00196 marker.pose.orientation.y = 0; 00197 marker.pose.orientation.z = 0; 00198 marker.pose.orientation.w = 1; 00199 marker.pose.position.x = 0; 00200 marker.pose.position.y = 0; 00201 marker.pose.position.z = z_pos_; 00202 marker.scale.x = MARKER_SCALE_ROT; 00203 marker.scale.y = MARKER_SCALE_ROT; 00204 marker.scale.z = 1.0; 00205 marker.color.r = 1.0; 00206 marker.color.g = 0.0; 00207 marker.color.b = 0.0; 00208 marker.color.a = 0.5; // adjust according to the velocity? 00209 00210 // Create the disc like geometry for the markers 00211 std::vector<geometry_msgs::Point> circle1, circle2, circle3; 00212 geometry_msgs::Point v1, v2, v3; 00213 00214 static const int STEPS = 48; 00215 static const int FIRST = 0; 00216 static const int LAST = 23; 00217 for( int i = FIRST; i <= LAST; ++i ) 00218 { 00219 float a = float(2*i) / float(STEPS) * M_PI * 2.0; 00220 float cosa = cos(a); 00221 float sina = sin(a); 00222 00223 v1.x = MARKER_RADIUS_ROT * cosa; 00224 v1.y = MARKER_RADIUS_ROT * sina; 00225 v2.x = (MARKER_RADIUS_ROT + MARKER_WIDTH_ROT) * cosa; 00226 v2.y = (MARKER_RADIUS_ROT + MARKER_WIDTH_ROT) * sina; 00227 00228 circle1.push_back(v1); 00229 circle2.push_back(v2); 00230 00231 a = float(2*i+1) / float(STEPS) * M_PI * 2.0; 00232 cosa = cos(a); 00233 sina = sin(a); 00234 00235 v3.x = (MARKER_RADIUS_ROT + 0.5 * MARKER_WIDTH_ROT) * cosa; 00236 v3.y = (MARKER_RADIUS_ROT + 0.5 * MARKER_WIDTH_ROT) * sina; 00237 00238 circle3.push_back(v3); 00239 } 00240 00241 marker.points.clear(); 00242 for( std::size_t i = 0; i < circle1.size(); ++i ) 00243 { 00244 marker.points.push_back(circle1[i]); 00245 marker.points.push_back(circle2[i]); 00246 marker.points.push_back(circle3[i]); 00247 } 00248 00249 // Particular messages for each axis 00250 theta_pos_marker_ = marker; 00251 theta_pos_marker_.id = 4; 00252 theta_pos_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0); 00253 00254 theta_neg_marker_ = marker; 00255 theta_neg_marker_.id = 5; 00256 theta_neg_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0, 0); 00257 } 00258 00259 00260 void VelocityLimitedMarker::publishMarkers( double vel_x_desired, 00261 double vel_x_actual, 00262 double vel_y_desired, 00263 double vel_y_actual, 00264 double vel_theta_desired, 00265 double vel_theta_actual) 00266 { 00267 if( disabled_ ) 00268 { 00269 return; 00270 } 00271 00272 // Publish the markers 00273 double epsilon = 0.05; 00274 00275 // acceleration 00276 double ax,ay,atheta; 00277 ax = vel_x_actual - vx_last_; 00278 ay = vel_y_actual - vy_last_; 00279 atheta = vel_theta_actual - vtheta_last_; 00280 vx_last_ = vel_x_actual; 00281 vy_last_ = vel_y_actual; 00282 vtheta_last_ = vel_theta_actual; 00283 00284 // for x-axis 00285 double x_vel_diff = fabs(vel_x_desired - vel_x_actual); 00286 if( x_vel_diff >= epsilon ) 00287 { 00288 double alpha = x_vel_diff * VELOCITY_COEFF; 00289 if (vel_x_desired >= 0.0 && ax <= 0.0) 00290 { 00291 x_pos_marker_.header.stamp = ros::Time::now(); 00292 x_pos_marker_.color.a = (alpha > 1.0) ? 1.0 : alpha; 00293 marker_pub_.publish(x_pos_marker_); 00294 } 00295 else if (vel_x_desired <= 0.0 && ax >= 0.0) 00296 { 00297 x_neg_marker_.header.stamp = ros::Time::now(); 00298 x_neg_marker_.color.a = (alpha > 1.0) ? 1.0 : alpha; 00299 marker_pub_.publish(x_neg_marker_); 00300 } 00301 } 00302 00303 // for y-axis 00304 double y_vel_diff = fabs(vel_y_desired - vel_y_actual); 00305 if( y_vel_diff >= epsilon ) 00306 { 00307 double alpha = y_vel_diff * VELOCITY_COEFF; 00308 if (vel_y_desired >= 0.0 && ay <= 0.0) 00309 { 00310 y_pos_marker_.header.stamp = ros::Time::now(); 00311 y_pos_marker_.color.a = (alpha > 1.0) ? 1.0 : alpha; 00312 marker_pub_.publish(y_pos_marker_); 00313 } 00314 else if (vel_y_desired <= 0.0 && ay >= 0.0) 00315 { 00316 y_neg_marker_.header.stamp = ros::Time::now(); 00317 y_neg_marker_.color.a = (alpha > 1.0) ? 1.0 : alpha; 00318 marker_pub_.publish(y_neg_marker_); 00319 } 00320 } 00321 00322 // for theta-axis 00323 double theta_vel_diff = fabs(vel_theta_desired - vel_theta_actual); 00324 if( theta_vel_diff >= epsilon ) 00325 { 00326 double alpha = theta_vel_diff * VELOCITY_COEFF; 00327 if (vel_theta_desired >= 0.0 && atheta <= 0.0) 00328 { 00329 theta_pos_marker_.header.stamp = ros::Time::now(); 00330 theta_pos_marker_.color.a = (alpha > 1.0) ? 1.0 : alpha; 00331 marker_pub_.publish(theta_pos_marker_); 00332 } 00333 else if (vel_theta_desired <= 0.0 && atheta >= 0.0) 00334 { 00335 theta_neg_marker_.header.stamp = ros::Time::now(); 00336 theta_neg_marker_.color.a = (alpha > 1.0) ? 1.0 : alpha; 00337 marker_pub_.publish(theta_neg_marker_); 00338 } 00339 } 00340 } 00341 00342 00343 } 00344