environment_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 
00038 #include "planning_environment/monitors/planning_monitor.h"
00039 
00040 #include <arm_navigation_msgs/GetRobotState.h>
00041 #include <arm_navigation_msgs/GetStateValidity.h>
00042 #include <arm_navigation_msgs/GetPlanningScene.h>
00043 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00044 #include <ros/package.h>
00045 #include <std_srvs/Empty.h>
00046 #include <arm_navigation_msgs/SyncPlanningSceneAction.h>
00047 #include <actionlib/client/simple_action_client.h>
00048 #include <planning_environment/models/model_utils.h>
00049 
00050 static const std::string SYNC_PLANNING_SCENE_NAME ="sync_planning_scene";
00051 static const unsigned int UNSUCCESSFUL_REPLY_LIMIT = 5;
00052 static const ros::Duration PLANNING_SCENE_CLIENT_TIMEOUT(5.0);
00053 
00054 namespace planning_environment
00055 {
00056 class EnvironmentServer
00057 {
00058 public: 
00059   EnvironmentServer() :
00060   private_handle_("~")
00061   {
00062     private_handle_.param<bool>("use_monitor", use_monitor_, true);
00063     private_handle_.param<bool>("use_collision_map", use_collision_map_, false);
00064 
00065     std::string robot_description_name = root_handle_.resolveName("robot_description", true);
00066 
00067     collision_models_ = new planning_environment::CollisionModels(robot_description_name);
00068     if(use_monitor_) {
00069       planning_monitor_ = new planning_environment::PlanningMonitor(collision_models_, &tf_);
00070       planning_monitor_->setUseCollisionMap(use_collision_map_);
00071 
00072       planning_monitor_->waitForState();
00073       planning_monitor_->startEnvironmentMonitor();
00074       bool printed = false;
00075       while(root_handle_.ok() && use_collision_map_ && !planning_monitor_->haveMap()){
00076         ros::Duration().fromSec(1.0).sleep();
00077         if(!printed) {
00078           ROS_INFO_STREAM("Waiting for collision map");
00079           printed = true;
00080         }
00081       }
00082     } else {
00083       planning_monitor_ = NULL;
00084     }
00085     
00086     register_planning_scene_service_ = root_handle_.advertiseService("register_planning_scene", &EnvironmentServer::registerPlanningScene, this);
00087     get_robot_state_service_ = private_handle_.advertiseService("get_robot_state", &EnvironmentServer::getRobotState, this);
00088     get_planning_scene_service_ = private_handle_.advertiseService("get_planning_scene", &EnvironmentServer::getPlanningScene, this);
00089     set_planning_scene_diff_service_ = private_handle_.advertiseService("set_planning_scene_diff", &EnvironmentServer::setPlanningSceneDiff, this);
00090 
00091     ROS_INFO("Environment server started");
00092   }
00093         
00094   virtual ~EnvironmentServer()
00095   {
00096     for(std::map<std::string, actionlib::SimpleActionClient<arm_navigation_msgs::SyncPlanningSceneAction>* >::iterator it = sync_planning_scene_clients_.begin();
00097         it != sync_planning_scene_clients_.end();
00098         it++) {
00099       delete it->second;
00100     }
00101     delete collision_models_;
00102     if(planning_monitor_) {
00103       delete planning_monitor_;
00104     }
00105   }
00106         
00107 private:
00108   
00109   bool registerPlanningScene(std_srvs::Empty::Request &req,
00110                              std_srvs::Empty::Response &res)
00111   {
00112     register_lock_.lock();
00113     if(req.__connection_header->find("callerid") == req.__connection_header->end()) {
00114       ROS_ERROR_STREAM("Request has no callerid");
00115       return false;
00116     }
00117     std::string callerid = req.__connection_header->find("callerid")->second;
00118     if(sync_planning_scene_clients_.find(callerid) != sync_planning_scene_clients_.end()) {
00119       delete sync_planning_scene_clients_[callerid];
00120     }
00121     sync_planning_scene_clients_[callerid] = new actionlib::SimpleActionClient<arm_navigation_msgs::SyncPlanningSceneAction>(callerid+"/"+SYNC_PLANNING_SCENE_NAME, true);
00122     if(!sync_planning_scene_clients_[callerid]->waitForServer(ros::Duration(10.0))) {
00123       ROS_INFO_STREAM("Couldn't connect back to action server for " << callerid << ". Removing from list");
00124       delete sync_planning_scene_clients_[callerid];
00125       sync_planning_scene_clients_.erase(callerid);
00126       register_lock_.unlock();
00127       return false;
00128     } 
00129     ROS_INFO_STREAM("Successfully connected to planning scene action server for " << callerid);
00130     register_lock_.unlock();
00131     return true;
00132   }
00133                 
00134   bool getRobotState(arm_navigation_msgs::GetRobotState::Request &req, 
00135                      arm_navigation_msgs::GetRobotState::Response &res)
00136   {
00137     planning_monitor_->getCurrentRobotState(res.robot_state);
00138     return true;    
00139   }
00140 
00141   bool getPlanningScene(arm_navigation_msgs::GetPlanningScene::Request &req, 
00142                         arm_navigation_msgs::GetPlanningScene::Response &res) 
00143   {
00144     if(use_monitor_) {
00145       planning_monitor_->getCompletePlanningScene(req.planning_scene_diff,
00146                                                   req.operations,
00147                                                   res.planning_scene);
00148     } else {
00149       res.planning_scene = req.planning_scene_diff;
00150     }
00151     return true;
00152   }
00153 
00154   bool setPlanningSceneDiff(arm_navigation_msgs::SetPlanningSceneDiff::Request &req, 
00155                             arm_navigation_msgs::SetPlanningSceneDiff::Response &res) 
00156   {
00157     ros::WallTime s1 = ros::WallTime::now();
00158     if(use_monitor_) {
00159       planning_monitor_->getCompletePlanningScene(req.planning_scene_diff,
00160                                                   req.operations,
00161                                                   res.planning_scene);
00162     } else {
00163       res.planning_scene = req.planning_scene_diff;
00164       if(req.operations.collision_operations.size() > 0) {
00165         std::vector<std::string> o_strings;
00166         for(unsigned int i = 0; i < req.planning_scene_diff.collision_objects.size(); i++) {
00167           o_strings.push_back(req.planning_scene_diff.collision_objects[i].id);
00168         }
00169         if(req.planning_scene_diff.collision_map.boxes.size() > 0) {
00170           o_strings.push_back(COLLISION_MAP_NAME);
00171         }
00172         std::vector<std::string> a_strings;
00173         for(unsigned int i = 0; i < req.planning_scene_diff.attached_collision_objects.size(); i++) {
00174           a_strings.push_back(req.planning_scene_diff.attached_collision_objects[i].object.id);
00175         }
00176         res.planning_scene.allowed_collision_matrix = 
00177           applyOrderedCollisionOperationsToCollisionsModel(collision_models_,
00178                                                            req.operations, 
00179                                                            o_strings,
00180                                                            a_strings);
00181       }
00182     }
00183     arm_navigation_msgs::SyncPlanningSceneGoal planning_scene_goal;
00184     planning_scene_goal.planning_scene = res.planning_scene;
00185     for(std::map<std::string, actionlib::SimpleActionClient<arm_navigation_msgs::SyncPlanningSceneAction>* >::iterator it = sync_planning_scene_clients_.begin();
00186         it != sync_planning_scene_clients_.end();
00187         it++) {
00188       it->second->sendGoal(planning_scene_goal);
00189     }
00190     std::vector<std::string> bad_list;
00191     for(std::map<std::string, actionlib::SimpleActionClient<arm_navigation_msgs::SyncPlanningSceneAction>* >::iterator it = sync_planning_scene_clients_.begin();
00192         it != sync_planning_scene_clients_.end();
00193         it++) {
00194       if(!it->second->waitForResult(PLANNING_SCENE_CLIENT_TIMEOUT)) {
00195         unsuccessful_planning_scene_client_replies_[it->first]++;
00196         ROS_INFO_STREAM("Did not get reply from planning scene client " << it->first 
00197                         << ".  Incrementing counter to " << unsuccessful_planning_scene_client_replies_[it->first]);
00198         if(unsuccessful_planning_scene_client_replies_[it->first] > UNSUCCESSFUL_REPLY_LIMIT) {
00199           ROS_WARN_STREAM("Failed to get reply from planning scene client " << it->first << " for " 
00200                           << UNSUCCESSFUL_REPLY_LIMIT << " consecutive tries.  Removing");
00201           bad_list.push_back(it->first);
00202         } 
00203         continue;
00204       } 
00205       unsuccessful_planning_scene_client_replies_[it->first] = 0;
00206     }
00207 
00208     for(unsigned int i = 0; i < bad_list.size(); i++) {
00209       delete sync_planning_scene_clients_[bad_list[i]];
00210       sync_planning_scene_clients_.erase(bad_list[i]);
00211     }
00212     ROS_DEBUG_STREAM("Setting planning scene diff took " << (ros::WallTime::now()-s1).toSec());
00213     return true;
00214   }
00215 
00216 private:
00217 
00218   boost::mutex register_lock_;
00219   ros::NodeHandle root_handle_, private_handle_;
00220   planning_environment::CollisionModels *collision_models_;
00221   planning_environment::PlanningMonitor *planning_monitor_;
00222   tf::TransformListener tf_;    
00223 
00224   bool use_monitor_;
00225   bool use_collision_map_;
00226 
00227   ros::ServiceServer get_robot_state_service_;
00228   ros::ServiceServer get_planning_scene_service_;
00229   ros::ServiceServer set_planning_scene_diff_service_;
00230 
00231   ros::ServiceServer register_planning_scene_service_;
00232   std::map<std::string, unsigned int> unsuccessful_planning_scene_client_replies_;
00233   std::map<std::string, actionlib::SimpleActionClient<arm_navigation_msgs::SyncPlanningSceneAction>* > sync_planning_scene_clients_;
00234 };    
00235 }
00236 
00237 int main(int argc, char** argv)
00238 {
00239   ros::init(argc, argv, "environment_server");
00240   //figuring out whether robot_description has been remapped
00241 
00242   ros::AsyncSpinner spinner(1); 
00243   spinner.start();
00244   planning_environment::EnvironmentServer environment_monitor;
00245   ros::waitForShutdown();
00246   return 0;
00247 }


planning_environment
Author(s): Ioan Sucan
autogenerated on Thu Dec 12 2013 11:09:24