$search
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 return true; 00072 } 00073 00074 bool getStateValidity(arm_navigation_msgs::GetStateValidity::Request &req, 00075 arm_navigation_msgs::GetStateValidity::Response &res) 00076 { 00077 collision_models_interface_->bodiesLock(); 00078 if(!collision_models_interface_->isPlanningSceneSet()) { 00079 res.error_code.val = res.error_code.COLLISION_CHECKING_UNAVAILABLE; 00080 ROS_WARN_STREAM("Calling getStateValidity with no planning scene set"); 00081 collision_models_interface_->bodiesUnlock(); 00082 return true; 00083 } 00084 collision_models_interface_->resetToStartState(*collision_models_interface_->getPlanningSceneState()); 00085 if(!req.group_name.empty()) { 00086 collision_models_interface_->disableCollisionsForNonUpdatedLinks(req.group_name); 00087 } 00088 planning_environment::setRobotStateAndComputeTransforms(req.robot_state, 00089 *collision_models_interface_->getPlanningSceneState()); 00090 arm_navigation_msgs::Constraints goal_constraints; 00091 if(req.check_goal_constraints) { 00092 goal_constraints = req.goal_constraints; 00093 } 00094 arm_navigation_msgs::Constraints path_constraints; 00095 if(req.check_path_constraints) { 00096 path_constraints = req.path_constraints; 00097 } 00098 std::vector<std::string> joint_names; 00099 if(req.check_joint_limits) { 00100 joint_names = req.robot_state.joint_state.name; 00101 } 00102 collision_models_interface_->isKinematicStateValid(*collision_models_interface_->getPlanningSceneState(), 00103 joint_names, 00104 res.error_code, 00105 goal_constraints, 00106 path_constraints); 00107 collision_models_interface_->bodiesUnlock(); 00108 return true; 00109 00110 } 00111 00112 void broadcastPlanningSceneMarkers() 00113 { 00114 collision_models_interface_->bodiesLock(); 00115 if(!collision_models_interface_->isPlanningSceneSet()) { 00116 collision_models_interface_->bodiesUnlock(); 00117 return; 00118 } 00119 collision_models_interface_->resetToStartState(*collision_models_interface_->getPlanningSceneState()); 00120 visualization_msgs::MarkerArray arr; 00121 std_msgs::ColorRGBA col; 00122 col.a = .9; 00123 col.r = 0.0; 00124 col.b = 1.0; 00125 col.g = 0.0; 00126 collision_models_interface_->getCollisionMapAsMarkers(arr, 00127 col, 00128 ros::Duration(0.2)); 00129 col.g = 1.0; 00130 col.b = 1.0; 00131 collision_models_interface_->getStaticCollisionObjectMarkers(arr, 00132 "", 00133 col, 00134 ros::Duration(0.2)); 00135 00136 col.r = 0.6; 00137 col.g = 0.4; 00138 col.b = 0.3; 00139 00140 collision_models_interface_->getAttachedCollisionObjectMarkers(*collision_models_interface_->getPlanningSceneState(), 00141 arr, 00142 "", 00143 col, 00144 ros::Duration(0.2)); 00145 vis_marker_array_publisher_.publish(arr); 00146 collision_models_interface_->bodiesUnlock(); 00147 } 00148 00149 00150 private: 00151 00152 00153 ros::NodeHandle root_handle_, private_handle_; 00154 planning_environment::CollisionModelsInterface *collision_models_interface_; 00155 00156 ros::ServiceServer get_trajectory_validity_service_; 00157 ros::ServiceServer get_state_validity_service_; 00158 00159 ros::Publisher vis_marker_publisher_; 00160 ros::Publisher vis_marker_array_publisher_; 00161 }; 00162 } 00163 00164 int main(int argc, char** argv) 00165 { 00166 ros::init(argc, argv, "planning_scene_validity_server"); 00167 //figuring out whether robot_description has been remapped 00168 00169 ros::AsyncSpinner spinner(1); 00170 spinner.start(); 00171 ros::NodeHandle nh; 00172 planning_environment::PlanningSceneValidityServer validity_server; 00173 ros::Rate r(10.0); 00174 while(nh.ok()) { 00175 validity_server.broadcastPlanningSceneMarkers(); 00176 r.sleep(); 00177 } 00178 ros::waitForShutdown(); 00179 return 0; 00180 }