planning_environment: planning_environment: collision_models

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 #include <std_srvs/Empty.h>
00038 
00039 #include "planning_environment/models/collision_models_interface.h"
00040 #include "planning_environment/models/model_utils.h"
00041 
00042 static const std::string REGISTER_PLANNING_SCENE_NAME = "register_planning_scene";
00043 
00044 planning_environment::CollisionModelsInterface::CollisionModelsInterface(const std::string& description, bool register_with_server)
00045   : CollisionModels(description)
00046 {
00047   planning_scene_state_ = NULL;
00048 
00049   set_planning_scene_callback_ = NULL;
00050   revert_planning_scene_callback_ = NULL;
00051 
00052   ros::NodeHandle root_nh;
00053   std::string env_service_name = root_nh.resolveName(REGISTER_PLANNING_SCENE_NAME, true);
00054 
00055   while(register_with_server && root_nh.ok()) {
00056     if(ros::service::waitForService(env_service_name, ros::Duration(1.0))) {
00057       break;
00058     }
00059     ROS_INFO_STREAM("Waiting for environment server planning scene registration service " << env_service_name);
00060   }
00061   
00062   //need to create action server before we request
00063   action_server_ = new actionlib::SimpleActionServer<arm_navigation_msgs::SyncPlanningSceneAction>(priv_nh_, "sync_planning_scene",
00064                                                                                                    boost::bind(&CollisionModelsInterface::syncPlanningSceneCallback, this, _1), false);
00065   action_server_->start();
00066 
00067   if(register_with_server) {
00068     env_server_register_client_ = root_nh.serviceClient<std_srvs::Empty>(env_service_name);
00069     
00070     std_srvs::Empty::Request req;
00071     std_srvs::Empty::Response res;
00072     while(ros::ok() && true) {
00073       if(!env_server_register_client_.call(req, res)) {
00074         ROS_INFO_STREAM("Couldn't register for planning scenes");
00075         ros::WallDuration(1.0).sleep();
00076       } else {
00077         break;
00078       }
00079     }
00080   }
00081 }
00082 
00083 planning_environment::CollisionModelsInterface::~CollisionModelsInterface()
00084 {
00085   delete action_server_;
00086   if(planning_scene_state_ != NULL) {
00087     delete planning_scene_state_;
00088   }
00089 }
00090 
00091 void planning_environment::CollisionModelsInterface::syncPlanningSceneCallback(const arm_navigation_msgs::SyncPlanningSceneGoalConstPtr& scene)
00092 {
00093   ros::WallTime t1 = ros::WallTime::now();
00094   bodiesLock();
00095   arm_navigation_msgs::SyncPlanningSceneResult res;
00096   res.ok = true;
00097 
00098   ROS_DEBUG("Syncing planning scene");
00099 
00100   if(planning_scene_set_) {
00101     ROS_DEBUG("Reverting planning scene");
00102     revertPlanningScene(planning_scene_state_);
00103     planning_scene_state_ = NULL;
00104     if(revert_planning_scene_callback_ != NULL) {
00105       revert_planning_scene_callback_();
00106     }
00107   }
00108   planning_scene_state_ = setPlanningScene(scene->planning_scene);
00109   if(planning_scene_state_ == NULL) {
00110     ROS_ERROR("Setting planning scene state to NULL");
00111     res.ok = false;
00112     action_server_->setAborted(res);
00113     bodiesUnlock();
00114     return;
00115   }
00116   last_planning_scene_ = scene->planning_scene;
00117   arm_navigation_msgs::SyncPlanningSceneFeedback feedback;
00118   feedback.client_processing = true;
00119   feedback.ready = false;
00120   action_server_->publishFeedback(feedback);
00121   //TODO - we can run the callback in a new thread, but it's going to mean communicating
00122   //preempts over semaphors and whatnot
00123   if(set_planning_scene_callback_ != NULL) {
00124     set_planning_scene_callback_(scene->planning_scene);
00125   }
00126   //if we're here, assuming client is ready
00127   feedback.ready = true;
00128   action_server_->publishFeedback(feedback);
00129   action_server_->setSucceeded(res);
00130   ROS_DEBUG_STREAM("Setting took " << (ros::WallTime::now()-t1).toSec());
00131   bodiesUnlock();
00132 }
00133 
00134 bool planning_environment::CollisionModelsInterface::setPlanningSceneWithCallbacks(const arm_navigation_msgs::PlanningScene& planning_scene)
00135 {
00136   if(planning_scene_set_) {
00137     revertPlanningScene(planning_scene_state_);
00138     planning_scene_state_ = NULL;
00139     if(revert_planning_scene_callback_ != NULL) {
00140       revert_planning_scene_callback_();
00141     }
00142   }
00143   planning_scene_state_ = setPlanningScene(planning_scene);
00144   if(planning_scene_state_ == NULL) {
00145     ROS_ERROR("Setting planning scene state to NULL");
00146     return false;
00147   }
00148   last_planning_scene_ = planning_scene;
00149   //TODO - we can run the callback in a new thread, but it's going to mean communicating
00150   //preempts over semaphors and whatnot
00151   if(set_planning_scene_callback_ != NULL) {
00152     set_planning_scene_callback_(planning_scene);
00153   }
00154   return true;
00155 }
00156 
00157 void planning_environment::CollisionModelsInterface::resetToStartState(planning_models::KinematicState& state) const {
00158   setRobotStateAndComputeTransforms(last_planning_scene_.robot_state, state);
00159 }
collision_models_interface.cpp
