arm_navigation_tests: arm_navigation_tests: warehouse_metrics

00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the <ORGANIZATION> nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Author: E. Gil Jones
00031 
00032 #include <termios.h>
00033 #include <signal.h>
00034 #include <cstdio>
00035 #include <cstdlib>
00036 #include <unistd.h>
00037 #include <math.h>
00038 
00039 #include <ros/ros.h>
00040 
00041 #include <arm_navigation_msgs/PlanningScene.h>
00042 #include <planning_environment/models/model_utils.h>
00043 #include <trajectory_msgs/JointTrajectory.h>
00044 #include <move_arm_warehouse/move_arm_warehouse_logger_reader.h>
00045 #include <collision_proximity/collision_proximity_space.h>
00046 #include <arm_navigation_msgs/JointLimits.h>
00047 #include <spline_smoother/cubic_trajectory.h>
00048 
00049 struct TrajectoryMetrics {
00050 
00051   TrajectoryMetrics() :
00052     average_closest_collision_distance_(DBL_MAX),
00053     overall_closest_collision_distance_(DBL_MAX),
00054     total_path_length_(0.0),
00055     spline_time_(0.0),
00056     processing_time_(0.0)
00057   {
00058   }
00059 
00060   double average_closest_collision_distance_;
00061   double overall_closest_collision_distance_;
00062   double total_path_length_;
00063   double spline_time_;
00064   ros::Duration processing_time_;
00065 };
00066 
00067 struct MoveArmInstanceMetrics {
00068   std::map<std::string, TrajectoryMetrics> stage_metrics_;
00069   arm_navigation_msgs::ArmNavigationErrorCodes error_code_;
00070   std::string pipeline_stage_;
00071   bool has_path_constraints_;
00072 };
00073 
00074 struct TrajectorySourceMetrics {
00075 
00076   TrajectorySourceMetrics() :
00077     total_average_closest_collision_distance_(0.0), 
00078     total_overall_closest_collision_distance_(0.0), 
00079     total_total_path_length_(0.0),
00080     total_spline_time_(0.0),
00081     total_processing_time_(0.0)
00082   {
00083   }
00084 
00085   void addTrajectoryMetric(const TrajectoryMetrics& metric) {
00086     instance_metrics_.push_back(metric);
00087     total_spline_time_ += metric.spline_time_;
00088     total_average_closest_collision_distance_ += metric.average_closest_collision_distance_;
00089     total_overall_closest_collision_distance_ += metric.overall_closest_collision_distance_;
00090     total_total_path_length_ += metric.total_path_length_;
00091     total_processing_time_ += metric.processing_time_;
00092   }
00093 
00094   double getSizeAsDouble() const {
00095     return (instance_metrics_.size()*1.0);
00096   }
00097 
00098   std::vector<TrajectoryMetrics> instance_metrics_;
00099   std::vector<bool> has_path_constraints;
00100 
00101   double total_average_closest_collision_distance_;
00102   double total_overall_closest_collision_distance_;
00103   double total_total_path_length_;
00104   double total_spline_time_;
00105   ros::Duration total_processing_time_;
00106 };
00107 
00108 struct OverallMetrics {
00109 
00110   OverallMetrics() : 
00111     successes_(0),
00112     recoverable_errors_(0),
00113     invocations_(0)
00114   {}
00115 
00116   unsigned int successes_;
00117   unsigned int recoverable_errors_;
00118   unsigned int invocations_;
00119   
00120   std::map<std::string, unsigned int> failure_stage_;
00121 
00122   std::vector<MoveArmInstanceMetrics> move_arm_metric_vector_;
00123   std::map<std::string, TrajectorySourceMetrics> trajectory_source_metrics_;
00124 };
00125 
00126 class WarehouseMetricsEvaluator {
00127 
00128 public:
00129 
00130   WarehouseMetricsEvaluator() {
00131     move_arm_warehouse_reader_ = new move_arm_warehouse::MoveArmWarehouseLoggerReader();
00132 
00133     std::string robot_description_name = root_nh_.resolveName("robot_description", true);
00134 
00135     cps_ = new collision_proximity::CollisionProximitySpace(robot_description_name);
00136 
00137     vis_marker_publisher_ = root_nh_.advertise<visualization_msgs::Marker>("metrics_markers", 128);
00138     vis_marker_array_publisher_ = root_nh_.advertise<visualization_msgs::MarkerArray>("metrics_markers_array", 128);
00139   };
00140 
00141   ~WarehouseMetricsEvaluator()
00142   {
00143     delete move_arm_warehouse_reader_;
00144     delete cps_;
00145   }
00146 
00147   void getJointLimits(const std::vector<std::string>& joint_names, std::vector<arm_navigation_msgs::JointLimits>& limit_vec) 
00148   {
00149     limit_vec.clear();
00150     for(unsigned int i = 0; i < joint_names.size(); i++) {
00151       if(joint_limits_map_.find(joint_names[i]) == joint_limits_map_.end()) {
00152         arm_navigation_msgs::JointLimits limits;
00153         root_nh_.param("joint_limits/"+joint_names[i]+"/min_position", limits.min_position, -std::numeric_limits<double>::max());
00154         root_nh_.param("joint_limits/"+joint_names[i]+"/max_position", limits.max_position, std::numeric_limits<double>::max());
00155         root_nh_.param("joint_limits/"+joint_names[i]+"/max_velocity", limits.max_velocity, std::numeric_limits<double>::max());
00156         root_nh_.param("joint_limits/"+joint_names[i]+"/max_acceleration", limits.max_acceleration, std::numeric_limits<double>::max());
00157         bool boolean;
00158         root_nh_.param("joint_limits/"+joint_names[i]+"/has_position_limits", boolean, false);
00159         limits.has_position_limits = boolean?1:0;
00160         root_nh_.param("joint_limits/"+joint_names[i]+"/has_velocity_limits", boolean, false);
00161         limits.has_velocity_limits = boolean?1:0;
00162         root_nh_.param("joint_limits/"+joint_names[i]+"/has_acceleration_limits", boolean, false);
00163         limits.has_acceleration_limits = boolean?1:0;
00164         joint_limits_map_[joint_names[i]] = limits;
00165         limit_vec.push_back(limits);
00166       } else {
00167         limit_vec.push_back(joint_limits_map_[joint_names[i]]);
00168       }
00169     }
00170   }
00171 
00172   TrajectoryMetrics calculateTrajectoryMetrics(const arm_navigation_msgs::RobotState& start_state,
00173                                                const trajectory_msgs::JointTrajectory& joint_trajectory) {
00174     TrajectoryMetrics traj_metrics;
00175     
00176     std::vector<arm_navigation_msgs::JointLimits> limit_vec;
00177     getJointLimits(joint_trajectory.joint_names, limit_vec);
00178     
00179     trajectory_msgs::JointTrajectory jtraj = joint_trajectory;
00180 
00181     if(joint_trajectory.points.size() == 0) {
00182       return traj_metrics;
00183     }
00184 
00185     for(unsigned int i = 0; i < jtraj.points.size(); i++) {
00186       if(jtraj.points[i].velocities.size() != jtraj.points[i].positions.size()) {
00187         jtraj.points[i].velocities.resize(jtraj.points[i].positions.size(), 0.0);
00188       }
00189     }    
00190     //create a spline from the trajectory
00191     spline_smoother::CubicTrajectory trajectory_solver;
00192     spline_smoother::SplineTrajectory spline;
00193     
00194     trajectory_solver.parameterize(jtraj,limit_vec,spline);  
00195     
00196     double smoother_time;
00197     spline_smoother::getTotalTime(spline, smoother_time);   
00198 
00199     traj_metrics.spline_time_ = smoother_time;
00200     
00201     jtraj.points.clear();
00202 
00203     unsigned int num_points = 200;
00204 
00205     double t = 0.0;
00206     std::vector<double> times(num_points);
00207     for(unsigned int i = 0; i < num_points; i++,t += smoother_time/(1.0*(num_points))) {
00208       times[i] = t;
00209     }
00210     
00211     if(!spline_smoother::sampleSplineTrajectory(spline, times, jtraj)) {
00212       ROS_INFO("Spline smoother reports problem");
00213     }
00214 
00215     //double planner_time = traj.points.back().time_from_start.toSec();
00216     double total_collision_distance = 0.0;  
00217     std::string overall_closest_link;
00218     unsigned int closest_collision_point = 0;
00219     t = 0.0;
00220     for(unsigned int i = 0; i < jtraj.points.size(); i++, t += smoother_time/(1.0*(num_points))) {
00221       jtraj.points[i].time_from_start = ros::Duration(t);
00222     }
00223 
00224     std::vector<std::string> objs = current_link_names_;
00225     objs.insert(objs.end(), current_attached_body_names_.begin(), current_attached_body_names_.end());
00226 
00227     std::map<std::string, double> joint_positions;
00228     for(unsigned int i = 0; i < jtraj.points.size(); i++) {
00229       for(unsigned int j = 0; j < joint_trajectory.joint_names.size(); j++) {
00230         joint_positions[joint_trajectory.joint_names[j]] = jtraj.points[i].positions[j];
00231         if(i != jtraj.points.size()-1) {
00232           traj_metrics.total_path_length_ += fabs(jtraj.points[i+1].positions[j]-joint_positions[joint_trajectory.joint_names[j]]);
00233         }
00234       }
00235       cps_->getCollisionModelsInterface()->getPlanningSceneState()->setKinematicState(joint_positions);
00236       cps_->setCurrentGroupState(*cps_->getCollisionModelsInterface()->getPlanningSceneState());
00237 
00238       std::vector<collision_proximity::GradientInfo> gradients;
00239       cps_->getEnvironmentProximityGradients(gradients, true);
00240 
00241       if(objs.size() != gradients.size()) {
00242         ROS_WARN_STREAM("Objs size not equal to gradients " << objs.size() << " " << current_link_names_.size() << " " << gradients.size());
00243         return traj_metrics;
00244       }
00245       double this_distance = DBL_MAX;
00246       std::string this_link;
00247       for(unsigned int j = 0; j < objs.size(); j++) {
00248         if(gradients[j].closest_distance < this_distance) {
00249           if(gradients[j].closest_distance <= 0.0) {
00250             //ROS_INFO_STREAM("Link " << objs[j] << " apparently in collision " << gradients[j].closest_distance);
00251           }
00252           this_distance = gradients[j].closest_distance;
00253           this_link = objs[j];
00254         }
00255       }
00256       total_collision_distance += this_distance;
00257       if(this_distance <= traj_metrics.overall_closest_collision_distance_) {
00258         traj_metrics.overall_closest_collision_distance_ = this_distance;
00259         overall_closest_link = this_link;
00260         closest_collision_point = i;
00261       }
00262     }
00263 
00264     traj_metrics.average_closest_collision_distance_ = total_collision_distance/jtraj.points.size();
00265 
00266     // for(unsigned int j = 0; j < joint_trajectory.joint_names.size(); j++) {
00267     //   joint_positions[joint_trajectory.joint_names[j]] = jtraj.points[closest_collision_point].positions[j];
00268     // }
00269 
00270     // cps_->getCollisionModelsInterface()->getPlanningSceneState()->setKinematicState(joint_positions);
00271     // cps_->setCurrentGroupState(*cps_->getCollisionModelsInterface()->getPlanningSceneState());
00272 
00273     // std::vector<std::string> link_names;
00274     // std::vector<std::string> attached_body_names;
00275     // std::vector<collision_proximity::GradientInfo> gradients;
00276     // cps_->getStateGradients(link_names, attached_body_names, gradients);
00277 
00278     // visualization_msgs::MarkerArray arr;
00279     // cps_->getProximityGradientMarkers(link_names,
00280     //                                   attached_body_names,
00281     //                                   gradients,
00282     //                                   "",
00283     //                                   arr);
00284     // vis_marker_array_publisher_.publish(arr);
00285 
00286 
00287     ROS_INFO_STREAM("Trajectory has closest overall distance " 
00288                     << traj_metrics.overall_closest_collision_distance_);
00289     ROS_INFO_STREAM("Path length " << traj_metrics.total_path_length_ << " spline time " << traj_metrics.spline_time_);    
00290     ROS_INFO_STREAM("Closest link is " << overall_closest_link);
00291     return traj_metrics;
00292   }
00293 
00294 
00295   void calculateMetrics() {
00296 
00297     std::vector<ros::Time> planning_scene_creation_times;
00298     move_arm_warehouse_reader_->getAvailablePlanningSceneList("", planning_scene_creation_times);
00299     
00300     overall_metrics_.move_arm_metric_vector_.resize(planning_scene_creation_times.size());
00301 
00302     for(unsigned int i = 0; i < planning_scene_creation_times.size(); i++) {
00303       arm_navigation_msgs::PlanningScene planning_scene;
00304       if(!move_arm_warehouse_reader_->getPlanningScene("", planning_scene_creation_times[i],
00305                                                        planning_scene)) {
00306         ROS_WARN_STREAM("Problem with planning scene for creation time " << planning_scene_creation_times[i]);
00307         continue;
00308       }
00309 
00310       if(!cps_->setPlanningScene(planning_scene)) {
00311         ROS_WARN_STREAM("Bad planning scene " << planning_scene_creation_times[i]);
00312         continue;
00313       }
00314       
00315       std::vector<std::string> pipeline_stages;
00316       std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> error_codes;
00317       std::vector<std::string> trajectories;
00318       if(!move_arm_warehouse_reader_->getAssociatedOutcomes("", planning_scene_creation_times[i],
00319                                                             pipeline_stages,
00320                                                             error_codes,
00321                                                             trajectories)) {
00322         ROS_WARN_STREAM("Problem with outcome for creation time " << planning_scene_creation_times[i]);
00323         arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00324         error_code.val = 0;
00325         error_codes.push_back(error_code);
00326         pipeline_stages.push_back("Unknown");
00327       }
00328       overall_metrics_.move_arm_metric_vector_[i].error_code_ = error_codes.back();
00329       overall_metrics_.move_arm_metric_vector_[i].pipeline_stage_ = pipeline_stages.back();
00330 
00331       overall_metrics_.invocations_++;
00332 
00333       if(error_codes.back().val != error_codes.back().SUCCESS) {
00334         overall_metrics_.failure_stage_[pipeline_stages.back()]++;
00335       } else if(error_codes.size() >1) {
00336         overall_metrics_.recoverable_errors_++;
00337         overall_metrics_.successes_++;
00338       } else {
00339         overall_metrics_.successes_++;
00340       }
00341 
00342       std::vector<std::string> motion_plan_request_stage_names;
00343       if(!move_arm_warehouse_reader_->getAssociatedMotionPlanRequestsStageNames("", planning_scene_creation_times[i],
00344                                                                                 motion_plan_request_stage_names)) {
00345         ROS_WARN_STREAM("No motion plan requests for creation time " << planning_scene_creation_times[i]);
00346         continue;
00347       }
00348 
00349       arm_navigation_msgs::MotionPlanRequest motion_plan_request;
00350       std::string ID;
00351       if(!move_arm_warehouse_reader_->getAssociatedMotionPlanRequest("", planning_scene_creation_times[i],
00352                                                                      motion_plan_request_stage_names[0],
00353                                                                      motion_plan_request,ID)) {
00354         ROS_WARN_STREAM("Specific motion plan request bad for stage names " << motion_plan_request_stage_names[0]);
00355         continue;
00356       }
00357 
00358       if(motion_plan_request.group_name.empty()) {
00359         ROS_WARN_STREAM("No group name for motion plan request stage name " << motion_plan_request_stage_names[0]);
00360         continue;
00361       }
00362 
00363       bool has_path_constraints = false;
00364       if(!motion_plan_request.path_constraints.orientation_constraints.empty() ||
00365          !motion_plan_request.path_constraints.position_constraints.empty()) {
00366         has_path_constraints = true;
00367         with_path_constraints_.invocations_++;
00368       } else {
00369         without_path_constraints_.invocations_++;
00370       }
00371 
00372       //just gonna take the first one to get the group name, as it shouldn't matter for this
00373       cps_->setupForGroupQueries(motion_plan_request.group_name,
00374                                  motion_plan_request.start_state,
00375                                  current_link_names_,
00376                                  current_attached_body_names_);
00377 
00378       std::vector<std::string> joint_trajectory_sources;
00379       if(!move_arm_warehouse_reader_->getAssociatedJointTrajectorySources("", planning_scene_creation_times[i],
00380                                                                           joint_trajectory_sources)) {
00381         ROS_WARN_STREAM("No trajectories associated with creation time " << planning_scene_creation_times[i]);
00382         continue;
00383       }
00384       
00385 
00386       for(unsigned int j = 0; j < joint_trajectory_sources.size(); j++) {
00387         trajectory_msgs::JointTrajectory jt;
00388         ros::Duration processing_time;
00389         if(!move_arm_warehouse_reader_->getAssociatedJointTrajectory("", planning_scene_creation_times[i],
00390                                                                      joint_trajectory_sources[j], 0,
00391                                                                      processing_time, 
00392                                                                      jt)) {
00393           ROS_WARN_STREAM("Specific joint trajectory not available for " << joint_trajectory_sources[j]);
00394           continue;
00395         }
00396         TrajectoryMetrics tm = calculateTrajectoryMetrics(motion_plan_request.start_state, jt);
00397         tm.processing_time_ = processing_time;
00398         overall_metrics_.move_arm_metric_vector_[i].stage_metrics_[joint_trajectory_sources[j]] = tm;
00399         overall_metrics_.trajectory_source_metrics_[joint_trajectory_sources[j]].addTrajectoryMetric(tm);
00400         if(has_path_constraints) {
00401           with_path_constraints_.trajectory_source_metrics_[joint_trajectory_sources[j]].addTrajectoryMetric(tm);
00402         } else {
00403           without_path_constraints_.trajectory_source_metrics_[joint_trajectory_sources[j]].addTrajectoryMetric(tm);
00404         }
00405       }
00406       if(has_path_constraints) {
00407         with_path_constraints_.move_arm_metric_vector_.push_back(overall_metrics_.move_arm_metric_vector_[i]);
00408       } else {
00409         without_path_constraints_.move_arm_metric_vector_.push_back(overall_metrics_.move_arm_metric_vector_[i]);
00410       }
00411     }
00412     ROS_INFO_STREAM("Overall metrics");
00413     printOverallMetrics(overall_metrics_);
00414 
00415     ROS_INFO_STREAM("With path constraints metrics");
00416     printOverallMetrics(with_path_constraints_);
00417 
00418     ROS_INFO_STREAM("Without path constraints metrics");
00419     printOverallMetrics(without_path_constraints_);
00420   }
00421 
00422   void printOverallMetrics(const OverallMetrics& metrics)
00423   {
00424     ROS_INFO_STREAM("\n\nMove arm had " << metrics.successes_ << " out of " 
00425                     << metrics.invocations_ << " successful invocations.  Recoverable errors: " << metrics.recoverable_errors_);
00426 
00427     for(std::map<std::string, unsigned int>::const_iterator it = metrics.failure_stage_.begin();
00428         it != metrics.failure_stage_.end();
00429         it++) {
00430       ROS_INFO_STREAM("Failure stage " << it->first << " instances " << it->second);
00431     }
00432 
00433     for(std::map<std::string, TrajectorySourceMetrics>::const_iterator it = metrics.trajectory_source_metrics_.begin();
00434         it != metrics.trajectory_source_metrics_.end();
00435         it++) {
00436       ROS_INFO_STREAM("\n");
00437       ROS_INFO_STREAM("Source: " << it->first);
00438       ROS_INFO_STREAM("-----------------------");
00439       ROS_INFO_STREAM("Average average closest collision distance: " 
00440                       << it->second.total_average_closest_collision_distance_/it->second.getSizeAsDouble());
00441       ROS_INFO_STREAM("Average overall closest collision distance: " 
00442                       << it->second.total_overall_closest_collision_distance_/it->second.getSizeAsDouble());
00443       ROS_INFO_STREAM("Average total path length: " 
00444                       << it->second.total_total_path_length_/it->second.getSizeAsDouble());
00445       ROS_INFO_STREAM("Average spline duration: " 
00446                       << it->second.total_spline_time_/it->second.getSizeAsDouble());
00447       ROS_INFO_STREAM("Average processing time: " 
00448                       << it->second.total_processing_time_.toSec()/it->second.getSizeAsDouble());
00449       ROS_INFO_STREAM("\n");
00450     }
00451   }
00452 
00453 public:
00454 
00455   ros::Publisher vis_marker_publisher_;
00456   ros::Publisher vis_marker_array_publisher_;
00457 
00458   std::vector<std::string> current_link_names_;
00459   std::vector<std::string> current_attached_body_names_;
00460 
00461   move_arm_warehouse::MoveArmWarehouseLoggerReader* move_arm_warehouse_reader_;
00462   collision_proximity::CollisionProximitySpace* cps_;
00463   ros::NodeHandle root_nh_;
00464   OverallMetrics overall_metrics_;
00465   OverallMetrics with_path_constraints_;
00466   OverallMetrics without_path_constraints_;
00467   std::map<std::string, arm_navigation_msgs::JointLimits> joint_limits_map_;
00468 };
00469 
00470 int main(int argc, char** argv)
00471 {
00472   ros::init(argc, argv, "warehouse_metrics_evaluator");
00473 
00474   ros::AsyncSpinner spinner(1); 
00475   spinner.start();
00476  
00477   WarehouseMetricsEvaluator wme;
00478   wme.calculateMetrics();
00479   ros::shutdown();
00480   return 0;
00481 }
warehouse_metrics_evaluator.cpp
