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
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
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
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
00075 nh_ = ros::NodeHandle("~");
00076
00077
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
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
00092 createDirectionalMarkers();
00093 createRotationalMarkers();
00094 }
00095
00096
00097 VelocityLimitedMarker::~VelocityLimitedMarker()
00098 {
00099 }
00100
00101
00102 void VelocityLimitedMarker::createDirectionalMarkers()
00103 {
00104
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;
00127
00128
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
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
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;
00209
00210
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
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
00273 double epsilon = 0.05;
00274
00275
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
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
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
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