rviz_visual_tools_demo.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2016, University of Colorado, Boulder
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Univ of CO, Boulder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman
00036    Desc:   Demo implementation of rviz_visual_tools
00037            To use, add a Rviz Marker Display subscribed to topic /rviz_visual_tools
00038 */
00039 
00040 // ROS
00041 #include <ros/ros.h>
00042 
00043 // For visualizing things in rviz
00044 #include <rviz_visual_tools/rviz_visual_tools.h>
00045 
00046 namespace rvt = rviz_visual_tools;
00047 
00048 namespace rviz_visual_tools
00049 {
00050 class RvizVisualToolsDemo
00051 {
00052 private:
00053   // A shared node handle
00054   ros::NodeHandle nh_;
00055 
00056   // For visualizing things in rviz
00057   rvt::RvizVisualToolsPtr visual_tools_;
00058 
00059   std::string name_;
00060 
00061 public:
00065   RvizVisualToolsDemo()
00066     : name_("rviz_demo")
00067   {
00068     visual_tools_.reset(new rvt::RvizVisualTools("base", "/rviz_visual_tools"));
00069 
00070     ROS_INFO("Sleeping 5 seconds before running demo");
00071     ros::Duration(5.0).sleep();
00072 
00073     // Clear messages
00074     visual_tools_->deleteAllMarkers();
00075     visual_tools_->enableBatchPublishing();
00076   }
00077 
00078   void publishLabelHelper(const Eigen::Affine3d& pose, const std::string& label)
00079   {
00080     Eigen::Affine3d pose_copy = pose;
00081     pose_copy.translation().x() -= 0.2;
00082     visual_tools_->publishText(pose_copy, label, rvt::WHITE, rvt::REGULAR, false);
00083   }
00084 
00085   void runTests()
00086   {
00087     // Create pose
00088     Eigen::Affine3d pose1 = Eigen::Affine3d::Identity();
00089     Eigen::Affine3d pose2 = Eigen::Affine3d::Identity();
00090 
00091     double space_between_rows = 0.2;
00092     double y = 0;
00093 
00094     // --------------------------------------------------------------------
00095     ROS_INFO_STREAM_NAMED(name_, "Displaying range of colors red->green");
00096     double step = 0.02;
00097     for (double i = 0; i <= 1.0; i += 0.02)
00098     {
00099       geometry_msgs::Vector3 scale = visual_tools_->getScale(XLARGE, false, 0.05);
00100       std_msgs::ColorRGBA color = visual_tools_->getColorScale(i);
00101       visual_tools_->publishSphere(visual_tools_->convertPose(pose1), color, scale, "Sphere");
00102       if (!i)
00103         publishLabelHelper(pose1, "Sphere Color Range");
00104       pose1.translation().x() += step;
00105     }
00106     visual_tools_->triggerBatchPublish();
00107 
00108     // --------------------------------------------------------------------
00109     ROS_INFO_STREAM_NAMED(name_, "Displaying Coordinate Axis");
00110     pose1.translation().x() = 0;
00111     y += space_between_rows;
00112     pose1.translation().y() = y;
00113     step = 0.025;
00114     for (double i = 0; i <= 1.0; i += step)
00115     {
00116       visual_tools_->publishAxis(pose1);
00117       if (!i)
00118         publishLabelHelper(pose1, "Coordinate Axis");
00119 
00120       pose1.translation().x() += step;
00121       pose1 = pose1 * Eigen::AngleAxisd(step*2*M_PI, Eigen::Vector3d::UnitX())
00122         * Eigen::AngleAxisd(step*2*M_PI, Eigen::Vector3d::UnitY())
00123         * Eigen::AngleAxisd(step*2*M_PI, Eigen::Vector3d::UnitZ());
00124     }
00125     visual_tools_->triggerBatchPublish();
00126 
00127 
00128     // --------------------------------------------------------------------
00129     ROS_INFO_STREAM_NAMED(name_, "Displaying Arrows");
00130     pose1 = Eigen::Affine3d::Identity();
00131     y += space_between_rows;
00132     pose1.translation().y() = y;
00133     step = 0.025;
00134     for (double i = 0; i <= 1.0; i += step)
00135     {
00136       visual_tools_->publishArrow(pose1, rvt::RAND);
00137       if (!i)
00138         publishLabelHelper(pose1, "Arrows");
00139 
00140       pose1.translation().x() += step;
00141       pose1 = pose1 * Eigen::AngleAxisd(step*2*M_PI, Eigen::Vector3d::UnitZ());
00142     }
00143     visual_tools_->triggerBatchPublish();
00144 
00145 
00146     // --------------------------------------------------------------------
00147     ROS_INFO_STREAM_NAMED(name_, "Displaying Rectangular Cuboid");
00148     double cuboid_max_size = 0.075;
00149     double cuboid_min_size = 0.01;
00150     pose1 = Eigen::Affine3d::Identity();
00151     pose2 = Eigen::Affine3d::Identity();
00152     y += space_between_rows;
00153     pose1.translation().y() = y;
00154     pose2.translation().y() = y;
00155     step = 0.1;
00156     for (double i = 0; i <= 1.0; i += step)
00157     {
00158       pose2 = pose1;
00159       pose2.translation().x() += i * cuboid_max_size + cuboid_min_size;
00160       pose2.translation().y() += i * cuboid_max_size + cuboid_min_size;
00161       pose2.translation().z() += i * cuboid_max_size + cuboid_min_size;
00162       visual_tools_->publishCuboid(pose1.translation(), pose2.translation(), rvt::RAND);
00163 
00164       if (!i)
00165         publishLabelHelper(pose1, "Cuboid");
00166 
00167       pose1.translation().x() += step;
00168     }
00169     visual_tools_->triggerBatchPublish();
00170 
00171     // --------------------------------------------------------------------
00172     ROS_INFO_STREAM_NAMED(name_, "Displaying Lines");
00173     double line_max_size = 0.075;
00174     double line_min_size = 0.01;
00175     pose1 = Eigen::Affine3d::Identity();
00176     pose2 = Eigen::Affine3d::Identity();
00177     y += space_between_rows;
00178     pose1.translation().y() = y;
00179     pose2.translation().y() = y;
00180     step = 0.1;
00181     for (double i = 0; i <= 1.0; i += step)
00182     {
00183       pose2 = pose1;
00184       pose2.translation().x() += i * line_max_size + line_min_size;
00185       pose2.translation().y() += i * line_max_size + line_min_size;
00186       pose2.translation().z() += i * line_max_size + line_min_size;
00187       visual_tools_->publishLine(pose1.translation(), pose2.translation(), rvt::RAND);
00188 
00189       if (!i)
00190         publishLabelHelper(pose1, "Line");
00191 
00192       pose1.translation().x() += step;
00193     }
00194     visual_tools_->triggerBatchPublish();
00195 
00196 
00197     // --------------------------------------------------------------------
00198     ROS_INFO_STREAM_NAMED(name_, "Displaying Cylinder");
00199     pose1 = Eigen::Affine3d::Identity();
00200     y += space_between_rows;
00201     pose1.translation().y() = y;
00202     step = 0.025;
00203     for (double i = 0; i <= 1.0; i += step)
00204     {
00205       visual_tools_->publishCylinder(pose1, rvt::RAND);
00206       if (!i)
00207         publishLabelHelper(pose1, "Cylinder");
00208 
00209       pose1.translation().x() += step;
00210       pose1 = pose1 * Eigen::AngleAxisd(step*2*M_PI, Eigen::Vector3d::UnitZ());
00211     }
00212     visual_tools_->triggerBatchPublish();
00213 
00214 
00215     // --------------------------------------------------------------------
00216     ROS_INFO_STREAM_NAMED(name_, "Displaying Axis Cone");
00217     pose1 = Eigen::Affine3d::Identity();
00218     y += space_between_rows;
00219     pose1.translation().y() = y;
00220     step = 0.025;
00221     for (double i = 0; i <= 1.0; i += step)
00222     {
00223       visual_tools_->publishCone(pose1, M_PI / 6.0, rvt::RAND, 0.05);
00224       if (!i)
00225         publishLabelHelper(pose1, "Cone");
00226 
00227       pose1.translation().x() += step;
00228       pose1 = pose1 * Eigen::AngleAxisd(step*2*M_PI, Eigen::Vector3d::UnitZ());
00229     }
00230     visual_tools_->triggerBatchPublish();
00231 
00232 
00233     // --------------------------------------------------------------------
00234     ROS_INFO_STREAM_NAMED(name_, "Displaying Wireframe Cuboid");
00235     pose1 = Eigen::Affine3d::Identity();
00236     y += space_between_rows;
00237     pose1.translation().y() = y;
00238     step = 0.1;
00239     // TODO(davetcoleman): use generateRandomCuboid()
00240     Eigen::Vector3d min_point, max_point;
00241     min_point << -0.05, -0.05, -0.05;
00242     max_point <<  0.05,  0.05,  0.05;
00243     for (double i = 0; i <= 1.0; i += step)
00244     {
00245       visual_tools_->publishWireframeCuboid(pose1, min_point, max_point, rvt::RAND);
00246       if (!i)
00247         publishLabelHelper(pose1, "Wireframe Cuboid");
00248 
00249       pose1.translation().x() += step;
00250       pose1 = pose1 * Eigen::AngleAxisd(step*2*M_PI, Eigen::Vector3d::UnitZ());
00251     }
00252     visual_tools_->triggerBatchPublish();
00253 
00254     // --------------------------------------------------------------------
00255     ROS_INFO_STREAM_NAMED(name_, "Displaying Sized Wireframe Cuboid");
00256     pose1 = Eigen::Affine3d::Identity();
00257     y += space_between_rows;
00258     pose1.translation().y() = y;
00259     step = 0.1;
00260     double depth = 0.05, width = 0.05, height = 0.05;
00261     for (double i = 0; i <= 1.0; i += step)
00262     {
00263       visual_tools_->publishWireframeCuboid(pose1, depth, width, height, rvt::RAND);
00264       if (!i)
00265         publishLabelHelper(pose1, "Wireframe Cuboid");
00266 
00267       pose1.translation().x() += step;
00268       pose1 = pose1 * Eigen::AngleAxisd(step*2*M_PI, Eigen::Vector3d::UnitZ());
00269     }
00270     visual_tools_->triggerBatchPublish();
00271 
00272     // --------------------------------------------------------------------
00273     ROS_INFO_STREAM_NAMED(name_, "Displaying Planes");
00274     pose1 = Eigen::Affine3d::Identity();
00275     y += space_between_rows;
00276     pose1.translation().y() = y;
00277     step = 0.2;
00278     double max_plane_size = 0.075;
00279     double min_plane_size = 0.01;
00280     for (double i = 0; i <= 1.0; i += step)
00281     {
00282       visual_tools_->publishXYPlane(pose1, rvt::RED,   i * max_plane_size + min_plane_size);
00283       visual_tools_->publishXZPlane(pose1, rvt::GREEN, i * max_plane_size + min_plane_size);
00284       visual_tools_->publishYZPlane(pose1, rvt::BLUE,  i * max_plane_size + min_plane_size);
00285       if (!i)
00286         publishLabelHelper(pose1, "Planes");
00287 
00288       pose1.translation().x() += step;
00289     }
00290     visual_tools_->triggerBatchPublish();
00291 
00292     // --------------------------------------------------------------------
00293     ROS_INFO_STREAM_NAMED(name_, "Displaying Graph");
00294     pose1 = Eigen::Affine3d::Identity();
00295     y += space_between_rows;
00296     pose1.translation().y() = y;
00297     step = 0.1;
00298     graph_msgs::GeometryGraph graph;
00299     for (double i = 0; i <= 1.0; i += step)
00300     {
00301       graph.nodes.push_back(visual_tools_->convertPose(pose1).position);
00302       graph_msgs::Edges edges;
00303       if (i > 0)
00304         edges.node_ids.push_back(0);
00305       graph.edges.push_back(edges);
00306 
00307       if (!i)
00308         publishLabelHelper(pose1, "Graph");
00309 
00310       pose1.translation().x() += step;
00311       pose1.translation().z() += visual_tools_->dRand(-0.1,0.1);
00312     }
00313     visual_tools_->publishGraph(graph, rvt::ORANGE, 0.005);
00314     visual_tools_->triggerBatchPublish();
00315 
00316 
00317 
00318     // --------------------------------------------------------------------
00319     // TODO publishMesh
00320 
00321     // --------------------------------------------------------------------
00322     // TODO publishPolygon
00323 
00324     // --------------------------------------------------------------------
00325     ROS_INFO_STREAM_NAMED(name_, "Displaying Labeled Coordinate Axis");
00326     pose1.translation().x() = 0;
00327     y += space_between_rows;
00328     pose1.translation().y() = y;
00329     pose1.translation().z() = 0;
00330     step = 0.2;
00331     for (double i = 0; i <= 1.0; i += step)
00332     {
00333       visual_tools_->publishAxisLabeled(pose1, "label of axis");
00334       if (!i)
00335         publishLabelHelper(pose1, "Labeled Coordinate Axis");
00336 
00337       pose1.translation().x() += step;
00338       pose1 = pose1 * Eigen::AngleAxisd(step*2*M_PI, Eigen::Vector3d::UnitX())
00339         * Eigen::AngleAxisd(step*2*M_PI, Eigen::Vector3d::UnitY())
00340         * Eigen::AngleAxisd(step*2*M_PI, Eigen::Vector3d::UnitZ());
00341     }
00342     visual_tools_->triggerBatchPublish();
00343 
00344 
00345   }
00346 
00350   ~RvizVisualToolsDemo() {}
00351 };  // end class
00352 
00353 }  // namespace rviz_visual_tools
00354 
00355 int main(int argc, char** argv)
00356 {
00357   ros::init(argc, argv, "visual_tools_demo");
00358   ROS_INFO_STREAM("Visual Tools Demo");
00359 
00360   // Allow the action server to recieve and send ros messages
00361   ros::AsyncSpinner spinner(1);
00362   spinner.start();
00363 
00364   rviz_visual_tools::RvizVisualToolsDemo demo;
00365   demo.runTests();
00366 
00367   ROS_INFO_STREAM("Shutting down.");
00368 
00369   return 0;
00370 }


rviz_visual_tools
Author(s): Dave Coleman
autogenerated on Thu Jun 6 2019 19:01:23