evaluate_state_operations_speed.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan */
00036 
00037 #include <moveit/robot_model_loader/robot_model_loader.h>
00038 #include <moveit/robot_state/robot_state.h>
00039 #include <moveit/profiler/profiler.h>
00040 #include <ros/ros.h>
00041 
00042 static const std::string ROBOT_DESCRIPTION = "robot_description";
00043 
00044 int main(int argc, char** argv)
00045 {
00046   ros::init(argc, argv, "evaluate_state_operations_speed");
00047 
00048   ros::AsyncSpinner spinner(1);
00049   spinner.start();
00050 
00051   robot_model_loader::RobotModelLoader rml(ROBOT_DESCRIPTION);
00052   ros::Duration(0.5).sleep();
00053 
00054   robot_model::RobotModelConstPtr robot_model = rml.getModel();
00055   if (robot_model)
00056   {
00057     static const int N = 10000;
00058     robot_state::RobotState state(robot_model);
00059 
00060     printf("Evaluating model '%s' using %d trials for each test\n", robot_model->getName().c_str(), N);
00061 
00062     moveit::tools::Profiler::Clear();
00063     moveit::tools::Profiler::Start();
00064 
00065     printf("Evaluating FK Default ...\n");
00066     for (int i = 0; i < N; ++i)
00067     {
00068       moveit::tools::Profiler::Begin("FK Default");
00069       state.setToDefaultValues();
00070       state.update();
00071       moveit::tools::Profiler::End("FK Default");
00072     }
00073 
00074     printf("Evaluating FK Random ...\n");
00075     for (int i = 0; i < N; ++i)
00076     {
00077       moveit::tools::Profiler::Begin("FK Random");
00078       state.setToRandomPositions();
00079       state.update();
00080       moveit::tools::Profiler::End("FK Random");
00081     }
00082 
00083     std::vector<robot_state::RobotState*> copies(N, (robot_state::RobotState*)NULL);
00084     printf("Evaluating Copy State ...\n");
00085     for (int i = 0; i < N; ++i)
00086     {
00087       moveit::tools::Profiler::Begin("Copy State");
00088       copies[i] = new robot_state::RobotState(state);
00089       moveit::tools::Profiler::End("Copy State");
00090     }
00091 
00092     printf("Evaluating Free State ...\n");
00093     for (int i = 0; i < N; ++i)
00094     {
00095       moveit::tools::Profiler::Begin("Free State");
00096       delete copies[i];
00097       moveit::tools::Profiler::End("Free State");
00098     }
00099 
00100     const std::vector<std::string>& groups = robot_model->getJointModelGroupNames();
00101     for (std::size_t j = 0; j < groups.size(); ++j)
00102     {
00103       printf("\n");
00104       const robot_model::JointModelGroup* jmg = robot_model->getJointModelGroup(groups[j]);
00105 
00106       printf("%s: Evaluating FK Random ...\n", groups[j].c_str());
00107       std::string pname = groups[j] + ":FK Random";
00108       for (int i = 0; i < N; ++i)
00109       {
00110         moveit::tools::Profiler::Begin(pname);
00111         state.setToRandomPositions(jmg);
00112         state.update();
00113         moveit::tools::Profiler::End(pname);
00114       }
00115     }
00116 
00117     moveit::tools::Profiler::Stop();
00118     moveit::tools::Profiler::Status();
00119   }
00120   else
00121     ROS_ERROR("Unable to initialize robot model.");
00122 
00123   ros::shutdown();
00124   return 0;
00125 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jun 19 2019 19:24:16