00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <velocity_limited_marker.h>
00019
00020 #include <tf/tf.h>
00021
00022
00023 namespace cob_collision_velocity_filter
00024 {
00025
00026 const double DEFAULT_LIFETIME = 1.0;
00027 const double DEFAULT_Z_POSITION = 0.25;
00028
00029 const double MARKER_SCALE_DIR = 1.0;
00030 const double MARKER_RADIUS_DIR = 0.5;
00031 const double MARKER_WIDTH_DIR = 0.2;
00032
00033 const double MARKER_SCALE_ROT = 1.0;
00034 const double MARKER_RADIUS_ROT = 0.6;
00035
00036 const double MARKER_WIDTH_ROT = 0.15;
00037
00038
00039 const double MAX_VELOCITY = 0.25;
00040 const double MIN_VELOCITY = 0.01;
00041 const double VELOCITY_COEFF = 1.0 / (MAX_VELOCITY - MIN_VELOCITY);
00042
00043 const float MAX_COLOR[4] = { 1.0f, 0.0f, 0.0f, 1.0f };
00044 const float MIN_COLOR[4] = { 1.0f, 0.8f, 0.0f, 0.2f };
00045
00046
00047 VelocityLimitedMarker::VelocityLimitedMarker()
00048 {
00049
00050 nh_ = ros::NodeHandle("~");
00051
00052
00053 nh_.param("marker_frame", base_frame_, std::string("/base_link"));
00054 nh_.param("marker_topic_name", topic_name_, std::string("velocity_limited_marker"));
00055 nh_.param("marker_lifetime", lifetime_, DEFAULT_LIFETIME);
00056 nh_.param("z_pos", z_pos_, DEFAULT_Z_POSITION);
00057 nh_.param("marker_disabled", disabled_, false);
00058
00059
00060 marker_pub_ = nh_.advertise<visualization_msgs::Marker>(topic_name_, 1);
00061
00062 vx_last_ = 0.0;
00063 vy_last_ = 0.0;
00064 vtheta_last_ = 0.0;
00065
00066
00067 createDirectionalMarkers();
00068 createRotationalMarkers();
00069 }
00070
00071
00072 VelocityLimitedMarker::~VelocityLimitedMarker()
00073 {
00074 }
00075
00076
00077 void VelocityLimitedMarker::createDirectionalMarkers()
00078 {
00079
00080 visualization_msgs::Marker marker;
00081 marker.header.frame_id = base_frame_;
00082 marker.header.stamp = ros::Time::now();
00083 marker.ns = "cob_velocity_limited_marker";
00084 marker.id = 0;
00085 marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
00086 marker.action = visualization_msgs::Marker::ADD;
00087 marker.lifetime = ros::Duration(lifetime_);
00088 marker.pose.orientation.x = 0;
00089 marker.pose.orientation.y = 0;
00090 marker.pose.orientation.z = 0;
00091 marker.pose.orientation.w = 1;
00092 marker.pose.position.x = 0;
00093 marker.pose.position.y = 0;
00094 marker.pose.position.z = z_pos_;
00095 marker.scale.x = MARKER_SCALE_DIR;
00096 marker.scale.y = MARKER_SCALE_DIR;
00097 marker.scale.z = 1.0;
00098 marker.color.r = 1.0;
00099 marker.color.g = 0.0;
00100 marker.color.b = 0.0;
00101 marker.color.a = 0.5;
00102
00103
00104 std::vector<geometry_msgs::Point> circle1, circle2;
00105 geometry_msgs::Point v1, v2;
00106
00107 static const int STEPS = 36;
00108 static const int FIRST = -4;
00109 static const int LAST = 4;
00110 for( int i = FIRST; i <= LAST; ++i )
00111 {
00112 float a = float(i) / float(STEPS) * M_PI * 2.0;
00113 float cosa = cos(a);
00114 float sina = sin(a);
00115
00116 v1.x = MARKER_RADIUS_DIR * cosa;
00117 v1.y = MARKER_RADIUS_DIR * sina;
00118 v2.x = (MARKER_RADIUS_DIR + MARKER_WIDTH_DIR) * cosa;
00119 v2.y = (MARKER_RADIUS_DIR + MARKER_WIDTH_DIR) * sina;
00120
00121 circle1.push_back(v1);
00122 circle2.push_back(v2);
00123 }
00124
00125 marker.points.clear();
00126 for( std::size_t i = 0; i < (circle1.size() - 1); ++i )
00127 {
00128 std::size_t i1 = i;
00129 std::size_t i2 = (i + 1) % circle1.size();
00130
00131 marker.points.push_back(circle1[i1]);
00132 marker.points.push_back(circle2[i1]);
00133 marker.points.push_back(circle1[i2]);
00134
00135 marker.points.push_back(circle1[i2]);
00136 marker.points.push_back(circle2[i2]);
00137 marker.points.push_back(circle2[i1]);
00138 }
00139
00140
00141 x_pos_marker_ = marker;
00142 x_pos_marker_.id = 0;
00143 x_pos_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);
00144
00145 x_neg_marker_ = marker;
00146 x_neg_marker_.id = 1;
00147 x_neg_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI);
00148
00149 y_pos_marker_ = marker;
00150 y_pos_marker_.id = 2;
00151 y_pos_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0.5 * M_PI);
00152
00153 y_neg_marker_ = marker;
00154 y_neg_marker_.id = 3;
00155 y_neg_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, -0.5 * M_PI);
00156 }
00157
00158
00159 void VelocityLimitedMarker::createRotationalMarkers()
00160 {
00161
00162 visualization_msgs::Marker marker;
00163 marker.header.frame_id = base_frame_;
00164 marker.header.stamp = ros::Time::now();
00165 marker.ns = "cob_velocity_limited_marker";
00166 marker.id = 0;
00167 marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
00168 marker.action = visualization_msgs::Marker::ADD;
00169 marker.lifetime = ros::Duration(lifetime_);
00170 marker.pose.orientation.x = 0;
00171 marker.pose.orientation.y = 0;
00172 marker.pose.orientation.z = 0;
00173 marker.pose.orientation.w = 1;
00174 marker.pose.position.x = 0;
00175 marker.pose.position.y = 0;
00176 marker.pose.position.z = z_pos_;
00177 marker.scale.x = MARKER_SCALE_ROT;
00178 marker.scale.y = MARKER_SCALE_ROT;
00179 marker.scale.z = 1.0;
00180 marker.color.r = 1.0;
00181 marker.color.g = 0.0;
00182 marker.color.b = 0.0;
00183 marker.color.a = 0.5;
00184
00185
00186 std::vector<geometry_msgs::Point> circle1, circle2, circle3;
00187 geometry_msgs::Point v1, v2, v3;
00188
00189 static const int STEPS = 48;
00190 static const int FIRST = 0;
00191 static const int LAST = 23;
00192 for( int i = FIRST; i <= LAST; ++i )
00193 {
00194 float a = float(2*i) / float(STEPS) * M_PI * 2.0;
00195 float cosa = cos(a);
00196 float sina = sin(a);
00197
00198 v1.x = MARKER_RADIUS_ROT * cosa;
00199 v1.y = MARKER_RADIUS_ROT * sina;
00200 v2.x = (MARKER_RADIUS_ROT + MARKER_WIDTH_ROT) * cosa;
00201 v2.y = (MARKER_RADIUS_ROT + MARKER_WIDTH_ROT) * sina;
00202
00203 circle1.push_back(v1);
00204 circle2.push_back(v2);
00205
00206 a = float(2*i+1) / float(STEPS) * M_PI * 2.0;
00207 cosa = cos(a);
00208 sina = sin(a);
00209
00210 v3.x = (MARKER_RADIUS_ROT + 0.5 * MARKER_WIDTH_ROT) * cosa;
00211 v3.y = (MARKER_RADIUS_ROT + 0.5 * MARKER_WIDTH_ROT) * sina;
00212
00213 circle3.push_back(v3);
00214 }
00215
00216 marker.points.clear();
00217 for( std::size_t i = 0; i < circle1.size(); ++i )
00218 {
00219 marker.points.push_back(circle1[i]);
00220 marker.points.push_back(circle2[i]);
00221 marker.points.push_back(circle3[i]);
00222 }
00223
00224
00225 theta_pos_marker_ = marker;
00226 theta_pos_marker_.id = 4;
00227 theta_pos_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);
00228
00229 theta_neg_marker_ = marker;
00230 theta_neg_marker_.id = 5;
00231 theta_neg_marker_.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0, 0);
00232 }
00233
00234
00235 void VelocityLimitedMarker::interpolateColor(double velocity, std_msgs::ColorRGBA& color)
00236 {
00237 if( velocity < MIN_VELOCITY )
00238 {
00239 color.r = color.g = color.b = color.a = 0.0f;
00240 return;
00241 }
00242
00243 float alpha = float((velocity - MIN_VELOCITY) * VELOCITY_COEFF);
00244 alpha = (alpha > 1.0f) ? 1.0f : alpha;
00245
00246 color.r = (1.0f - alpha) * MIN_COLOR[0] + alpha * MAX_COLOR[0];
00247 color.g = (1.0f - alpha) * MIN_COLOR[1] + alpha * MAX_COLOR[1];
00248 color.b = (1.0f - alpha) * MIN_COLOR[2] + alpha * MAX_COLOR[2];
00249 color.a = (1.0f - alpha) * MIN_COLOR[3] + alpha * MAX_COLOR[3];
00250 }
00251
00252
00253 void VelocityLimitedMarker::publishMarkers( double vel_x_desired,
00254 double vel_x_actual,
00255 double vel_y_desired,
00256 double vel_y_actual,
00257 double vel_theta_desired,
00258 double vel_theta_actual)
00259 {
00260 if( disabled_ )
00261 {
00262 return;
00263 }
00264
00265
00266 double epsilon = 0.05;
00267
00268
00269 double ax,ay,atheta;
00270 ax = vel_x_actual - vx_last_;
00271 ay = vel_y_actual - vy_last_;
00272 atheta = vel_theta_actual - vtheta_last_;
00273 vx_last_ = vel_x_actual;
00274 vy_last_ = vel_y_actual;
00275 vtheta_last_ = vel_theta_actual;
00276
00277
00278 double x_vel_diff = fabs(vel_x_desired - vel_x_actual);
00279 if( x_vel_diff >= epsilon )
00280 {
00281
00282 if (vel_x_desired >= 0.0 && ax <= 0.0)
00283 {
00284 x_pos_marker_.header.stamp = ros::Time::now();
00285 interpolateColor(x_vel_diff, x_pos_marker_.color);
00286
00287 marker_pub_.publish(x_pos_marker_);
00288 }
00289 else if (vel_x_desired <= 0.0 && ax >= 0.0)
00290 {
00291 x_neg_marker_.header.stamp = ros::Time::now();
00292 interpolateColor(x_vel_diff, x_neg_marker_.color);
00293
00294 marker_pub_.publish(x_neg_marker_);
00295 }
00296 }
00297
00298
00299 double y_vel_diff = fabs(vel_y_desired - vel_y_actual);
00300 if( y_vel_diff >= epsilon )
00301 {
00302
00303 if (vel_y_desired >= 0.0 && ay <= 0.0)
00304 {
00305 y_pos_marker_.header.stamp = ros::Time::now();
00306 interpolateColor(y_vel_diff, y_pos_marker_.color);
00307
00308 marker_pub_.publish(y_pos_marker_);
00309 }
00310 else if (vel_y_desired <= 0.0 && ay >= 0.0)
00311 {
00312 y_neg_marker_.header.stamp = ros::Time::now();
00313 interpolateColor(y_vel_diff, y_neg_marker_.color);
00314
00315 marker_pub_.publish(y_neg_marker_);
00316 }
00317 }
00318
00319
00320 double theta_vel_diff = fabs(vel_theta_desired - vel_theta_actual);
00321 if( theta_vel_diff >= epsilon )
00322 {
00323
00324 if (vel_theta_desired >= 0.0 && atheta <= 0.0)
00325 {
00326 theta_pos_marker_.header.stamp = ros::Time::now();
00327 interpolateColor(theta_vel_diff, theta_pos_marker_.color);
00328
00329 marker_pub_.publish(theta_pos_marker_);
00330 }
00331 else if (vel_theta_desired <= 0.0 && atheta >= 0.0)
00332 {
00333 theta_neg_marker_.header.stamp = ros::Time::now();
00334 interpolateColor(theta_vel_diff, theta_neg_marker_.color);
00335
00336 marker_pub_.publish(theta_neg_marker_);
00337 }
00338 }
00339 }
00340
00341
00342 }
00343