Go to the documentation of this file.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 <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
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
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 }