collision_proximity: collision_proximity: collision_proximity_server

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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 <ros/ros.h>
00038 #include <collision_proximity/collision_proximity_space.h>
00039 #include <planning_environment/models/model_utils.h>
00040 #include <arm_navigation_msgs/GetMotionPlan.h>
00041 #include <arm_navigation_msgs/GetStateValidity.h>
00042 
00043 struct CollisionProximitySpacePlannerInterface
00044 {
00045   CollisionProximitySpacePlannerInterface(const std::string& robot_description_name)
00046   {
00047     cps_ = new collision_proximity::CollisionProximitySpace(robot_description_name);
00048     
00049     ros::NodeHandle priv("~");
00050 
00051     motion_planning_service_ = priv.advertiseService("get_distance_aware_plan", &CollisionProximitySpacePlannerInterface::motionPlanCallback, this);
00052 
00053     get_state_validity_service_ = priv.advertiseService("get_state_validity", &CollisionProximitySpacePlannerInterface::getStateValidity, this);
00054 
00055     vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("collision_proximity_markers", 128);
00056     vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("collision_proximity_markers_array", 128);
00057 
00058   }
00059 
00060   ~CollisionProximitySpacePlannerInterface()
00061   {
00062     delete cps_;
00063   }
00064   
00065   bool motionPlanCallback(arm_navigation_msgs::GetMotionPlan::Request &req,
00066                           arm_navigation_msgs::GetMotionPlan::Response &res)
00067   {
00068     if(!req.motion_plan_request.group_name.empty()) {
00069       ROS_INFO_STREAM("Setting up for group " << req.motion_plan_request.group_name);
00070       cps_->setupForGroupQueries(req.motion_plan_request.group_name,
00071                                  req.motion_plan_request.start_state,
00072                                  current_link_names_,
00073                                  current_attached_body_names_);
00074     } else {
00075       return false;
00076     }
00077     return true;
00078   }
00079 
00080   bool getStateValidity(arm_navigation_msgs::GetStateValidity::Request &req, 
00081                         arm_navigation_msgs::GetStateValidity::Response &res) 
00082   {
00083     cps_->getCollisionModelsInterface()->bodiesLock();
00084     if(!cps_->getCollisionModelsInterface()->isPlanningSceneSet()) {
00085       cps_->getCollisionModelsInterface()->bodiesUnlock();
00086       return true;
00087     }
00088     planning_environment::setRobotStateAndComputeTransforms(req.robot_state,
00089                                                             *cps_->getCollisionModelsInterface()->getPlanningSceneState());
00090     cps_->getCollisionModelsInterface()->bodiesUnlock();
00091     return true;
00092   }
00093   
00094   void broadcastCollisionMarkers() {
00095     cps_->getCollisionModelsInterface()->bodiesLock();
00096     if(!cps_->getCollisionModelsInterface()->isPlanningSceneSet() || cps_->getCurrentGroupName().empty()) {
00097       cps_->getCollisionModelsInterface()->bodiesUnlock();
00098       return;
00099     }
00100     cps_->setCurrentGroupState(*cps_->getCollisionModelsInterface()->getPlanningSceneState());
00101     std::vector<collision_proximity::GradientInfo> gradients;
00102     cps_->getStateGradients(gradients);
00103 
00104     visualization_msgs::MarkerArray arr;
00105     cps_->getProximityGradientMarkers(current_link_names_,
00106                                       current_attached_body_names_,
00107                                       gradients,
00108                                       "",
00109                                       arr);
00110     cps_->visualizeObjectSpheres(current_link_names_);
00111     vis_marker_array_publisher_.publish(arr);
00112     cps_->getCollisionModelsInterface()->bodiesUnlock();
00113   }
00114 
00115   std::vector<std::string> current_link_names_;
00116   std::vector<std::string> current_attached_body_names_;
00117 
00118   ros::NodeHandle root_handle_;
00119   ros::ServiceServer motion_planning_service_;
00120   ros::ServiceServer get_state_validity_service_;
00121   collision_proximity::CollisionProximitySpace* cps_;
00122   ros::Publisher vis_marker_publisher_;
00123   ros::Publisher vis_marker_array_publisher_;
00124 };
00125 
00126 
00127 int main(int argc, char** argv)
00128 {
00129   ros::init(argc, argv, "collision_proximity_server_test");
00130 
00131   ros::AsyncSpinner spinner(1); 
00132   spinner.start();
00133  
00134   ros::NodeHandle nh;
00135   std::string robot_description_name = nh.resolveName("robot_description", true);
00136 
00137   CollisionProximitySpacePlannerInterface cps(robot_description_name);
00138 
00139   ros::Rate r(10.0);
00140   while(nh.ok()) {
00141     cps.broadcastCollisionMarkers();
00142     r.sleep();
00143   }
00144   ros::waitForShutdown();
00145   return 0;
00146 
00147 
00148 }
collision_proximity_server_test.cpp
