planning_environment: planning_monitor.h Source File

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, 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 the 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 
00037 #ifndef PLANNING_ENVIRONMENT_MONITORS_PLANNING_MONITOR_
00038 #define PLANNING_ENVIRONMENT_MONITORS_PLANNING_MONITOR_
00039 
00040 #include "planning_environment/monitors/collision_space_monitor.h"
00041 #include <trajectory_msgs/JointTrajectory.h>
00042 #include <motion_planning_msgs/Constraints.h>
00043 #include <motion_planning_msgs/GetMotionPlan.h>
00044 #include <motion_planning_msgs/ArmNavigationErrorCodes.h>
00045 #include <iostream>
00046 
00047 namespace planning_environment
00048 {
00053 class PlanningMonitor : public CollisionSpaceMonitor
00054 {
00055 public:
00056                 
00057   PlanningMonitor(CollisionModels *cm, tf::TransformListener *tf) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), tf)
00058   {
00059     onCollisionContact_ = NULL;     
00060     loadParams();
00061     use_collision_map_ = true;
00062   }
00063         
00064   virtual ~PlanningMonitor(void)
00065   {
00066   }
00067         
00069   enum 
00070     {
00071       COLLISION_TEST        = 1,
00072       PATH_CONSTRAINTS_TEST = 2,
00073       GOAL_CONSTRAINTS_TEST = 4,
00074       JOINT_LIMITS_TEST     = 8,
00075       CHECK_FULL_TRAJECTORY = 16
00076 
00077     };  
00078 
00079   bool prepareForValidityChecks(const std::vector<std::string>& joint_names,
00080                                 const motion_planning_msgs::OrderedCollisionOperations& ordered_collision_operations,
00081                                 const std::vector<motion_planning_msgs::AllowedContactSpecification>& allowed_contacts,
00082                                 const motion_planning_msgs::Constraints& path_constraints,
00083                                 const motion_planning_msgs::Constraints& goal_constraints,
00084                                 const std::vector<motion_planning_msgs::LinkPadding>& link_padding,
00085                                 motion_planning_msgs::ArmNavigationErrorCodes &error_code);
00086 
00087   void revertToDefaultState();
00088             
00090   bool isEnvironmentSafe(motion_planning_msgs::ArmNavigationErrorCodes &error_code) const;
00091        
00100   bool isTrajectoryValid(const trajectory_msgs::JointTrajectory &trajectory, 
00101                          motion_planning_msgs::RobotState &robot_state, 
00102                          const int test, 
00103                          bool verbose, 
00104                          motion_planning_msgs::ArmNavigationErrorCodes &error_code,
00105                          std::vector<motion_planning_msgs::ArmNavigationErrorCodes> &trajectory_error_codes);
00106 
00115   bool isTrajectoryValid(const trajectory_msgs::JointTrajectory &trajectory, 
00116                          motion_planning_msgs::RobotState &robot_state, 
00117                          unsigned int start, 
00118                          unsigned int end, 
00119                          const int test, 
00120                          bool verbose,
00121                          motion_planning_msgs::ArmNavigationErrorCodes &error_code,
00122                          std::vector<motion_planning_msgs::ArmNavigationErrorCodes> &trajectory_error_codes);
00123 
00124   bool isStateValid(const motion_planning_msgs::RobotState &robot_state, 
00125                     const int test, 
00126                     bool verbose, 
00127                     motion_planning_msgs::ArmNavigationErrorCodes &error_code);
00128         
00130   int  closestStateOnTrajectory(const trajectory_msgs::JointTrajectory &trajectory, 
00131                                 motion_planning_msgs::RobotState &robot_state, 
00132                                 motion_planning_msgs::ArmNavigationErrorCodes &error_code) const;
00133 
00135   int  closestStateOnTrajectory(const trajectory_msgs::JointTrajectory &trajectory, 
00136                                 motion_planning_msgs::RobotState &robot_state, 
00137                                 unsigned int start, 
00138                                 unsigned int end, 
00139                                 motion_planning_msgs::ArmNavigationErrorCodes &error_code) const;
00140         
00142   bool setPathConstraints(const motion_planning_msgs::Constraints &kc, 
00143                           motion_planning_msgs::ArmNavigationErrorCodes &error_code);
00144 
00146   const motion_planning_msgs::Constraints& getPathConstraints(void) const
00147   {
00148     return path_constraints_;
00149   }
00150         
00152   bool setGoalConstraints(const motion_planning_msgs::Constraints &kc, 
00153                           motion_planning_msgs::ArmNavigationErrorCodes &error_code);
00154         
00156   const motion_planning_msgs::Constraints& getGoalConstraints(void) const
00157   {
00158     return goal_constraints_;
00159   }
00160 
00162   void printConstraints(std::ostream &out = std::cout);
00163 
00165   void clearConstraints(void);
00166         
00168   bool transformConstraintsToFrame(motion_planning_msgs::Constraints &kc, 
00169                                    const std::string &target,
00170                                    motion_planning_msgs::ArmNavigationErrorCodes &error_code) const;
00171         
00173   bool transformTrajectoryToFrame(trajectory_msgs::JointTrajectory &kp, 
00174                                   motion_planning_msgs::RobotState &robot_state, 
00175                                   const std::string &target,
00176                                   motion_planning_msgs::ArmNavigationErrorCodes &error_code) const;
00177 
00179   bool transformJointToFrame(double &value, 
00180                              const std::string &joint_name, 
00181                              std::string &frame_id, 
00182                              const std::string &target,
00183                              motion_planning_msgs::ArmNavigationErrorCodes &error_code) const;
00184 
00186   void setAllowedContacts(const std::vector<motion_planning_msgs::AllowedContactSpecification> &allowedContacts);
00187         
00189   void setAllowedContacts(const std::vector<collision_space::EnvironmentModel::AllowedContact> &allowedContacts);
00190         
00192   const std::vector<collision_space::EnvironmentModel::AllowedContact>& getAllowedContacts(void) const;
00193 
00195   void printAllowedContacts(std::ostream &out = std::cout);
00196         
00198   void clearAllowedContacts(void);
00199 
00200   bool broadcastCollisions();
00201         
00203   void setOnCollisionContactCallback(const boost::function<void(collision_space::EnvironmentModel::Contact&)> &callback)
00204   {
00205     onCollisionContact_ = callback;
00206   }
00207 
00209   double getExpectedMapUpdateInterval(void) const
00210   {
00211     return intervalCollisionMap_;
00212   }
00213 
00215   double getExpectedJointStateUpdateInterval(void) const
00216   {
00217     return intervalState_;
00218   }
00219 
00221   double getExpectedPoseUpdateInterval(void) const
00222   {
00223     return intervalPose_;
00224   }
00225 
00226   void setCollisionCheck(const std::string link_name, bool state);
00227 
00228   void setCollisionCheckAll(bool state);
00229 
00230   void setCollisionCheckLinks(const std::vector<std::string> &link_names, bool state);
00231 
00232   void setCollisionCheckOnlyLinks(const std::vector<std::string> &link_names, bool state);
00233 
00234   void getChildLinks(const std::vector<std::string> &joints,std::vector<std::string> &link_names);
00235 
00236   void getOrderedCollisionOperationsForOnlyCollideLinks(const std::vector<std::string> &collision_check_links, 
00237                                                         const motion_planning_msgs::OrderedCollisionOperations &requested_collision_operations,
00238                                                         motion_planning_msgs::OrderedCollisionOperations &result_collision_operations);
00239 
00240   bool checkPathConstraints(const planning_models::KinematicState* state, bool verbose) const;
00241 
00242   bool checkGoalConstraints(const planning_models::KinematicState* state, bool verbose) const;
00243 
00244 protected:
00245 
00247   void loadParams(void);
00248 
00250   bool transformJoint(const std::string &name, 
00251                       unsigned int index, 
00252                       double &param, 
00253                       std::string& frame_id, 
00254                       const std::string& target,
00255                       motion_planning_msgs::ArmNavigationErrorCodes &error_code) const;
00256         
00258   bool isTrajectoryValidAux(const trajectory_msgs::JointTrajectory &trajectory, 
00259                             motion_planning_msgs::RobotState &robot_state, 
00260                             unsigned int start, 
00261                             unsigned int end, 
00262                             const int test, 
00263                             bool verbose,
00264                             motion_planning_msgs::ArmNavigationErrorCodes &error_code,
00265                             std::vector<motion_planning_msgs::ArmNavigationErrorCodes> &trajectory_error_codes);
00266 
00268   int closestStateOnTrajectoryAux(const trajectory_msgs::JointTrajectory &trajectory, 
00269                                   unsigned int start, 
00270                                   unsigned int end, 
00271                                   motion_planning_msgs::ArmNavigationErrorCodes &error_code) const;
00272         
00274   boost::function<void(collision_space::EnvironmentModel::Contact&)> onCollisionContact_;
00275         
00276   std::vector<collision_space::EnvironmentModel::AllowedContact>     allowedContacts_;
00277 
00278   motion_planning_msgs::Constraints path_constraints_;
00279   motion_planning_msgs::Constraints goal_constraints_;
00280 
00281   int num_contacts_allowable_contacts_test_;
00282   int num_contacts_for_display_;
00283         
00284   double intervalCollisionMap_;
00285   double intervalState_;
00286   double intervalPose_; 
00287 
00288   ros::Publisher display_collision_pose_publisher_;
00289   ros::Publisher display_state_validity_publisher_;
00290 
00291 };      
00292 }
00293 
00294 #endif