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 #include <rviz_visual_tools/rviz_visual_tools.h>
00040
00041
00042 #include <tf_conversions/tf_eigen.h>
00043 #include <eigen_conversions/eigen_msg.h>
00044
00045
00046 #include <utility>
00047 #include <string>
00048 #include <vector>
00049 #include <set>
00050 #include <cmath>
00051
00052 namespace rviz_visual_tools
00053 {
00054 RvizVisualTools::RvizVisualTools(const std::string &base_frame, const std::string &marker_topic)
00055 : nh_("~")
00056 , name_("visual_tools")
00057 , marker_topic_(marker_topic)
00058 , base_frame_(base_frame)
00059 , batch_publishing_enabled_(false)
00060 , pub_rviz_markers_connected_(false)
00061 , pub_rviz_markers_waited_(false)
00062 , psychedelic_mode_(false)
00063 {
00064 initialize();
00065 double bug;
00066 }
00067
00068 void RvizVisualTools::initialize()
00069 {
00070 marker_lifetime_ = ros::Duration(0.0);
00071 alpha_ = 1.0;
00072 global_scale_ = 1.0;
00073
00074 loadRvizMarkers();
00075 }
00076
00077 bool RvizVisualTools::deleteAllMarkers()
00078 {
00079
00080 return publishMarker(reset_marker_);
00081 }
00082
00083 void RvizVisualTools::resetMarkerCounts()
00084 {
00085 arrow_marker_.id++;
00086 sphere_marker_.id++;
00087 block_marker_.id++;
00088 cylinder_marker_.id++;
00089 mesh_marker_.id++;
00090 text_marker_.id++;
00091 cuboid_marker_.id++;
00092 line_strip_marker_.id++;
00093 line_list_marker_.id++;
00094 spheres_marker_.id++;
00095 triangle_marker_.id++;
00096 }
00097
00098 bool RvizVisualTools::loadRvizMarkers()
00099 {
00100
00101 reset_marker_.header.frame_id = base_frame_;
00102 reset_marker_.header.stamp = ros::Time();
00103 reset_marker_.action = 3;
00104
00105
00106
00107 arrow_marker_.header.frame_id = base_frame_;
00108
00109
00110 arrow_marker_.ns = "Arrow";
00111
00112 arrow_marker_.type = visualization_msgs::Marker::ARROW;
00113
00114 arrow_marker_.action = visualization_msgs::Marker::ADD;
00115
00116 arrow_marker_.lifetime = marker_lifetime_;
00117
00118
00119
00120 cuboid_marker_.header.frame_id = base_frame_;
00121
00122
00123 cuboid_marker_.ns = "Cuboid";
00124
00125 cuboid_marker_.type = visualization_msgs::Marker::CUBE;
00126
00127 cuboid_marker_.action = visualization_msgs::Marker::ADD;
00128
00129 cuboid_marker_.lifetime = marker_lifetime_;
00130
00131
00132
00133 line_strip_marker_.header.frame_id = base_frame_;
00134
00135
00136 line_strip_marker_.ns = "Line";
00137
00138 line_strip_marker_.type = visualization_msgs::Marker::LINE_STRIP;
00139
00140 line_strip_marker_.action = visualization_msgs::Marker::ADD;
00141
00142 line_strip_marker_.lifetime = marker_lifetime_;
00143
00144
00145
00146 line_list_marker_.header.frame_id = base_frame_;
00147
00148
00149 line_list_marker_.ns = "Line_List";
00150
00151 line_list_marker_.type = visualization_msgs::Marker::LINE_LIST;
00152
00153 line_list_marker_.action = visualization_msgs::Marker::ADD;
00154
00155 line_list_marker_.lifetime = marker_lifetime_;
00156
00157 line_list_marker_.pose.position.x = 0.0;
00158 line_list_marker_.pose.position.y = 0.0;
00159 line_list_marker_.pose.position.z = 0.0;
00160
00161 line_list_marker_.pose.orientation.x = 0.0;
00162 line_list_marker_.pose.orientation.y = 0.0;
00163 line_list_marker_.pose.orientation.z = 0.0;
00164 line_list_marker_.pose.orientation.w = 1.0;
00165
00166
00167
00168 spheres_marker_.header.frame_id = base_frame_;
00169
00170
00171 spheres_marker_.ns = "Spheres";
00172
00173 spheres_marker_.type = visualization_msgs::Marker::SPHERE_LIST;
00174
00175 spheres_marker_.action = visualization_msgs::Marker::ADD;
00176
00177 spheres_marker_.lifetime = marker_lifetime_;
00178
00179 spheres_marker_.pose.position.x = 0.0;
00180 spheres_marker_.pose.position.y = 0.0;
00181 spheres_marker_.pose.position.z = 0.0;
00182
00183 spheres_marker_.pose.orientation.x = 0.0;
00184 spheres_marker_.pose.orientation.y = 0.0;
00185 spheres_marker_.pose.orientation.z = 0.0;
00186 spheres_marker_.pose.orientation.w = 1.0;
00187
00188
00189 block_marker_.header.frame_id = base_frame_;
00190
00191
00192 block_marker_.ns = "Block";
00193
00194 block_marker_.action = visualization_msgs::Marker::ADD;
00195
00196 block_marker_.type = visualization_msgs::Marker::CUBE;
00197
00198 block_marker_.lifetime = marker_lifetime_;
00199
00200
00201 cylinder_marker_.header.frame_id = base_frame_;
00202
00203 cylinder_marker_.action = visualization_msgs::Marker::ADD;
00204
00205 cylinder_marker_.type = visualization_msgs::Marker::CYLINDER;
00206
00207 cylinder_marker_.lifetime = marker_lifetime_;
00208
00209
00210 mesh_marker_.header.frame_id = base_frame_;
00211
00212
00213 mesh_marker_.action = visualization_msgs::Marker::ADD;
00214
00215 mesh_marker_.type = visualization_msgs::Marker::MESH_RESOURCE;
00216
00217 mesh_marker_.lifetime = marker_lifetime_;
00218
00219
00220 sphere_marker_.header.frame_id = base_frame_;
00221
00222
00223 sphere_marker_.ns = "Sphere";
00224
00225 sphere_marker_.type = visualization_msgs::Marker::SPHERE;
00226
00227 sphere_marker_.action = visualization_msgs::Marker::ADD;
00228
00229 sphere_marker_.pose.position.x = 0;
00230 sphere_marker_.pose.position.y = 0;
00231 sphere_marker_.pose.position.z = 0;
00232 sphere_marker_.pose.orientation.x = 0.0;
00233 sphere_marker_.pose.orientation.y = 0.0;
00234 sphere_marker_.pose.orientation.z = 0.0;
00235 sphere_marker_.pose.orientation.w = 1.0;
00236
00237
00238 sphere_marker_.points.resize(1);
00239 sphere_marker_.colors.resize(1);
00240
00241 sphere_marker_.lifetime = marker_lifetime_;
00242
00243
00244
00245
00246 text_marker_.ns = "Text";
00247
00248 text_marker_.action = visualization_msgs::Marker::ADD;
00249
00250 text_marker_.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00251
00252 text_marker_.lifetime = marker_lifetime_;
00253
00254
00255
00256 triangle_marker_.header.frame_id = base_frame_;
00257 triangle_marker_.ns = "Triangle";
00258
00259 triangle_marker_.action = visualization_msgs::Marker::ADD;
00260
00261 triangle_marker_.type = visualization_msgs::Marker::TRIANGLE_LIST;
00262
00263 triangle_marker_.lifetime = marker_lifetime_;
00264
00265 return true;
00266 }
00267
00268 void RvizVisualTools::loadMarkerPub(bool wait_for_subscriber, bool latched)
00269 {
00270 if (pub_rviz_markers_)
00271 return;
00272
00273
00274 pub_rviz_markers_ = nh_.advertise<visualization_msgs::MarkerArray>(marker_topic_, 10, latched);
00275 ROS_DEBUG_STREAM_NAMED(name_, "Publishing Rviz markers on topic " << pub_rviz_markers_.getTopic());
00276
00277 if (wait_for_subscriber)
00278 waitForSubscriber(pub_rviz_markers_);
00279 }
00280
00281 void RvizVisualTools::waitForMarkerPub()
00282 {
00283 bool blocking = true;
00284 waitForSubscriber(pub_rviz_markers_, 0, blocking);
00285 }
00286
00287 bool RvizVisualTools::waitForSubscriber(const ros::Publisher &pub, const double &wait_time, const bool blocking)
00288 {
00289
00290 ros::Time max_time(ros::Time::now() + ros::Duration(wait_time));
00291
00292
00293
00294 int num_existing_subscribers = pub.getNumSubscribers();
00295
00296
00297 ros::Rate poll_rate(200);
00298
00299 if (blocking && num_existing_subscribers == 0)
00300 ROS_INFO_STREAM_NAMED(name_, "Topic '" << pub.getTopic() << "' waiting for subscriber...");
00301
00302
00303 while (num_existing_subscribers == 0)
00304 {
00305 if (!blocking && ros::Time::now() > max_time)
00306 {
00307 ROS_WARN_STREAM_NAMED(name_, "Topic '" << pub.getTopic() << "' unable to connect to any subscribers within "
00308 << wait_time << " sec. It is possible initially published visual messages "
00309 "will be lost.");
00310 return false;
00311 }
00312 ros::spinOnce();
00313
00314
00315 poll_rate.sleep();
00316
00317
00318 num_existing_subscribers = pub.getNumSubscribers();
00319
00320
00321 if (!ros::ok())
00322 return false;
00323 }
00324 pub_rviz_markers_connected_ = true;
00325
00326 return true;
00327 }
00328
00329 void RvizVisualTools::setFloorToBaseHeight(double floor_to_base_height)
00330 {
00331 ROS_WARN_STREAM_NAMED(name_, "Deperecated function");
00332 }
00333
00334 void RvizVisualTools::setLifetime(double lifetime)
00335 {
00336 marker_lifetime_ = ros::Duration(lifetime);
00337
00338
00339 arrow_marker_.lifetime = marker_lifetime_;
00340 cuboid_marker_.lifetime = marker_lifetime_;
00341 line_strip_marker_.lifetime = marker_lifetime_;
00342 sphere_marker_.lifetime = marker_lifetime_;
00343 block_marker_.lifetime = marker_lifetime_;
00344 mesh_marker_.lifetime = marker_lifetime_;
00345 cylinder_marker_.lifetime = marker_lifetime_;
00346 text_marker_.lifetime = marker_lifetime_;
00347 }
00348
00349 colors RvizVisualTools::getRandColor()
00350 {
00351 std::vector<colors> all_colors;
00352
00353 all_colors.push_back(RED);
00354 all_colors.push_back(GREEN);
00355 all_colors.push_back(BLUE);
00356 all_colors.push_back(GREY);
00357 all_colors.push_back(DARK_GREY);
00358 all_colors.push_back(WHITE);
00359 all_colors.push_back(ORANGE);
00360
00361 all_colors.push_back(YELLOW);
00362 all_colors.push_back(BROWN);
00363 all_colors.push_back(PINK);
00364 all_colors.push_back(LIME_GREEN);
00365 all_colors.push_back(PURPLE);
00366 all_colors.push_back(CYAN);
00367 all_colors.push_back(MAGENTA);
00368
00369 int rand_num = iRand(0, all_colors.size() - 1);
00370 return all_colors[rand_num];
00371 }
00372
00373 std_msgs::ColorRGBA RvizVisualTools::getColor(const colors &color)
00374 {
00375 std_msgs::ColorRGBA result;
00376
00377 switch (color)
00378 {
00379 case RED:
00380 result.r = 0.8;
00381 result.g = 0.1;
00382 result.b = 0.1;
00383 result.a = alpha_;
00384 break;
00385 case GREEN:
00386 result.r = 0.1;
00387 result.g = 0.8;
00388 result.b = 0.1;
00389 result.a = alpha_;
00390 break;
00391 case GREY:
00392 result.r = 0.9;
00393 result.g = 0.9;
00394 result.b = 0.9;
00395 result.a = alpha_;
00396 case DARK_GREY:
00397 result.r = 0.6;
00398 result.g = 0.6;
00399 result.b = 0.6;
00400 result.a = alpha_;
00401 break;
00402 case WHITE:
00403 result.r = 1.0;
00404 result.g = 1.0;
00405 result.b = 1.0;
00406 result.a = alpha_;
00407 break;
00408 case ORANGE:
00409 result.r = 1.0;
00410 result.g = 0.5;
00411 result.b = 0.0;
00412 result.a = alpha_;
00413 break;
00414 case TRANSLUCENT_LIGHT:
00415 result.r = 0.1;
00416 result.g = 0.1;
00417 result.b = 0.1;
00418 result.a = 0.1;
00419 break;
00420 case TRANSLUCENT:
00421 result.r = 0.1;
00422 result.g = 0.1;
00423 result.b = 0.1;
00424 result.a = 0.25;
00425 break;
00426 case TRANSLUCENT_DARK:
00427 result.r = 0.1;
00428 result.g = 0.1;
00429 result.b = 0.1;
00430 result.a = 0.5;
00431 break;
00432 case BLACK:
00433 result.r = 0.0;
00434 result.g = 0.0;
00435 result.b = 0.0;
00436 result.a = alpha_;
00437 break;
00438 case YELLOW:
00439 result.r = 1.0;
00440 result.g = 1.0;
00441 result.b = 0.0;
00442 result.a = alpha_;
00443 break;
00444 case BROWN:
00445 result.r = 0.597;
00446 result.g = 0.296;
00447 result.b = 0.0;
00448 result.a = alpha_;
00449 break;
00450 case PINK:
00451 result.r = 1.0;
00452 result.g = 0.4;
00453 result.b = 1;
00454 result.a = alpha_;
00455 break;
00456 case LIME_GREEN:
00457 result.r = 0.6;
00458 result.g = 1.0;
00459 result.b = 0.2;
00460 result.a = alpha_;
00461 break;
00462 case CLEAR:
00463 result.r = 1.0;
00464 result.g = 1.0;
00465 result.b = 1.0;
00466 result.a = 0.0;
00467 break;
00468 case PURPLE:
00469 result.r = 0.597;
00470 result.g = 0.0;
00471 result.b = 0.597;
00472 result.a = alpha_;
00473 break;
00474 case CYAN:
00475 result.r = 0.0;
00476 result.g = 1.0;
00477 result.b = 1.0;
00478 result.a = alpha_;
00479 break;
00480 case MAGENTA:
00481 result.r = 1.0;
00482 result.g = 0.0;
00483 result.b = 1.0;
00484 result.a = alpha_;
00485 break;
00486 case RAND:
00487 result = createRandColor();
00488 break;
00489 case DEFAULT:
00490 ROS_WARN_STREAM_NAMED(name_, "The 'DEFAULT' color should probably not "
00491 "be used with getColor(). Defaulting to "
00492 "blue.");
00493 case BLUE:
00494 default:
00495 result.r = 0.1;
00496 result.g = 0.1;
00497 result.b = 0.8;
00498 result.a = alpha_;
00499 }
00500
00501 return result;
00502 }
00503
00504 std_msgs::ColorRGBA RvizVisualTools::createRandColor()
00505 {
00506 std_msgs::ColorRGBA result;
00507
00508 const std::size_t MAX_ATTEMPTS = 20;
00509 std::size_t attempts = 0;
00510
00511
00512 do
00513 {
00514 result.r = fRand(0.0, 1.0);
00515 result.g = fRand(0.0, 1.0);
00516 result.b = fRand(0.0, 1.0);
00517
00518
00519 attempts++;
00520 if (attempts > MAX_ATTEMPTS)
00521 {
00522 ROS_WARN_STREAM_NAMED(name_, "Unable to find appropriate random color after " << MAX_ATTEMPTS << " attempts");
00523 break;
00524 }
00525 } while (result.r + result.g + result.b < 1.5);
00526
00527
00528 result.a = alpha_;
00529
00530 return result;
00531 }
00532
00533 double RvizVisualTools::slerp(double start, double end, double range, double value)
00534 {
00535
00536 return start + (((end - start) / range) * value);
00537 }
00538
00539 std_msgs::ColorRGBA RvizVisualTools::getColorScale(double value)
00540 {
00541
00542 if (value < 0)
00543 {
00544 ROS_WARN_STREAM_NAMED(name_, "Intensity value for color scale is below range [0,1], value: " << value);
00545 value = 0;
00546 }
00547 else if (value > 1)
00548 {
00549 ROS_WARN_STREAM_NAMED(name_, "Intensity value for color scale is above range [0,1], value: " << value);
00550 value = 1;
00551 }
00552
00553 std_msgs::ColorRGBA start;
00554 std_msgs::ColorRGBA end;
00555
00556
00557 if (value == 0.0)
00558 return getColor(RED);
00559 else if (value == 1.0)
00560 return getColor(GREEN);
00561 else if (value <= 0.5)
00562 {
00563 start = getColor(RED);
00564 end = getColor(YELLOW);
00565 }
00566 else
00567 {
00568 start = getColor(YELLOW);
00569 end = getColor(GREEN);
00570 value = fmod(value, 0.5);
00571 }
00572
00573 std_msgs::ColorRGBA result;
00574 result.r = slerp(start.r, end.r, 0.5, value);
00575 result.g = slerp(start.g, end.g, 0.5, value);
00576 result.b = slerp(start.b, end.b, 0.5, value);
00577 result.a = alpha_;
00578
00579 return result;
00580 }
00581
00582 geometry_msgs::Vector3 RvizVisualTools::getScale(const scales &scale, bool arrow_scale, double marker_scale)
00583 {
00584 geometry_msgs::Vector3 result;
00585 double val(0.0);
00586 switch (scale)
00587 {
00588 case XXSMALL:
00589 val = 0.005;
00590 break;
00591 case XSMALL:
00592 val = 0.01;
00593 break;
00594 case SMALL:
00595 val = 0.03;
00596 break;
00597 case REGULAR:
00598 val = 0.05;
00599 break;
00600 case LARGE:
00601 val = 0.1;
00602 break;
00603 case xLARGE:
00604 val = 0.2;
00605 break;
00606 case xxLARGE:
00607 val = 0.3;
00608 break;
00609 case xxxLARGE:
00610 val = 0.4;
00611 break;
00612 case XLARGE:
00613 val = 0.5;
00614 break;
00615 case XXLARGE:
00616 val = 1.0;
00617 break;
00618 default:
00619 ROS_ERROR_STREAM_NAMED(name_, "Not implemented yet");
00620 break;
00621 }
00622
00623
00624 result.x = val * marker_scale * global_scale_;
00625 result.y = val * marker_scale * global_scale_;
00626 result.z = val * marker_scale * global_scale_;
00627
00628
00629 if (arrow_scale)
00630 {
00631 result.y *= 0.1;
00632 result.z *= 0.1;
00633 }
00634
00635 return result;
00636 }
00637
00638 Eigen::Vector3d RvizVisualTools::getCenterPoint(Eigen::Vector3d a, Eigen::Vector3d b)
00639 {
00640 Eigen::Vector3d center;
00641 center[0] = (a[0] + b[0]) / 2;
00642 center[1] = (a[1] + b[1]) / 2;
00643 center[2] = (a[2] + b[2]) / 2;
00644 return center;
00645 }
00646
00647 Eigen::Affine3d RvizVisualTools::getVectorBetweenPoints(Eigen::Vector3d a, Eigen::Vector3d b)
00648 {
00649
00650
00651
00652
00653
00654
00655
00656 Eigen::Quaterniond q;
00657
00658 Eigen::Vector3d axis_vector = b - a;
00659 axis_vector.normalize();
00660
00661 Eigen::Vector3d up_vector(0.0, 0.0, 1.0);
00662 Eigen::Vector3d right_axis_vector = axis_vector.cross(up_vector);
00663 right_axis_vector.normalized();
00664 double theta = axis_vector.dot(up_vector);
00665 double angle_rotation = -1.0 * acos(theta);
00666
00667
00668
00669
00670 tf::Vector3 tf_right_axis_vector;
00671 tf::vectorEigenToTF(right_axis_vector, tf_right_axis_vector);
00672
00673
00674 tf::Quaternion tf_q(tf_right_axis_vector, angle_rotation);
00675
00676
00677 tf::quaternionTFToEigen(tf_q, q);
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688 Eigen::Affine3d pose;
00689 q.normalize();
00690 pose = q * Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitY());
00691 pose.translation() = a;
00692
00693 return pose;
00694 }
00695
00696 bool RvizVisualTools::publishMarker(visualization_msgs::Marker &marker)
00697 {
00698
00699 markers_.markers.push_back(marker);
00700
00701
00702 if (!batch_publishing_enabled_ && !internal_batch_publishing_enabled_)
00703 {
00704 return triggerBatchPublish();
00705 }
00706
00707 return true;
00708 }
00709
00710 void RvizVisualTools::enableBatchPublishing(bool enable)
00711 {
00712 batch_publishing_enabled_ = enable;
00713 }
00714
00715 bool RvizVisualTools::triggerBatchPublish()
00716 {
00717 bool result = publishMarkers(markers_);
00718
00719 markers_.markers.clear();
00720 return result;
00721 }
00722
00723 bool RvizVisualTools::triggerBatchPublishAndDisable()
00724 {
00725 triggerBatchPublish();
00726 batch_publishing_enabled_ = false;
00727 }
00728
00729 bool RvizVisualTools::publishMarkers(visualization_msgs::MarkerArray &markers)
00730 {
00731 if (!pub_rviz_markers_)
00732 loadMarkerPub();
00733
00734
00735 if (!pub_rviz_markers_waited_ && !pub_rviz_markers_connected_)
00736 {
00737 ROS_DEBUG_STREAM_NAMED(name_, "Waiting for subscribers before publishing markers...");
00738 waitForSubscriber(pub_rviz_markers_);
00739
00740
00741 pub_rviz_markers_waited_ = true;
00742 }
00743
00744
00745 if (markers.markers.empty())
00746 return false;
00747
00748
00749 if (psychedelic_mode_)
00750 {
00751 for (std::size_t i = 0; i < markers.markers.size(); ++i)
00752 {
00753 markers.markers[i].color.r = 1.0 - markers.markers[i].color.r;
00754 markers.markers[i].color.g = 1.0 - markers.markers[i].color.g;
00755 markers.markers[i].color.b = 1.0 - markers.markers[i].color.b;
00756 for (std::size_t j = 0; j < markers.markers[i].colors.size(); ++j)
00757 {
00758 markers.markers[i].colors[j].r = 1.0 - markers.markers[i].colors[j].r;
00759 markers.markers[i].colors[j].g = 1.0 - markers.markers[i].colors[j].g;
00760 markers.markers[i].colors[j].b = 1.0 - markers.markers[i].colors[j].b;
00761 }
00762 }
00763 }
00764
00765
00766 pub_rviz_markers_.publish(markers);
00767 ros::spinOnce();
00768 return true;
00769 }
00770
00771 bool RvizVisualTools::publishCone(const Eigen::Affine3d &pose, double angle, const colors &color, double scale)
00772 {
00773 return publishCone(convertPose(pose), angle, color, scale);
00774 }
00775
00776 bool RvizVisualTools::publishCone(const geometry_msgs::Pose &pose, double angle, const colors &color, double scale)
00777 {
00778 triangle_marker_.header.stamp = ros::Time::now();
00779 triangle_marker_.id++;
00780
00781 triangle_marker_.color = getColor(color);
00782 triangle_marker_.pose = pose;
00783
00784 geometry_msgs::Point p[3];
00785 static const double delta_theta = M_PI / 16.0;
00786 double theta = 0;
00787
00788 triangle_marker_.points.clear();
00789 for (std::size_t i = 0; i < 32; i++)
00790 {
00791 p[0].x = 0;
00792 p[0].y = 0;
00793 p[0].z = 0;
00794
00795 p[1].x = scale;
00796 p[1].y = scale * cos(theta);
00797 p[1].z = scale * sin(theta);
00798
00799 p[2].x = scale;
00800 p[2].y = scale * cos(theta + delta_theta);
00801 p[2].z = scale * sin(theta + delta_theta);
00802
00803 triangle_marker_.points.push_back(p[0]);
00804 triangle_marker_.points.push_back(p[1]);
00805 triangle_marker_.points.push_back(p[2]);
00806
00807 theta += delta_theta;
00808 }
00809
00810 triangle_marker_.scale.x = 1.0;
00811 triangle_marker_.scale.y = 1.0;
00812 triangle_marker_.scale.z = 1.0;
00813
00814 return publishMarker(triangle_marker_);
00815 }
00816
00817 bool RvizVisualTools::publishXYPlane(const Eigen::Affine3d &pose, const colors &color, double scale)
00818 {
00819 return publishXYPlane(convertPose(pose), color, scale);
00820 }
00821
00822 bool RvizVisualTools::publishXYPlane(const geometry_msgs::Pose &pose, const colors &color, double scale)
00823 {
00824 triangle_marker_.header.stamp = ros::Time::now();
00825 triangle_marker_.id++;
00826
00827 triangle_marker_.color = getColor(color);
00828 triangle_marker_.pose = pose;
00829
00830 geometry_msgs::Point p[4];
00831 p[0].x = 1.0 * scale;
00832 p[0].y = 1.0 * scale;
00833 p[0].z = 0.0;
00834
00835 p[1].x = -1.0 * scale;
00836 p[1].y = 1.0 * scale;
00837 p[1].z = 0.0;
00838
00839 p[2].x = -1.0 * scale;
00840 p[2].y = -1.0 * scale;
00841 p[2].z = 0.0;
00842
00843 p[3].x = 1.0 * scale;
00844 p[3].y = -1.0 * scale;
00845 p[3].z = 0.0;
00846
00847 triangle_marker_.scale.x = 1.0;
00848 triangle_marker_.scale.y = 1.0;
00849 triangle_marker_.scale.z = 1.0;
00850
00851 triangle_marker_.points.clear();
00852 triangle_marker_.points.push_back(p[0]);
00853 triangle_marker_.points.push_back(p[1]);
00854 triangle_marker_.points.push_back(p[2]);
00855
00856 triangle_marker_.points.push_back(p[2]);
00857 triangle_marker_.points.push_back(p[3]);
00858 triangle_marker_.points.push_back(p[0]);
00859
00860 return publishMarker(triangle_marker_);
00861 }
00862
00863 bool RvizVisualTools::publishXZPlane(const Eigen::Affine3d &pose, const colors &color, double scale)
00864 {
00865 return publishXZPlane(convertPose(pose), color, scale);
00866 }
00867
00868 bool RvizVisualTools::publishXZPlane(const geometry_msgs::Pose &pose, const colors &color, double scale)
00869 {
00870 triangle_marker_.header.stamp = ros::Time::now();
00871 triangle_marker_.id++;
00872
00873 triangle_marker_.color = getColor(color);
00874 triangle_marker_.pose = pose;
00875
00876 geometry_msgs::Point p[4];
00877 p[0].x = 1.0 * scale;
00878 p[0].y = 0;
00879 p[0].z = 1.0 * scale;
00880
00881 p[1].x = -1.0 * scale;
00882 p[1].y = 0;
00883 p[1].z = 1.0 * scale;
00884
00885 p[2].x = -1.0 * scale;
00886 p[2].y = 0;
00887 p[2].z = -1.0 * scale;
00888
00889 p[3].x = 1.0 * scale;
00890 p[3].y = 0;
00891 p[3].z = -1.0 * scale;
00892
00893 triangle_marker_.scale.x = 1.0;
00894 triangle_marker_.scale.y = 1.0;
00895 triangle_marker_.scale.z = 1.0;
00896
00897 triangle_marker_.points.clear();
00898 triangle_marker_.points.push_back(p[0]);
00899 triangle_marker_.points.push_back(p[1]);
00900 triangle_marker_.points.push_back(p[2]);
00901
00902 triangle_marker_.points.push_back(p[2]);
00903 triangle_marker_.points.push_back(p[3]);
00904 triangle_marker_.points.push_back(p[0]);
00905
00906 return publishMarker(triangle_marker_);
00907 }
00908
00909 bool RvizVisualTools::publishYZPlane(const Eigen::Affine3d &pose, const colors &color, double scale)
00910 {
00911 return publishYZPlane(convertPose(pose), color, scale);
00912 }
00913
00914 bool RvizVisualTools::publishYZPlane(const geometry_msgs::Pose &pose, const colors &color, double scale)
00915 {
00916 triangle_marker_.header.stamp = ros::Time::now();
00917 triangle_marker_.id++;
00918
00919 triangle_marker_.color = getColor(color);
00920 triangle_marker_.pose = pose;
00921
00922 geometry_msgs::Point p[4];
00923 p[0].x = 0;
00924 p[0].y = 1.0 * scale;
00925 p[0].z = 1.0 * scale;
00926
00927 p[1].x = 0;
00928 p[1].y = -1.0 * scale;
00929 p[1].z = 1.0 * scale;
00930
00931 p[2].x = 0;
00932 p[2].y = -1.0 * scale;
00933 p[2].z = -1.0 * scale;
00934
00935 p[3].x = 0;
00936 p[3].y = 1.0 * scale;
00937 p[3].z = -1.0 * scale;
00938
00939 triangle_marker_.scale.x = 1.0;
00940 triangle_marker_.scale.y = 1.0;
00941 triangle_marker_.scale.z = 1.0;
00942
00943 triangle_marker_.points.clear();
00944 triangle_marker_.points.push_back(p[0]);
00945 triangle_marker_.points.push_back(p[1]);
00946 triangle_marker_.points.push_back(p[2]);
00947
00948 triangle_marker_.points.push_back(p[2]);
00949 triangle_marker_.points.push_back(p[3]);
00950 triangle_marker_.points.push_back(p[0]);
00951
00952 return publishMarker(triangle_marker_);
00953 }
00954
00955 bool RvizVisualTools::publishSphere(const Eigen::Affine3d &pose, const colors &color, const scales &scale,
00956 const std::string &ns, const std::size_t &id)
00957 {
00958 return publishSphere(convertPose(pose), color, scale, ns, id);
00959 }
00960
00961 bool RvizVisualTools::publishSphere(const Eigen::Vector3d &point, const colors &color, const scales &scale,
00962 const std::string &ns, const std::size_t &id)
00963 {
00964 geometry_msgs::Pose pose_msg;
00965 tf::pointEigenToMsg(point, pose_msg.position);
00966 return publishSphere(pose_msg, color, scale, ns, id);
00967 }
00968
00969 bool RvizVisualTools::publishSphere(const Eigen::Vector3d &point, const colors &color, const double scale,
00970 const std::string &ns, const std::size_t &id)
00971 {
00972 geometry_msgs::Pose pose_msg;
00973 tf::pointEigenToMsg(point, pose_msg.position);
00974 return publishSphere(pose_msg, color, scale, ns, id);
00975 }
00976
00977 bool RvizVisualTools::publishSphere(const geometry_msgs::Point &point, const colors &color, const scales &scale,
00978 const std::string &ns, const std::size_t &id)
00979 {
00980 geometry_msgs::Pose pose_msg;
00981 pose_msg.position = point;
00982 return publishSphere(pose_msg, color, scale, ns, id);
00983 }
00984
00985 bool RvizVisualTools::publishSphere(const geometry_msgs::Pose &pose, const colors &color, const scales &scale,
00986 const std::string &ns, const std::size_t &id)
00987 {
00988 return publishSphere(pose, color, getScale(scale, false, 0.1), ns, id);
00989 }
00990
00991 bool RvizVisualTools::publishSphere(const geometry_msgs::Pose &pose, const colors &color, double scale,
00992 const std::string &ns, const std::size_t &id)
00993 {
00994 geometry_msgs::Vector3 scale_msg;
00995 scale_msg.x = scale;
00996 scale_msg.y = scale;
00997 scale_msg.z = scale;
00998 return publishSphere(pose, color, scale_msg, ns, id);
00999 }
01000
01001 bool RvizVisualTools::publishSphere(const geometry_msgs::Pose &pose, const colors &color,
01002 const geometry_msgs::Vector3 scale, const std::string &ns, const std::size_t &id)
01003 {
01004 return publishSphere(pose, getColor(color), scale, ns, id);
01005 }
01006
01007 bool RvizVisualTools::publishSphere(const Eigen::Affine3d &pose, const std_msgs::ColorRGBA &color,
01008 const geometry_msgs::Vector3 scale, const std::string &ns, const std::size_t &id)
01009 {
01010 return publishSphere(convertPose(pose), color, scale, ns, id);
01011 }
01012
01013 bool RvizVisualTools::publishSphere(const geometry_msgs::Pose &pose, const std_msgs::ColorRGBA &color,
01014 const geometry_msgs::Vector3 scale, const std::string &ns, const std::size_t &id)
01015 {
01016
01017 sphere_marker_.header.stamp = ros::Time::now();
01018
01019
01020 if (id == 0)
01021 sphere_marker_.id++;
01022 else
01023 sphere_marker_.id = id;
01024
01025 sphere_marker_.pose = pose;
01026 sphere_marker_.color = color;
01027 sphere_marker_.scale = scale;
01028 sphere_marker_.ns = ns;
01029
01030
01031 return publishMarker(sphere_marker_);
01032 }
01033
01034 bool RvizVisualTools::publishSphere(const geometry_msgs::PoseStamped &pose, const colors &color,
01035 const geometry_msgs::Vector3 scale, const std::string &ns, const std::size_t &id)
01036 {
01037
01038 sphere_marker_.header = pose.header;
01039
01040 if (id == 0)
01041 sphere_marker_.id++;
01042 else
01043 sphere_marker_.id = id;
01044
01045 sphere_marker_.pose = pose.pose;
01046 sphere_marker_.color = getColor(color);
01047 sphere_marker_.scale = scale;
01048 sphere_marker_.ns = ns;
01049
01050
01051 publishMarker(sphere_marker_);
01052
01053 sphere_marker_.header.frame_id = base_frame_;
01054 return true;
01055 }
01056
01057 bool RvizVisualTools::publishXArrow(const Eigen::Affine3d &pose, const colors &color, const scales &scale,
01058 double length)
01059 {
01060 return publishArrow(convertPose(pose), color, scale, length);
01061 }
01062
01063 bool RvizVisualTools::publishXArrow(const geometry_msgs::Pose &pose, const colors &color, const scales &scale,
01064 double length)
01065 {
01066 return publishArrow(pose, color, scale, length);
01067 }
01068
01069 bool RvizVisualTools::publishXArrow(const geometry_msgs::PoseStamped &pose, const colors &color, const scales &scale,
01070 double length)
01071 {
01072 return publishArrow(pose, color, scale, length);
01073 }
01074
01075 bool RvizVisualTools::publishYArrow(const Eigen::Affine3d &pose, const colors &color, const scales &scale,
01076 double length)
01077 {
01078 Eigen::Affine3d arrow_pose = pose * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
01079 return publishArrow(convertPose(arrow_pose), color, scale, length);
01080 }
01081
01082 bool RvizVisualTools::publishYArrow(const geometry_msgs::Pose &pose, const colors &color, const scales &scale,
01083 double length)
01084 {
01085 Eigen::Affine3d arrow_pose = convertPose(pose) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
01086 return publishArrow(convertPose(arrow_pose), color, scale, length);
01087 }
01088
01089 bool RvizVisualTools::publishYArrow(const geometry_msgs::PoseStamped &pose, const colors &color, const scales &scale,
01090 double length)
01091 {
01092 Eigen::Affine3d arrow_pose = convertPose(pose.pose) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
01093 geometry_msgs::PoseStamped new_pose = pose;
01094 new_pose.pose = convertPose(arrow_pose);
01095 return publishArrow(new_pose, color, scale, length);
01096 }
01097
01098 bool RvizVisualTools::publishZArrow(const Eigen::Affine3d &pose, const colors &color, const scales &scale,
01099 double length, const std::size_t &id)
01100 {
01101 Eigen::Affine3d arrow_pose = pose * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY());
01102 return publishArrow(convertPose(arrow_pose), color, scale, length, id);
01103 }
01104
01105 bool RvizVisualTools::publishZArrow(const geometry_msgs::Pose &pose, const colors &color, const scales &scale,
01106 double length)
01107 {
01108 Eigen::Affine3d arrow_pose = convertPose(pose) * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY());
01109 return publishArrow(convertPose(arrow_pose), color, scale, length);
01110 }
01111
01112 bool RvizVisualTools::publishZArrow(const geometry_msgs::PoseStamped &pose, const colors &color, const scales &scale,
01113 double length)
01114 {
01115 Eigen::Affine3d arrow_pose = convertPose(pose.pose) * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY());
01116 geometry_msgs::PoseStamped new_pose = pose;
01117 new_pose.pose = convertPose(arrow_pose);
01118 return publishArrow(new_pose, color, scale, length);
01119 }
01120
01121 bool RvizVisualTools::publishZArrow(const geometry_msgs::PoseStamped &pose, const colors &color, const scales &scale,
01122 double length, const std::size_t &id)
01123 {
01124 Eigen::Affine3d arrow_pose = convertPose(pose.pose) * Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY());
01125 geometry_msgs::PoseStamped new_pose = pose;
01126 new_pose.pose = convertPose(arrow_pose);
01127 return publishArrow(new_pose, color, scale, length, id);
01128 }
01129
01130 bool RvizVisualTools::publishArrow(const Eigen::Affine3d &pose, const colors &color, const scales &scale, double length,
01131 const std::size_t &id)
01132 {
01133 return publishArrow(convertPose(pose), color, scale, length, id);
01134 }
01135
01136 bool RvizVisualTools::publishArrow(const geometry_msgs::Pose &pose, const colors &color, const scales &scale,
01137 double length, const std::size_t &id)
01138 {
01139
01140 arrow_marker_.header.stamp = ros::Time::now();
01141 arrow_marker_.header.frame_id = base_frame_;
01142
01143 if (id == 0)
01144 arrow_marker_.id++;
01145 else
01146 arrow_marker_.id = id;
01147
01148 arrow_marker_.pose = pose;
01149 arrow_marker_.color = getColor(color);
01150 arrow_marker_.scale = getScale(scale, true);
01151 arrow_marker_.scale.x = length;
01152
01153
01154 return publishMarker(arrow_marker_);
01155 }
01156
01157 bool RvizVisualTools::publishArrow(const geometry_msgs::PoseStamped &pose, const colors &color, const scales &scale,
01158 double length, const std::size_t &id)
01159 {
01160
01161 arrow_marker_.header = pose.header;
01162
01163 if (id == 0)
01164 arrow_marker_.id++;
01165 else
01166 arrow_marker_.id = id;
01167
01168 arrow_marker_.pose = pose.pose;
01169 arrow_marker_.color = getColor(color);
01170 arrow_marker_.scale = getScale(scale, true);
01171 arrow_marker_.scale.x = length;
01172
01173
01174 publishMarker(arrow_marker_);
01175
01176 arrow_marker_.header.frame_id = base_frame_;
01177 }
01178
01179 bool RvizVisualTools::publishBlock(const geometry_msgs::Pose &pose, const colors &color, const double &block_size)
01180 {
01181
01182 block_marker_.header.stamp = ros::Time::now();
01183
01184 block_marker_.id++;
01185
01186
01187 block_marker_.pose = pose;
01188
01189
01190 block_marker_.scale.x = block_size;
01191 block_marker_.scale.y = block_size;
01192 block_marker_.scale.z = block_size;
01193
01194
01195 block_marker_.color = getColor(color);
01196
01197
01198 return publishMarker(block_marker_);
01199 }
01200
01201 bool RvizVisualTools::publishBlock(const Eigen::Affine3d &pose, const colors &color, const double &block_size)
01202 {
01203 return publishBlock(convertPose(pose), color, block_size);
01204 }
01205
01206 bool RvizVisualTools::publishAxisLabeled(const Eigen::Affine3d &pose, const std::string &label, const scales &scale, const colors &color)
01207 {
01208 return publishAxisLabeled(convertPose(pose), label, scale, color);
01209 }
01210
01211 bool RvizVisualTools::publishAxisLabeled(const geometry_msgs::Pose &pose, const std::string &label, const scales &scale, const colors &color)
01212 {
01213 publishAxis(pose, 0.1, 0.01, label);
01214
01215 geometry_msgs::Pose pose_shifted = pose;
01216 pose_shifted.position.y -= 0.05;
01217 publishText(pose_shifted, label, color, scale, false);
01218 return true;
01219 }
01220
01221 bool RvizVisualTools::publishAxis(const geometry_msgs::Pose &pose, double length, double radius, const std::string &ns)
01222 {
01223 return publishAxis(convertPose(pose), length, radius, ns);
01224 }
01225
01226 bool RvizVisualTools::publishAxis(const Eigen::Affine3d &pose, double length, double radius, const std::string &ns)
01227 {
01228
01229 enableInternalBatchPublishing(true);
01230
01231
01232 Eigen::Affine3d x_pose =
01233 Eigen::Translation3d(length / 2.0, 0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY());
01234 x_pose = pose * x_pose;
01235 publishCylinder(x_pose, rviz_visual_tools::RED, length, radius, ns);
01236
01237
01238 Eigen::Affine3d y_pose =
01239 Eigen::Translation3d(0, length / 2.0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX());
01240 y_pose = pose * y_pose;
01241 publishCylinder(y_pose, rviz_visual_tools::GREEN, length, radius, ns);
01242
01243
01244 Eigen::Affine3d z_pose = Eigen::Translation3d(0, 0, length / 2.0) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
01245 z_pose = pose * z_pose;
01246 publishCylinder(z_pose, rviz_visual_tools::BLUE, length, radius, ns);
01247
01248
01249 return triggerInternalBatchPublishAndDisable();
01250 }
01251
01252 bool RvizVisualTools::publishCylinder(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, const colors &color,
01253 double radius, const std::string &ns)
01254 {
01255 return publishCylinder(point1, point2, getColor(color), radius, ns);
01256 }
01257
01258 bool RvizVisualTools::publishCylinder(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2,
01259 const std_msgs::ColorRGBA &color, double radius, const std::string &ns)
01260 {
01261
01262 double height = (point1 - point2).lpNorm<2>();
01263
01264
01265 Eigen::Vector3d pt_center = getCenterPoint(point1, point2);
01266
01267
01268 Eigen::Affine3d pose;
01269 pose = getVectorBetweenPoints(pt_center, point2);
01270
01271
01272 Eigen::Affine3d rotation;
01273 rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
01274 pose = pose * rotation;
01275
01276
01277 publishCylinder(convertPose(pose), color, height, radius);
01278 }
01279
01280 bool RvizVisualTools::publishCylinder(const Eigen::Affine3d &pose, const colors &color, double height, double radius,
01281 const std::string &ns)
01282 {
01283 return publishCylinder(convertPose(pose), color, height, radius, ns);
01284 }
01285
01286 bool RvizVisualTools::publishCylinder(const geometry_msgs::Pose &pose, const colors &color, double height,
01287 double radius, const std::string &ns)
01288 {
01289 return publishCylinder(pose, getColor(color), height, radius, ns);
01290 }
01291
01292 bool RvizVisualTools::publishCylinder(const geometry_msgs::Pose &pose, const std_msgs::ColorRGBA &color, double height,
01293 double radius, const std::string &ns)
01294 {
01295
01296 cylinder_marker_.header.stamp = ros::Time::now();
01297 cylinder_marker_.ns = ns;
01298 cylinder_marker_.id++;
01299
01300
01301 cylinder_marker_.pose = pose;
01302
01303
01304 cylinder_marker_.scale.x = radius;
01305 cylinder_marker_.scale.y = radius;
01306 cylinder_marker_.scale.z = height;
01307
01308
01309 cylinder_marker_.color = color;
01310
01311
01312 return publishMarker(cylinder_marker_);
01313 }
01314
01315 bool RvizVisualTools::publishMesh(const Eigen::Affine3d &pose, const std::string &file_name, const colors &color,
01316 double scale, const std::string &ns, const std::size_t &id)
01317 {
01318 return publishMesh(convertPose(pose), file_name, color, scale, ns, id);
01319 }
01320
01321 bool RvizVisualTools::publishMesh(const geometry_msgs::Pose &pose, const std::string &file_name, const colors &color,
01322 double scale, const std::string &ns, const std::size_t &id)
01323 {
01324
01325 mesh_marker_.header.stamp = ros::Time::now();
01326
01327 if (id == 0)
01328 mesh_marker_.id++;
01329 else
01330 mesh_marker_.id = id;
01331
01332
01333 mesh_marker_.mesh_resource = file_name;
01334 mesh_marker_.mesh_use_embedded_materials = true;
01335
01336
01337 mesh_marker_.pose = pose;
01338
01339
01340 mesh_marker_.scale.x = scale;
01341 mesh_marker_.scale.y = scale;
01342 mesh_marker_.scale.z = scale;
01343
01344
01345
01346 mesh_marker_.ns = ns;
01347
01348
01349 mesh_marker_.color = getColor(color);
01350
01351
01352 return publishMarker(mesh_marker_);
01353 }
01354
01355 bool RvizVisualTools::publishGraph(const graph_msgs::GeometryGraph &graph, const colors &color, double radius)
01356 {
01357
01358
01359 typedef std::pair<std::size_t, std::size_t> node_ids;
01360 std::set<node_ids> added_edges;
01361 std::pair<std::set<node_ids>::iterator, bool> return_value;
01362 Eigen::Vector3d a, b;
01363 for (std::size_t i = 0; i < graph.nodes.size(); ++i)
01364 {
01365 for (std::size_t j = 0; j < graph.edges[i].node_ids.size(); ++j)
01366 {
01367
01368 return_value = added_edges.insert(node_ids(i, j));
01369 if (return_value.second == false)
01370 {
01371
01372 }
01373 else
01374 {
01375
01376 a = convertPoint(graph.nodes[i]);
01377 b = convertPoint(graph.nodes[graph.edges[i].node_ids[j]]);
01378
01379 publishCylinder(a, b, color, radius);
01380 }
01381 }
01382 }
01383
01384 return true;
01385 }
01386
01387 bool RvizVisualTools::publishCuboid(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, const colors &color)
01388 {
01389 return publishCuboid(convertPoint(point1), convertPoint(point2), color);
01390 }
01391
01392 bool RvizVisualTools::publishCuboid(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2,
01393 const colors &color, const std::string &ns, const std::size_t &id)
01394 {
01395
01396 cuboid_marker_.header.stamp = ros::Time::now();
01397 cuboid_marker_.ns = ns;
01398
01399 if (id == 0)
01400 cuboid_marker_.id++;
01401 else
01402 cuboid_marker_.id = id;
01403
01404 cuboid_marker_.color = getColor(color);
01405
01406
01407 geometry_msgs::Pose pose;
01408 pose.position.x = (point1.x - point2.x) / 2.0 + point2.x;
01409 pose.position.y = (point1.y - point2.y) / 2.0 + point2.y;
01410 pose.position.z = (point1.z - point2.z) / 2.0 + point2.z;
01411 cuboid_marker_.pose = pose;
01412
01413
01414 cuboid_marker_.scale.x = fabs(point1.x - point2.x);
01415 cuboid_marker_.scale.y = fabs(point1.y - point2.y);
01416 cuboid_marker_.scale.z = fabs(point1.z - point2.z);
01417
01418
01419 if (!cuboid_marker_.scale.x)
01420 cuboid_marker_.scale.x = SMALL_SCALE;
01421 if (!cuboid_marker_.scale.y)
01422 cuboid_marker_.scale.y = SMALL_SCALE;
01423 if (!cuboid_marker_.scale.z)
01424 cuboid_marker_.scale.z = SMALL_SCALE;
01425
01426
01427 return publishMarker(cuboid_marker_);
01428 }
01429
01430 bool RvizVisualTools::publishCuboid(const Eigen::Affine3d &pose, const double depth, const double width,
01431 const double height, const colors &color)
01432 {
01433 return publishCuboid(convertPose(pose), depth, width, height, color);
01434 }
01435
01436 bool RvizVisualTools::publishCuboid(const geometry_msgs::Pose &pose, const double depth, const double width,
01437 const double height, const colors &color)
01438 {
01439 cuboid_marker_.header.stamp = ros::Time::now();
01440
01441 cuboid_marker_.id++;
01442 cuboid_marker_.color = getColor(color);
01443
01444 cuboid_marker_.pose = pose;
01445
01446
01447 if (depth <= 0)
01448 cuboid_marker_.scale.x = SMALL_SCALE;
01449 else
01450 cuboid_marker_.scale.x = depth;
01451
01452 if (width <= 0)
01453 cuboid_marker_.scale.y = SMALL_SCALE;
01454 else
01455 cuboid_marker_.scale.y = width;
01456
01457 if (height <= 0)
01458 cuboid_marker_.scale.z = SMALL_SCALE;
01459 else
01460 cuboid_marker_.scale.z = height;
01461
01462 return publishMarker(cuboid_marker_);
01463 }
01464
01465 bool RvizVisualTools::publishLine(const Eigen::Affine3d &point1, const Eigen::Affine3d &point2, const colors &color,
01466 const scales &scale)
01467 {
01468 return publishLine(convertPoseToPoint(point1), convertPoseToPoint(point2), color, scale);
01469 }
01470
01471 bool RvizVisualTools::publishLine(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, const colors &color,
01472 const scales &scale)
01473 {
01474 return publishLine(convertPoint(point1), convertPoint(point2), color, scale);
01475 }
01476 bool RvizVisualTools::publishLine(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2, const colors &color,
01477 const double &radius)
01478 {
01479 geometry_msgs::Vector3 scale;
01480 scale.x = radius;
01481 scale.y = radius;
01482 scale.z = radius;
01483 return publishLine(convertPoint(point1), convertPoint(point2), getColor(color), scale);
01484 }
01485
01486 bool RvizVisualTools::publishLine(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2,
01487 const std_msgs::ColorRGBA &color, const scales &scale)
01488 {
01489 return publishLine(convertPoint(point1), convertPoint(point2), color, scale);
01490 }
01491
01492 bool RvizVisualTools::publishLine(const Eigen::Vector3d &point1, const Eigen::Vector3d &point2,
01493 const std_msgs::ColorRGBA &color, const double &radius)
01494 {
01495 geometry_msgs::Vector3 scale;
01496 scale.x = radius;
01497 scale.y = radius;
01498 scale.z = radius;
01499 return publishLine(convertPoint(point1), convertPoint(point2), color, scale);
01500 }
01501
01502 bool RvizVisualTools::publishLine(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2,
01503 const colors &color, const scales &scale)
01504 {
01505 return publishLine(point1, point2, getColor(color), scale);
01506 }
01507
01508 bool RvizVisualTools::publishLine(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2,
01509 const std_msgs::ColorRGBA &color, const scales &scale)
01510 {
01511 return publishLine(point1, point2, color, getScale(scale, false, 0.1));
01512 }
01513
01514 bool RvizVisualTools::publishLine(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2,
01515 const std_msgs::ColorRGBA &color, const geometry_msgs::Vector3 &scale)
01516 {
01517
01518 line_strip_marker_.header.stamp = ros::Time::now();
01519
01520 line_strip_marker_.id++;
01521 line_strip_marker_.color = color;
01522 line_strip_marker_.scale = scale;
01523
01524 line_strip_marker_.points.clear();
01525 line_strip_marker_.points.push_back(point1);
01526 line_strip_marker_.points.push_back(point2);
01527
01528
01529 return publishMarker(line_strip_marker_);
01530 }
01531
01532 bool RvizVisualTools::publishPath(const std::vector<geometry_msgs::Point> &path, const colors &color,
01533 const scales &scale, const std::string &ns)
01534 {
01535 if (path.size() < 2)
01536 {
01537 ROS_WARN_STREAM_NAMED(name_, "Skipping path because " << path.size() << " points passed in.");
01538 return true;
01539 }
01540
01541 line_strip_marker_.header.stamp = ros::Time();
01542 line_strip_marker_.ns = ns;
01543
01544
01545 line_strip_marker_.id++;
01546
01547 std_msgs::ColorRGBA this_color = getColor(color);
01548 line_strip_marker_.scale = getScale(scale, false, 0.25);
01549 line_strip_marker_.color = this_color;
01550 line_strip_marker_.points.clear();
01551 line_strip_marker_.colors.clear();
01552
01553 for (std::size_t i = 1; i < path.size(); ++i)
01554 {
01555
01556 line_strip_marker_.points.push_back(path[i - 1]);
01557 line_strip_marker_.points.push_back(path[i]);
01558 line_strip_marker_.colors.push_back(this_color);
01559 line_strip_marker_.colors.push_back(this_color);
01560 }
01561
01562
01563 return publishMarker(line_strip_marker_);
01564 }
01565
01566 bool RvizVisualTools::publishPath(const std::vector<Eigen::Vector3d> &path, const colors &color, const double radius,
01567 const std::string &ns)
01568 {
01569 if (path.size() < 2)
01570 {
01571 ROS_WARN_STREAM_NAMED(name_, "Skipping path because " << path.size() << " points passed in.");
01572 return true;
01573 }
01574
01575
01576 enableInternalBatchPublishing(true);
01577
01578
01579 for (std::size_t i = 1; i < path.size(); ++i)
01580 {
01581 publishCylinder(path[i - 1], path[i], color, radius, ns);
01582 }
01583
01584
01585 return triggerInternalBatchPublishAndDisable();
01586 }
01587
01588 bool RvizVisualTools::publishPolygon(const geometry_msgs::Polygon &polygon, const colors &color, const scales &scale,
01589 const std::string &ns)
01590 {
01591 std::vector<geometry_msgs::Point> points;
01592 geometry_msgs::Point temp;
01593 geometry_msgs::Point first;
01594
01595
01596 for (std::size_t i = 0; i < polygon.points.size(); ++i)
01597 {
01598 temp.x = polygon.points[i].x;
01599 temp.y = polygon.points[i].y;
01600 temp.z = polygon.points[i].z;
01601 if (i == 0)
01602 first = temp;
01603 points.push_back(temp);
01604 }
01605 points.push_back(first);
01606
01607 publishPath(points, color, scale, ns);
01608 }
01609
01610 bool RvizVisualTools::publishWireframeCuboid(const Eigen::Affine3d &pose, double depth, double width, double height,
01611 const colors &color, const std::string &ns, const std::size_t &id)
01612 {
01613 Eigen::Vector3d min_point, max_point;
01614 min_point << -depth / 2, -width / 2, -height / 2;
01615 max_point << depth / 2, width / 2, height / 2;
01616 return publishWireframeCuboid(pose, min_point, max_point, color, ns, id);
01617 }
01618
01619 bool RvizVisualTools::publishWireframeCuboid(const Eigen::Affine3d &pose, const Eigen::Vector3d &min_point,
01620 const Eigen::Vector3d &max_point, const colors &color,
01621 const std::string &ns, const std::size_t &id)
01622 {
01623
01624 Eigen::Vector3d p1(min_point[0], min_point[1], min_point[2]);
01625 Eigen::Vector3d p2(min_point[0], min_point[1], max_point[2]);
01626 Eigen::Vector3d p3(max_point[0], min_point[1], max_point[2]);
01627 Eigen::Vector3d p4(max_point[0], min_point[1], min_point[2]);
01628 Eigen::Vector3d p5(min_point[0], max_point[1], min_point[2]);
01629 Eigen::Vector3d p6(min_point[0], max_point[1], max_point[2]);
01630 Eigen::Vector3d p7(max_point[0], max_point[1], max_point[2]);
01631 Eigen::Vector3d p8(max_point[0], max_point[1], min_point[2]);
01632
01633 p1 = pose * p1;
01634 p2 = pose * p2;
01635 p3 = pose * p3;
01636 p4 = pose * p4;
01637 p5 = pose * p5;
01638 p6 = pose * p6;
01639 p7 = pose * p7;
01640 p8 = pose * p8;
01641
01642
01643 line_list_marker_.header.stamp = ros::Time();
01644 line_list_marker_.ns = ns;
01645
01646 if (id == 0)
01647 line_list_marker_.id++;
01648 else
01649 line_list_marker_.id = id;
01650
01651 std_msgs::ColorRGBA this_color = getColor(color);
01652 line_list_marker_.scale = getScale(XXSMALL);
01653 line_list_marker_.color = this_color;
01654 line_list_marker_.points.clear();
01655 line_list_marker_.colors.clear();
01656
01657
01658 line_list_marker_.points.push_back(convertPoint(p1));
01659 line_list_marker_.points.push_back(convertPoint(p2));
01660 line_list_marker_.colors.push_back(this_color);
01661 line_list_marker_.colors.push_back(this_color);
01662
01663 line_list_marker_.points.push_back(convertPoint(p1));
01664 line_list_marker_.points.push_back(convertPoint(p4));
01665 line_list_marker_.colors.push_back(this_color);
01666 line_list_marker_.colors.push_back(this_color);
01667
01668 line_list_marker_.points.push_back(convertPoint(p1));
01669 line_list_marker_.points.push_back(convertPoint(p5));
01670 line_list_marker_.colors.push_back(this_color);
01671 line_list_marker_.colors.push_back(this_color);
01672
01673 line_list_marker_.points.push_back(convertPoint(p5));
01674 line_list_marker_.points.push_back(convertPoint(p6));
01675 line_list_marker_.colors.push_back(this_color);
01676 line_list_marker_.colors.push_back(this_color);
01677
01678 line_list_marker_.points.push_back(convertPoint(p5));
01679 line_list_marker_.points.push_back(convertPoint(p8));
01680 line_list_marker_.colors.push_back(this_color);
01681 line_list_marker_.colors.push_back(this_color);
01682
01683 line_list_marker_.points.push_back(convertPoint(p2));
01684 line_list_marker_.points.push_back(convertPoint(p6));
01685 line_list_marker_.colors.push_back(this_color);
01686 line_list_marker_.colors.push_back(this_color);
01687
01688 line_list_marker_.points.push_back(convertPoint(p6));
01689 line_list_marker_.points.push_back(convertPoint(p7));
01690 line_list_marker_.colors.push_back(this_color);
01691 line_list_marker_.colors.push_back(this_color);
01692
01693 line_list_marker_.points.push_back(convertPoint(p7));
01694 line_list_marker_.points.push_back(convertPoint(p8));
01695 line_list_marker_.colors.push_back(this_color);
01696 line_list_marker_.colors.push_back(this_color);
01697
01698 line_list_marker_.points.push_back(convertPoint(p2));
01699 line_list_marker_.points.push_back(convertPoint(p3));
01700 line_list_marker_.colors.push_back(this_color);
01701 line_list_marker_.colors.push_back(this_color);
01702
01703 line_list_marker_.points.push_back(convertPoint(p4));
01704 line_list_marker_.points.push_back(convertPoint(p8));
01705 line_list_marker_.colors.push_back(this_color);
01706 line_list_marker_.colors.push_back(this_color);
01707
01708 line_list_marker_.points.push_back(convertPoint(p3));
01709 line_list_marker_.points.push_back(convertPoint(p4));
01710 line_list_marker_.colors.push_back(this_color);
01711 line_list_marker_.colors.push_back(this_color);
01712
01713 line_list_marker_.points.push_back(convertPoint(p3));
01714 line_list_marker_.points.push_back(convertPoint(p7));
01715 line_list_marker_.colors.push_back(this_color);
01716 line_list_marker_.colors.push_back(this_color);
01717
01718
01719 return publishMarker(line_list_marker_);
01720 }
01721
01722 bool RvizVisualTools::publishWireframeRectangle(const Eigen::Affine3d &pose, const double &height, const double &width,
01723 const colors &color, const scales &scale, const std::size_t &id)
01724 {
01725 if (id == 0)
01726 line_list_marker_.id++;
01727 else
01728 line_list_marker_.id = id;
01729
01730
01731 Eigen::Vector3d p1(-width / 2.0, -height / 2.0, 0.0);
01732 Eigen::Vector3d p2(-width / 2.0, height / 2.0, 0.0);
01733 Eigen::Vector3d p3(width / 2.0, height / 2.0, 0.0);
01734 Eigen::Vector3d p4(width / 2.0, -height / 2.0, 0.0);
01735
01736 p1 = pose * p1;
01737 p2 = pose * p2;
01738 p3 = pose * p3;
01739 p4 = pose * p4;
01740
01741
01742 line_list_marker_.header.stamp = ros::Time();
01743 line_list_marker_.ns = "Wireframe Rectangle";
01744
01745 std_msgs::ColorRGBA this_color = getColor(color);
01746 line_list_marker_.scale = getScale(scale, false, 0.25);
01747 line_list_marker_.color = this_color;
01748 line_list_marker_.points.clear();
01749 line_list_marker_.colors.clear();
01750
01751
01752 line_list_marker_.points.push_back(convertPoint(p1));
01753 line_list_marker_.points.push_back(convertPoint(p2));
01754 line_list_marker_.colors.push_back(this_color);
01755 line_list_marker_.colors.push_back(this_color);
01756
01757 line_list_marker_.points.push_back(convertPoint(p2));
01758 line_list_marker_.points.push_back(convertPoint(p3));
01759 line_list_marker_.colors.push_back(this_color);
01760 line_list_marker_.colors.push_back(this_color);
01761
01762 line_list_marker_.points.push_back(convertPoint(p3));
01763 line_list_marker_.points.push_back(convertPoint(p4));
01764 line_list_marker_.colors.push_back(this_color);
01765 line_list_marker_.colors.push_back(this_color);
01766
01767 line_list_marker_.points.push_back(convertPoint(p4));
01768 line_list_marker_.points.push_back(convertPoint(p1));
01769 line_list_marker_.colors.push_back(this_color);
01770 line_list_marker_.colors.push_back(this_color);
01771
01772
01773 return publishMarker(line_list_marker_);
01774 }
01775
01776 bool RvizVisualTools::publishWireframeRectangle(const Eigen::Affine3d &pose, const Eigen::Vector3d &p1_in,
01777 const Eigen::Vector3d &p2_in, const Eigen::Vector3d &p3_in,
01778 const Eigen::Vector3d &p4_in, const colors &color, const scales &scale)
01779 {
01780 Eigen::Vector3d p1;
01781 Eigen::Vector3d p2;
01782 Eigen::Vector3d p3;
01783 Eigen::Vector3d p4;
01784
01785
01786 p1 = pose * p1_in;
01787 p2 = pose * p2_in;
01788 p3 = pose * p3_in;
01789 p4 = pose * p4_in;
01790
01791
01792 line_list_marker_.header.stamp = ros::Time();
01793 line_list_marker_.ns = "Wireframe Rectangle";
01794
01795
01796 line_list_marker_.id++;
01797
01798 std_msgs::ColorRGBA this_color = getColor(color);
01799 line_list_marker_.scale = getScale(scale, false, 0.25);
01800 line_list_marker_.color = this_color;
01801 line_list_marker_.points.clear();
01802 line_list_marker_.colors.clear();
01803
01804
01805 line_list_marker_.points.push_back(convertPoint(p1));
01806 line_list_marker_.points.push_back(convertPoint(p2));
01807 line_list_marker_.colors.push_back(this_color);
01808 line_list_marker_.colors.push_back(this_color);
01809
01810 line_list_marker_.points.push_back(convertPoint(p2));
01811 line_list_marker_.points.push_back(convertPoint(p3));
01812 line_list_marker_.colors.push_back(this_color);
01813 line_list_marker_.colors.push_back(this_color);
01814
01815 line_list_marker_.points.push_back(convertPoint(p3));
01816 line_list_marker_.points.push_back(convertPoint(p4));
01817 line_list_marker_.colors.push_back(this_color);
01818 line_list_marker_.colors.push_back(this_color);
01819
01820 line_list_marker_.points.push_back(convertPoint(p4));
01821 line_list_marker_.points.push_back(convertPoint(p1));
01822 line_list_marker_.colors.push_back(this_color);
01823 line_list_marker_.colors.push_back(this_color);
01824
01825
01826 return publishMarker(line_list_marker_);
01827 }
01828
01829 bool RvizVisualTools::publishSpheres(const std::vector<Eigen::Vector3d> &points, const colors &color,
01830 const double scale, const std::string &ns)
01831 {
01832 std::vector<geometry_msgs::Point> points_msg;
01833
01834
01835 for (std::size_t i = 0; i < points.size(); ++i)
01836 {
01837
01838 points_msg.push_back(convertPoint(points[i]));
01839 }
01840
01841 return publishSpheres(points_msg, color, scale, ns);
01842 }
01843
01844 bool RvizVisualTools::publishSpheres(const std::vector<geometry_msgs::Point> &points, const colors &color,
01845 const double scale, const std::string &ns)
01846 {
01847 geometry_msgs::Vector3 scale_vector;
01848 scale_vector.x = scale;
01849 scale_vector.y = scale;
01850 scale_vector.z = scale;
01851 publishSpheres(points, color, scale_vector, ns);
01852 }
01853
01854 bool RvizVisualTools::publishSpheres(const std::vector<geometry_msgs::Point> &points, const colors &color,
01855 const scales &scale, const std::string &ns)
01856 {
01857 publishSpheres(points, color, getScale(scale, false, 0.25), ns);
01858 }
01859
01860 bool RvizVisualTools::publishSpheres(const std::vector<geometry_msgs::Point> &points, const colors &color,
01861 const geometry_msgs::Vector3 &scale, const std::string &ns)
01862 {
01863 spheres_marker_.header.stamp = ros::Time();
01864 spheres_marker_.ns = ns;
01865
01866
01867 spheres_marker_.id++;
01868
01869 std_msgs::ColorRGBA this_color = getColor(color);
01870 spheres_marker_.scale = scale;
01871 spheres_marker_.color = this_color;
01872 spheres_marker_.colors.clear();
01873
01874 spheres_marker_.points = points;
01875
01876
01877 for (std::size_t i = 0; i < points.size(); ++i)
01878 {
01879 spheres_marker_.colors.push_back(this_color);
01880 }
01881
01882
01883 return publishMarker(spheres_marker_);
01884 }
01885
01886 bool RvizVisualTools::publishText(const Eigen::Affine3d &pose, const std::string &text, const colors &color,
01887 const scales &scale, bool static_id)
01888 {
01889 return publishText(convertPose(pose), text, color, getScale(scale), static_id);
01890 }
01891
01892 bool RvizVisualTools::publishText(const Eigen::Affine3d &pose, const std::string &text, const colors &color,
01893 const geometry_msgs::Vector3 scale, bool static_id)
01894 {
01895 return publishText(convertPose(pose), text, color, scale, static_id);
01896 }
01897
01898 bool RvizVisualTools::publishText(const geometry_msgs::Pose &pose, const std::string &text, const colors &color,
01899 const scales &scale, bool static_id)
01900 {
01901 return publishText(pose, text, color, getScale(scale), static_id);
01902 }
01903
01904 bool RvizVisualTools::publishText(const geometry_msgs::Pose &pose, const std::string &text, const colors &color,
01905 const geometry_msgs::Vector3 scale, bool static_id)
01906 {
01907
01908 double temp_id = text_marker_.id;
01909 if (static_id)
01910 {
01911 text_marker_.id = 0;
01912 }
01913 else
01914 {
01915 text_marker_.id++;
01916 }
01917
01918 text_marker_.header.stamp = ros::Time::now();
01919 text_marker_.header.frame_id = base_frame_;
01920 text_marker_.text = text;
01921 text_marker_.pose = pose;
01922 text_marker_.color = getColor(color);
01923 text_marker_.scale = scale;
01924
01925
01926 publishMarker(text_marker_);
01927
01928
01929 if (static_id)
01930 text_marker_.id = temp_id;
01931
01932 return true;
01933 }
01934
01935 bool RvizVisualTools::publishTests()
01936 {
01937
01938 geometry_msgs::Pose pose1;
01939 geometry_msgs::Pose pose2;
01940
01941
01942 ROS_INFO_STREAM_NAMED(name_, "Publising range of colors red->green");
01943 generateRandomPose(pose1);
01944 for (double i = 0; i < 1.0; i += 0.01)
01945 {
01946
01947 geometry_msgs::Vector3 scale = getScale(XLARGE, false, 0.1);
01948 std_msgs::ColorRGBA color = getColorScale(i);
01949 std::size_t id = 1;
01950 publishSphere(pose1, color, scale, "Sphere", id);
01951 ros::Duration(0.1).sleep();
01952
01953 if (!ros::ok())
01954 return false;
01955 }
01956
01957
01958
01959 ROS_INFO_STREAM_NAMED(name_, "Publishing Axis");
01960 generateRandomPose(pose1);
01961 publishAxis(pose1);
01962 ros::Duration(1.0).sleep();
01963
01964 ROS_INFO_STREAM_NAMED(name_, "Publishing Arrow");
01965 generateRandomPose(pose1);
01966 publishArrow(pose1, rviz_visual_tools::RAND);
01967 ros::Duration(1.0).sleep();
01968
01969 ROS_INFO_STREAM_NAMED(name_, "Publishing Sphere");
01970 generateRandomPose(pose1);
01971 publishSphere(pose1, rviz_visual_tools::RAND);
01972 ros::Duration(1.0).sleep();
01973
01974 ROS_INFO_STREAM_NAMED(name_, "Publishing Rectangular Cuboid");
01975
01976 generateRandomPose(pose1);
01977 generateRandomPose(pose2);
01978 publishCuboid(pose1.position, pose2.position, rviz_visual_tools::RAND);
01979 ros::Duration(1.0).sleep();
01980
01981 ROS_INFO_STREAM_NAMED(name_, "Publishing Line");
01982 generateRandomPose(pose1);
01983 generateRandomPose(pose2);
01984 publishLine(pose1.position, pose2.position, rviz_visual_tools::RAND);
01985 ros::Duration(1.0).sleep();
01986
01987
01988
01989
01990
01991
01992
01993 ROS_INFO_STREAM_NAMED(name_, "Publishing Cylinder");
01994 generateRandomPose(pose1);
01995 publishCylinder(pose1, rviz_visual_tools::RAND);
01996 ros::Duration(1.0).sleep();
01997
01998 ROS_INFO_STREAM_NAMED(name_, "Publishing Text");
01999 generateRandomPose(pose1);
02000 publishText(pose1, "Test", rviz_visual_tools::RAND);
02001 ros::Duration(1.0).sleep();
02002
02003 ROS_INFO_STREAM_NAMED(name_, "Publishing Wireframe Cuboid");
02004 Eigen::Vector3d min_point, max_point;
02005
02006 min_point << -0.1, -.25, -.3;
02007 max_point << .3, .2, .1;
02008 generateRandomPose(pose1);
02009 publishWireframeCuboid(convertPose(pose1), min_point, max_point);
02010 ros::Duration(1.0).sleep();
02011
02012 ROS_INFO_STREAM_NAMED(name_, "Publishing depth/width/height Wireframe Cuboid");
02013 double depth = 0.5, width = 0.25, height = 0.125;
02014 generateRandomPose(pose1);
02015 publishWireframeCuboid(convertPose(pose1), depth, width, height);
02016 ros::Duration(1.0).sleep();
02017
02018 ROS_INFO_STREAM_NAMED(name_, "Publishing Planes");
02019 generateRandomPose(pose1);
02020 publishXYPlane(pose1, rviz_visual_tools::RED, 0.1);
02021 ros::Duration(1.0).sleep();
02022 publishXZPlane(pose1, rviz_visual_tools::GREEN, 0.1);
02023 ros::Duration(1.0).sleep();
02024 publishYZPlane(pose1, rviz_visual_tools::BLUE, 0.1);
02025 ros::Duration(1.0).sleep();
02026
02027 ROS_INFO_STREAM_NAMED(name_, "Publising Axis Cone");
02028 generateRandomPose(pose1);
02029 publishCone(pose1, M_PI / 6.0, rviz_visual_tools::RAND, 0.2);
02030 ros::Duration(1.0).sleep();
02031
02032 return true;
02033 }
02034
02035 geometry_msgs::Pose RvizVisualTools::convertPose(const Eigen::Affine3d &pose)
02036 {
02037 tf::poseEigenToMsg(pose, shared_pose_msg_);
02038 return shared_pose_msg_;
02039 }
02040
02041 void RvizVisualTools::convertPoseSafe(const Eigen::Affine3d &pose, geometry_msgs::Pose &pose_msg)
02042 {
02043 tf::poseEigenToMsg(pose, pose_msg);
02044 }
02045
02046 Eigen::Affine3d RvizVisualTools::convertPose(const geometry_msgs::Pose &pose)
02047 {
02048 tf::poseMsgToEigen(pose, shared_pose_eigen_);
02049 return shared_pose_eigen_;
02050 }
02051
02052 Eigen::Affine3d RvizVisualTools::convertPoint32ToPose(const geometry_msgs::Point32 &point)
02053 {
02054 shared_pose_eigen_ = Eigen::Affine3d::Identity();
02055 shared_pose_eigen_.translation().x() = point.x;
02056 shared_pose_eigen_.translation().y() = point.y;
02057 shared_pose_eigen_.translation().z() = point.z;
02058 return shared_pose_eigen_;
02059 }
02060
02061 geometry_msgs::Pose RvizVisualTools::convertPointToPose(const geometry_msgs::Point &point)
02062 {
02063 shared_pose_msg_.orientation.x = 0.0;
02064 shared_pose_msg_.orientation.y = 0.0;
02065 shared_pose_msg_.orientation.z = 0.0;
02066 shared_pose_msg_.orientation.w = 1.0;
02067 shared_pose_msg_.position = point;
02068 return shared_pose_msg_;
02069 }
02070
02071 Eigen::Affine3d RvizVisualTools::convertPointToPose(const Eigen::Vector3d &point)
02072 {
02073 shared_pose_eigen_ = Eigen::Affine3d::Identity();
02074 shared_pose_eigen_.translation() = point;
02075 return shared_pose_eigen_;
02076 }
02077
02078 geometry_msgs::Point RvizVisualTools::convertPoseToPoint(const Eigen::Affine3d &pose)
02079 {
02080 tf::poseEigenToMsg(pose, shared_pose_msg_);
02081 return shared_pose_msg_.position;
02082 }
02083
02084 Eigen::Vector3d RvizVisualTools::convertPoint(const geometry_msgs::Point &point)
02085 {
02086 shared_point_eigen_[0] = point.x;
02087 shared_point_eigen_[1] = point.y;
02088 shared_point_eigen_[2] = point.z;
02089 return shared_point_eigen_;
02090 }
02091
02092 Eigen::Vector3d RvizVisualTools::convertPoint32(const geometry_msgs::Point32 &point)
02093 {
02094 shared_point_eigen_[0] = point.x;
02095 shared_point_eigen_[1] = point.y;
02096 shared_point_eigen_[2] = point.z;
02097 return shared_point_eigen_;
02098 }
02099
02100 geometry_msgs::Point32 RvizVisualTools::convertPoint32(const Eigen::Vector3d &point)
02101 {
02102 shared_point32_msg_.x = point[0];
02103 shared_point32_msg_.y = point[1];
02104 shared_point32_msg_.z = point[2];
02105 return shared_point32_msg_;
02106 }
02107
02108 geometry_msgs::Point RvizVisualTools::convertPoint(const geometry_msgs::Vector3 &point)
02109 {
02110 shared_point_msg_.x = point.x;
02111 shared_point_msg_.y = point.y;
02112 shared_point_msg_.z = point.z;
02113 return shared_point_msg_;
02114 }
02115
02116 geometry_msgs::Point RvizVisualTools::convertPoint(const Eigen::Vector3d &point)
02117 {
02118 shared_point_msg_.x = point.x();
02119 shared_point_msg_.y = point.y();
02120 shared_point_msg_.z = point.z();
02121 return shared_point_msg_;
02122 }
02123
02124 Eigen::Affine3d RvizVisualTools::convertFromXYZRPY(std::vector<double> transform6)
02125 {
02126 if (transform6.size() != 6)
02127 {
02128
02129 ROS_ERROR_STREAM_NAMED("visual_tools", "Incorrect number of variables passed for 6-size transform");
02130 throw;
02131 }
02132
02133 return convertFromXYZRPY(transform6[0], transform6[1], transform6[2], transform6[3], transform6[4], transform6[5]);
02134 }
02135
02136 Eigen::Affine3d RvizVisualTools::convertFromXYZRPY(const double &x, const double &y, const double &z,
02137 const double &roll, const double &pitch, const double &yaw)
02138 {
02139
02140 Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX());
02141 Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());
02142 Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());
02143 Eigen::Quaternion<double> quaternion = roll_angle * pitch_angle * yaw_angle;
02144
02145 return Eigen::Translation3d(x, y, z) * quaternion;
02146 }
02147
02148 void RvizVisualTools::convertToXYZRPY(const Eigen::Affine3d &pose, std::vector<double> &xyzrpy)
02149 {
02150 xyzrpy.resize(6);
02151 convertToXYZRPY(pose, xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4], xyzrpy[5]);
02152 }
02153
02154 void RvizVisualTools::convertToXYZRPY(const Eigen::Affine3d &pose, double &x, double &y, double &z, double &roll,
02155 double &pitch, double &yaw)
02156 {
02157 x = pose(0, 3);
02158 y = pose(1, 3);
02159 z = pose(2, 3);
02160
02161
02162 Eigen::Vector3d vec = pose.rotation().eulerAngles(0, 1, 2);
02163 roll = vec[0];
02164 pitch = vec[1];
02165 yaw = vec[2];
02166 }
02167
02168 void RvizVisualTools::generateRandomPose(geometry_msgs::Pose &pose, RandomPoseBounds pose_bounds)
02169 {
02170 generateRandomPose(shared_pose_eigen_, pose_bounds);
02171 pose = convertPose(shared_pose_eigen_);
02172 }
02173
02174 void RvizVisualTools::generateRandomCuboid(geometry_msgs::Pose &cuboid_pose, double &depth, double &width,
02175 double &height, RandomPoseBounds pose_bounds,
02176 RandomCuboidBounds cuboid_bounds)
02177 {
02178
02179 depth = fRand(cuboid_bounds.cuboid_size_min_, cuboid_bounds.cuboid_size_max_);
02180 width = fRand(cuboid_bounds.cuboid_size_min_, cuboid_bounds.cuboid_size_max_);
02181 height = fRand(cuboid_bounds.cuboid_size_min_, cuboid_bounds.cuboid_size_max_);
02182
02183
02184 generateRandomPose(cuboid_pose, pose_bounds);
02185 }
02186
02187 void RvizVisualTools::generateRandomPose(Eigen::Affine3d &pose, RandomPoseBounds pose_bounds)
02188 {
02189
02190
02191
02192 if (pose_bounds.elevation_min_ < 0)
02193 {
02194 ROS_WARN_STREAM_NAMED(name_, "min elevation bound < 0, setting equal to 0");
02195 pose_bounds.elevation_min_ = 0;
02196 }
02197
02198 if (pose_bounds.elevation_max_ > M_PI)
02199 {
02200 ROS_WARN_STREAM_NAMED(name_, "max elevation bound > pi, setting equal to pi ");
02201 pose_bounds.elevation_max_ = M_PI;
02202 }
02203
02204 if (pose_bounds.azimuth_min_ < 0)
02205 {
02206 ROS_WARN_STREAM_NAMED(name_, "min azimuth bound < 0, setting equal to 0");
02207 pose_bounds.azimuth_min_ = 0;
02208 }
02209
02210 if (pose_bounds.azimuth_max_ > 2 * M_PI)
02211 {
02212 ROS_WARN_STREAM_NAMED(name_, "max azimuth bound > 2 pi, setting equal to 2 pi ");
02213 pose_bounds.azimuth_max_ = 2 * M_PI;
02214 }
02215
02216
02217 pose.translation().x() = dRand(pose_bounds.x_min_, pose_bounds.x_max_);
02218 pose.translation().y() = dRand(pose_bounds.y_min_, pose_bounds.y_max_);
02219 pose.translation().z() = dRand(pose_bounds.z_min_, pose_bounds.z_max_);
02220
02221
02222 double angle = dRand(pose_bounds.angle_min_, pose_bounds.angle_max_);
02223 double elevation = dRand(pose_bounds.elevation_min_, pose_bounds.elevation_max_);
02224 double azimuth = dRand(pose_bounds.azimuth_min_, pose_bounds.azimuth_max_);
02225
02226 Eigen::Vector3d axis;
02227 axis[0] = sin(elevation) * cos(azimuth);
02228 axis[1] = sin(elevation) * sin(azimuth);
02229 axis[2] = cos(elevation);
02230
02231 Eigen::Quaterniond quaternion(Eigen::AngleAxis<double>(static_cast<double>(angle), axis));
02232 pose = Eigen::Translation3d(pose.translation().x(), pose.translation().y(), pose.translation().z()) * quaternion;
02233 }
02234
02235 void RvizVisualTools::generateEmptyPose(geometry_msgs::Pose &pose)
02236 {
02237
02238 pose.position.x = 0;
02239 pose.position.y = 0;
02240 pose.position.z = 0;
02241
02242
02243 pose.orientation.x = 0;
02244 pose.orientation.y = 0;
02245 pose.orientation.z = 0;
02246 pose.orientation.w = 1;
02247 }
02248
02249 bool RvizVisualTools::posesEqual(const Eigen::Affine3d &pose1, const Eigen::Affine3d &pose2, const double &threshold)
02250 {
02251 static const std::size_t NUM_VARS = 16;
02252
02253 for (std::size_t i = 0; i < NUM_VARS; ++i)
02254 {
02255 if (fabs(pose1.data()[i] - pose2.data()[i]) > threshold)
02256 {
02257 return false;
02258 }
02259 }
02260
02261 return true;
02262 }
02263
02264 double RvizVisualTools::dRand(double dMin, double dMax)
02265 {
02266 double d = static_cast<double>(rand()) / RAND_MAX;
02267 return dMin + d * (dMax - dMin);
02268 }
02269
02270 float RvizVisualTools::fRand(float dMin, float dMax)
02271 {
02272 float d = static_cast<float>(rand()) / RAND_MAX;
02273 return dMin + d * (dMax - dMin);
02274 }
02275
02276 int RvizVisualTools::iRand(int min, int max)
02277 {
02278 int n = max - min + 1;
02279 int remainder = RAND_MAX % n;
02280 int x;
02281 do
02282 {
02283 x = rand();
02284 } while (x >= RAND_MAX - remainder);
02285 return min + x % n;
02286 }
02287
02288 void RvizVisualTools::enableInternalBatchPublishing(bool enable)
02289 {
02290
02291 if (batch_publishing_enabled_)
02292 {
02293 return;
02294 }
02295 internal_batch_publishing_enabled_ = true;
02296 }
02297
02298 bool RvizVisualTools::triggerInternalBatchPublishAndDisable()
02299 {
02300 internal_batch_publishing_enabled_ = false;
02301
02302 bool result = publishMarkers(markers_);
02303
02304 markers_.markers.clear();
02305 return result;
02306 }
02307
02308 void RvizVisualTools::printTransform(const Eigen::Affine3d &transform)
02309 {
02310 Eigen::Quaterniond q(transform.rotation());
02311 std::cout << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", "
02312 << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", "
02313 << q.w() << "]" << std::endl;
02314 }
02315
02316 void RvizVisualTools::printTransformRPY(const Eigen::Affine3d &transform)
02317 {
02318 double x, y, z, r, p, yaw;
02319 convertToXYZRPY(transform, x, y, z, r, p, yaw);
02320 std::cout << "transform: [" << x << ", " << y << ", " << z << ", " << r << ", " << p << ", " << yaw << "]\n";
02321 }
02322
02323 }