current_state_validator.cpp
Go to the documentation of this file.
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 <ros/ros.h>
00038 
00039 #include <planning_environment/models/collision_models.h>
00040 #include <planning_environment/monitors/collision_space_monitor.h>
00041 #include <planning_environment/models/model_utils.h>
00042 
00043 #include <arm_navigation_msgs/GetStateValidity.h>
00044 
00045 #include <visualization_msgs/MarkerArray.h>
00046 #include <visualization_msgs/Marker.h>
00047 
00048 #include <tf/transform_listener.h>
00049 
00050 static const ros::Duration MARKER_DUR(.2);
00051 
00052 class CurrentStateValidator {
00053 
00054 public:
00055 
00056   CurrentStateValidator(void): priv_handle_("~")
00057   {
00058     std::string robot_description_name = root_handle_.resolveName("robot_description", true);
00059     
00060     collision_models_ = new planning_environment::CollisionModels(robot_description_name);
00061     csm_ = new planning_environment::CollisionSpaceMonitor(collision_models_, &tf_);
00062 
00063     csm_->waitForState();
00064     csm_->setUseCollisionMap(true);
00065     csm_->startEnvironmentMonitor();
00066 
00067     priv_handle_.param<std::string>("group_name_1", group_name_1_, "");
00068     priv_handle_.param<std::string>("group_name_2", group_name_2_, "");
00069 
00070     priv_handle_.param<bool>("send_markers", send_markers_, "false");
00071 
00072     vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("current_state_validator", 128);
00073     vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("current_state_validator_array", 128);
00074     
00075     csm_->addOnStateUpdateCallback(boost::bind(&CurrentStateValidator::jointStateCallback, this, _1));
00076 
00077     get_state_validity_service_ = priv_handle_.advertiseService("get_state_validity", &CurrentStateValidator::getStateValidity, this);
00078   }
00079 
00080   bool getStateValidity(arm_navigation_msgs::GetStateValidity::Request &req, 
00081                         arm_navigation_msgs::GetStateValidity::Response &res) 
00082   {
00083     collision_models_->bodiesLock();
00084 
00085     collision_space::EnvironmentModel::AllowedCollisionMatrix acm = collision_models_->getDefaultAllowedCollisionMatrix();
00086 
00087     if(!acm.hasEntry(COLLISION_MAP_NAME)) {
00088       acm.addEntry(COLLISION_MAP_NAME, false);
00089       collision_models_->setAlteredAllowedCollisionMatrix(acm);
00090     } 
00091 
00092     if(!req.group_name.empty()) {
00093       collision_models_->disableCollisionsForNonUpdatedLinks(req.group_name, true);
00094     }    
00095 
00096     planning_models::KinematicState state(collision_models_->getKinematicModel());
00097     csm_->setStateValuesFromCurrentValues(state);
00098 
00099     planning_environment::setRobotStateAndComputeTransforms(req.robot_state,
00100                                                             state);
00101     arm_navigation_msgs::Constraints goal_constraints;
00102     if(req.check_goal_constraints) {
00103       goal_constraints = req.goal_constraints;
00104     }
00105     arm_navigation_msgs::Constraints path_constraints;
00106     if(req.check_path_constraints) {
00107       path_constraints = req.path_constraints;
00108     }
00109     std::vector<std::string> joint_names;
00110     if(req.check_joint_limits) {
00111       joint_names = req.robot_state.joint_state.name;
00112     }
00113     collision_models_->isKinematicStateValid(state,
00114                                              joint_names,
00115                                              res.error_code,
00116                                              goal_constraints,
00117                                              path_constraints);
00118 
00119     if(!req.group_name.empty()) {
00120       collision_models_->revertAllowedCollisionToDefault();
00121     }    
00122 
00123     collision_models_->bodiesUnlock();
00124     return true;
00125 
00126   }
00127 
00128   void sendMarkersForGroup(const std::string& group) {
00129 
00130     collision_models_->bodiesLock();
00131     
00132     collision_space::EnvironmentModel::AllowedCollisionMatrix acm = collision_models_->getDefaultAllowedCollisionMatrix();
00133 
00134     if(!acm.hasEntry(COLLISION_MAP_NAME)) {
00135       acm.addEntry(COLLISION_MAP_NAME, false);
00136       collision_models_->setAlteredAllowedCollisionMatrix(acm);
00137     } 
00138 
00139     //ROS_INFO_STREAM("Has collision map " << collision_models_->getCollisionSpace()->hasObject(COLLISION_MAP_NAME));
00140 
00141     if(!group.empty()) {
00142       collision_models_->disableCollisionsForNonUpdatedLinks(group, true);
00143     }    
00144 
00145     planning_models::KinematicState state(collision_models_->getKinematicModel());
00146     csm_->setStateValuesFromCurrentValues(state);
00147     
00148     std_msgs::ColorRGBA good_color;
00149     good_color.a = 1.0;
00150     good_color.r = 0.1;
00151     good_color.g = 0.8;
00152     good_color.b = 0.3;
00153 
00154     std_msgs::ColorRGBA bad_color;
00155     bad_color.a = 1.0;
00156     bad_color.r = 1.0;
00157     bad_color.g = 0.0;
00158     bad_color.b = 0.0;
00159 
00160     std_msgs::ColorRGBA col;
00161     bool send = true;
00162     if(collision_models_->isKinematicStateInCollision(state)) {
00163       col = bad_color; 
00164       ROS_DEBUG_STREAM("Setting bad color");
00165     } else {
00166       send = false;
00167       col = good_color;
00168       ROS_DEBUG_STREAM("Setting good color");
00169     }
00170     
00171     visualization_msgs::MarkerArray arr;
00172     if(group.empty()) {
00173       collision_models_->getRobotMarkersGivenState(state,
00174                                                    arr,
00175                                                    col,
00176                                                    "validator",
00177                                                    ros::Duration(MARKER_DUR));
00178 
00179     } else {
00180       const planning_models::KinematicModel::JointModelGroup* joint_model_group = collision_models_->getKinematicModel()->getModelGroup(group);
00181       if(joint_model_group == NULL) {
00182         ROS_INFO_STREAM("No joint group " << group);
00183       }
00184       std::vector<std::string> group_links = joint_model_group->getGroupLinkNames();
00185       collision_models_->getRobotMarkersGivenState(state,
00186                                                    arr,
00187                                                    col,
00188                                                    group,
00189                                                    ros::Duration(MARKER_DUR),
00190                                                    &group_links);      
00191     }
00192     if(send) {
00193       vis_marker_array_publisher_.publish(arr);
00194     }
00195     if(!group.empty()) {
00196       collision_models_->revertAllowedCollisionToDefault();
00197     }    
00198     collision_models_->bodiesUnlock();
00199   }
00200 
00201   void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state) {
00202     if((ros::Time::now()-last_update_time_).toSec() < (MARKER_DUR.toSec()/2.0)) {
00203       return;
00204     }
00205     last_update_time_ = ros::Time::now();
00206     if(!send_markers_) return;
00207     collision_models_->bodiesLock();
00208     if(group_name_1_.empty() && group_name_2_.empty()) {
00209       sendMarkersForGroup("");
00210     } 
00211     if(!group_name_1_.empty()) {
00212       sendMarkersForGroup(group_name_1_);
00213     } 
00214     if(!group_name_2_.empty()) {
00215       sendMarkersForGroup(group_name_2_);
00216     }
00217     collision_models_->bodiesUnlock();
00218   }
00219 protected:
00220 
00221   ros::ServiceServer get_state_validity_service_;
00222     
00223   planning_environment::CollisionModels* collision_models_;
00224   planning_environment::CollisionSpaceMonitor* csm_;
00225 
00226   std::string group_name_1_;
00227   std::string group_name_2_;
00228 
00229   bool send_markers_;
00230   ros::Time last_update_time_;
00231 
00232   ros::NodeHandle root_handle_;
00233   ros::NodeHandle priv_handle_;
00234 
00235   ros::Publisher vis_marker_publisher_;
00236   ros::Publisher vis_marker_array_publisher_;
00237 
00238   tf::TransformListener tf_;
00239 };  
00240 
00241 int main(int argc, char **argv)
00242 {
00243   ros::init(argc, argv, "current_state_validator");
00244 
00245   ros::AsyncSpinner spinner(1); // Use 2 threads
00246   spinner.start();
00247   
00248   CurrentStateValidator cko;
00249   ros::waitForShutdown();
00250   
00251   return 0;
00252 }
00253 


arm_navigation_experimental_tools
Author(s): Gil Jones
autogenerated on Thu Dec 12 2013 11:07:52