planning_scene_validity_server.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *  \author Sachin Chitta, Ioan Sucan
00036  *********************************************************************/
00037 #include <ros/ros.h>
00038 
00039 #include <planning_environment/models/collision_models_interface.h>
00040 #include <planning_environment/models/model_utils.h>
00041 
00042 #include <arm_navigation_msgs/GetJointTrajectoryValidity.h>
00043 #include <arm_navigation_msgs/GetStateValidity.h>
00044 
00045 namespace planning_environment
00046 {
00047 class PlanningSceneValidityServer
00048 {
00049 public: 
00050   PlanningSceneValidityServer() :
00051   private_handle_("~")
00052   {
00053     std::string robot_description_name = root_handle_.resolveName("robot_description", true);
00054     collision_models_interface_ = new planning_environment::CollisionModelsInterface(robot_description_name);
00055 
00056     vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("planning_scene_markers", 128);
00057     vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("planning_scene_markers_array", 128);
00058 
00059     get_trajectory_validity_service_ = private_handle_.advertiseService("get_trajectory_validity", &PlanningSceneValidityServer::getTrajectoryValidity, this);
00060     get_state_validity_service_ = private_handle_.advertiseService("get_state_validity", &PlanningSceneValidityServer::getStateValidity, this);
00061   }
00062         
00063   virtual ~PlanningSceneValidityServer()
00064   {
00065     delete collision_models_interface_;
00066   }
00067 
00068   bool getTrajectoryValidity(arm_navigation_msgs::GetJointTrajectoryValidity::Request &req, 
00069                              arm_navigation_msgs::GetJointTrajectoryValidity::Response &res) 
00070   {
00071     collision_models_interface_->bodiesLock();
00072     if(!collision_models_interface_->isPlanningSceneSet()) {
00073       res.error_code.val = res.error_code.COLLISION_CHECKING_UNAVAILABLE;
00074       ROS_WARN_STREAM("Calling getTrajectoryValidity with no planning scene set");
00075       collision_models_interface_->bodiesUnlock();
00076       return true;
00077     }
00078     collision_models_interface_->resetToStartState(*collision_models_interface_->getPlanningSceneState());
00079     planning_environment::setRobotStateAndComputeTransforms(req.robot_state,
00080                                                             *collision_models_interface_->getPlanningSceneState());
00081 
00082     arm_navigation_msgs::Constraints goal_constraints;
00083     if(req.check_goal_constraints) {
00084       goal_constraints = req.goal_constraints;
00085     }
00086     arm_navigation_msgs::Constraints path_constraints;
00087     if(req.check_path_constraints) {
00088       path_constraints = req.path_constraints;
00089     }
00090     collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(),
00091                                                         req.trajectory,
00092                                                         goal_constraints,
00093                                                         path_constraints,
00094                                                         res.error_code,
00095                                                         res.trajectory_error_codes,
00096                                                         req.check_full_trajectory);
00097     collision_models_interface_->bodiesUnlock();
00098     return true;
00099   }
00100 
00101   bool getStateValidity(arm_navigation_msgs::GetStateValidity::Request &req, 
00102                         arm_navigation_msgs::GetStateValidity::Response &res) 
00103   {
00104     collision_models_interface_->bodiesLock();
00105     if(!collision_models_interface_->isPlanningSceneSet()) {
00106       res.error_code.val = res.error_code.COLLISION_CHECKING_UNAVAILABLE;
00107       ROS_WARN_STREAM("Calling getStateValidity with no planning scene set");
00108       collision_models_interface_->bodiesUnlock();
00109       return true;
00110     }
00111     collision_models_interface_->resetToStartState(*collision_models_interface_->getPlanningSceneState());
00112     const planning_models::KinematicModel::JointModelGroup* jmg = NULL;
00113 
00114     if(!req.group_name.empty()) {
00115       jmg = collision_models_interface_->getKinematicModel()->getModelGroup(req.group_name);
00116       if(!jmg) {
00117         ROS_WARN_STREAM("No group name " << req.group_name << " in state validity check");
00118         res.error_code.val = res.error_code.INVALID_GROUP_NAME;
00119         collision_models_interface_->bodiesUnlock();
00120         return true;
00121       }
00122       collision_models_interface_->disableCollisionsForNonUpdatedLinks(req.group_name);
00123     }
00124     planning_environment::setRobotStateAndComputeTransforms(req.robot_state,
00125                                                             *collision_models_interface_->getPlanningSceneState());
00126     arm_navigation_msgs::Constraints goal_constraints;
00127     if(req.check_goal_constraints) {
00128       goal_constraints = req.goal_constraints;
00129     }
00130     arm_navigation_msgs::Constraints path_constraints;
00131     if(req.check_path_constraints) {
00132       path_constraints = req.path_constraints;
00133     }
00134     std::vector<std::string> joint_names;
00135     if(req.check_joint_limits) {
00136       if(!jmg) {
00137         joint_names = req.robot_state.joint_state.name;
00138       } else {
00139         joint_names = jmg->getJointModelNames();
00140       }
00141     }
00142     collision_models_interface_->isKinematicStateValid(*collision_models_interface_->getPlanningSceneState(),
00143                                                        joint_names,
00144                                                        res.error_code,
00145                                                        goal_constraints,
00146                                                        path_constraints,
00147                                                        true);
00148     if(res.error_code.val == arm_navigation_msgs::ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00149       collision_models_interface_->getAllCollisionsForState(*collision_models_interface_->getPlanningSceneState(),
00150                                                             res.contacts, 
00151                                                             1);
00152     }
00153     collision_models_interface_->bodiesUnlock();
00154     return true;
00155 
00156   }
00157 
00158   void broadcastPlanningSceneMarkers()
00159   {
00160     collision_models_interface_->bodiesLock();
00161     if(!collision_models_interface_->isPlanningSceneSet()) {
00162       collision_models_interface_->bodiesUnlock();
00163       return;
00164     }
00165     collision_models_interface_->resetToStartState(*collision_models_interface_->getPlanningSceneState());
00166     visualization_msgs::MarkerArray arr;
00167     std_msgs::ColorRGBA col;
00168     col.a = .9;
00169     col.r = 0.0;
00170     col.b = 1.0;
00171     col.g = 0.0;
00172     collision_models_interface_->getCollisionMapAsMarkers(arr,
00173                                                           col,
00174                                                           ros::Duration(0.2));
00175     col.g = 1.0;
00176     col.b = 1.0;
00177     collision_models_interface_->getStaticCollisionObjectMarkers(arr,
00178                                                                  "",
00179                                                                  col,
00180                                                                  ros::Duration(0.2));
00181 
00182     col.r = 0.6;
00183     col.g = 0.4;
00184     col.b = 0.3;
00185 
00186     collision_models_interface_->getAttachedCollisionObjectMarkers(*collision_models_interface_->getPlanningSceneState(),
00187                                                                    arr,
00188                                                                    "",
00189                                                                    col,
00190                                                                    ros::Duration(0.2));
00191     vis_marker_array_publisher_.publish(arr);
00192     collision_models_interface_->bodiesUnlock();
00193   }
00194 
00195         
00196 private:
00197                 
00198         
00199   ros::NodeHandle root_handle_, private_handle_;
00200   planning_environment::CollisionModelsInterface *collision_models_interface_;
00201 
00202   ros::ServiceServer get_trajectory_validity_service_;
00203   ros::ServiceServer get_state_validity_service_;
00204 
00205   ros::Publisher vis_marker_publisher_;
00206   ros::Publisher vis_marker_array_publisher_;
00207 };    
00208 }
00209 
00210 int main(int argc, char** argv)
00211 {
00212   ros::init(argc, argv, "planning_scene_validity_server");
00213   //figuring out whether robot_description has been remapped
00214 
00215   ros::AsyncSpinner spinner(1); 
00216   spinner.start();
00217   ros::NodeHandle nh;
00218   planning_environment::PlanningSceneValidityServer validity_server;
00219   ros::Rate r(10.0);
00220   while(nh.ok()) {
00221     validity_server.broadcastPlanningSceneMarkers();
00222     r.sleep();
00223   }
00224   ros::waitForShutdown();
00225   return 0;
00226 }


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43