Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
00122
00123 if(set_planning_scene_callback_ != NULL) {
00124 set_planning_scene_callback_(scene->planning_scene);
00125 }
00126
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
00150
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 }