trajectory_stats.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, 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  *********************************************************************/
00036 
00037 /* \author: Ken Anderson */
00038 
00039 #include <cmath>
00040 #include <ros/ros.h>
00041 #include <trajectory_execution_monitor/trajectory_stats.h>
00042 
00043 using namespace std;
00044 using namespace trajectory_execution_monitor;
00045 
00046 ros::Duration TrajectoryStats::getDuration(const trajectory_msgs::JointTrajectory& trajectory)
00047 {
00048   size_t tsize = trajectory.points.size();
00049   if( tsize < 1 )
00050   {
00051     return ros::Duration(0,0);
00052   }
00053 
00054   const trajectory_msgs::JointTrajectoryPoint& point1 = trajectory.points[0];
00055   const trajectory_msgs::JointTrajectoryPoint& point2 = trajectory.points[tsize-1];
00056   return point2.time_from_start - point1.time_from_start;
00057 }
00058 
00059 double TrajectoryStats::getAngularDistance(const trajectory_msgs::JointTrajectory& trajectory, unsigned int start_index )
00060 {
00061   double angular_diff_sum = 0.0;
00062   size_t tsize = trajectory.points.size();
00063 
00064   // Loop through trajectory points
00065   if( tsize > 1 )
00066   {
00067     for(unsigned int i=start_index; i<tsize-1; i++)
00068     {
00069       trajectory_msgs::JointTrajectoryPoint point1 = trajectory.points[i];
00070       trajectory_msgs::JointTrajectoryPoint point2 = trajectory.points[i+1];
00071 
00072       if(point1.positions.size() != point2.positions.size())
00073       {
00074         ROS_ERROR_STREAM("Invalid Trajectory, the number of joints is inconsistent");
00075         return 0.0;
00076       }
00077 
00078       angular_diff_sum += distance(point1,point2);
00079     }
00080   }
00081 
00082   return angular_diff_sum;
00083 }
00084 
00085 double TrajectoryStats::getMaxAngularVelocity(const trajectory_msgs::JointTrajectory& trajectory, unsigned int start_index )
00086 {
00087   double max_angular_vel = 0.0;
00088   size_t tsize = trajectory.points.size();
00089 
00090   // Loop through trajectory points
00091   if( tsize > 1 )
00092   {
00093     for(unsigned int i=start_index; i<tsize-1; i++)
00094     {
00095       trajectory_msgs::JointTrajectoryPoint point1 = trajectory.points[i];
00096       trajectory_msgs::JointTrajectoryPoint point2 = trajectory.points[i+1];
00097 
00098       if(point1.positions.size() != point2.positions.size())
00099       {
00100         ROS_ERROR_STREAM("Invalid Trajectory, the number of joints is inconsistent");
00101         return 0.0;
00102       }
00103 
00104       const double seconds = point2.time_from_start.toSec() - point1.time_from_start.toSec();
00105       const double dist = distance(point1,point2);
00106       const double velocity = std::abs( dist/seconds );
00107 
00108       if( velocity > max_angular_vel )
00109       {
00110         max_angular_vel = velocity;
00111       }
00112     }
00113   }
00114 
00115   return max_angular_vel;
00116 }
00117 
00118 double TrajectoryStats::distance(
00119   const trajectory_msgs::JointTrajectoryPoint& point1,
00120   const trajectory_msgs::JointTrajectoryPoint& point2,
00121   const std::vector<std::string> joint_names
00122 )
00123 {
00124   double total_distance = 0.0;
00125   for(unsigned int i = 0; i < point1.positions.size(); i++) {
00126     if(joint_names.size()>i)
00127     {
00128       ROS_DEBUG_STREAM("Distance for " << joint_names[i] << " is " << fabs(point1.positions[i]-point2.positions[i]));
00129     }
00130     total_distance += fabs(point1.positions[i]-point2.positions[i]);
00131   }
00132   return total_distance;
00133 }


trajectory_execution_monitor
Author(s): Gil Jones
autogenerated on Thu Dec 12 2013 11:08:01