kinematic_options_map.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014, SRI International
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 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 
00035 /* Author: Acorn Pooley */
00036 
00037 #include <moveit/robot_interaction/kinematic_options_map.h>
00038 #include <ros/console.h>
00039 #include <algorithm>
00040 
00041 // These strings have no content.  They are compared by address.
00042 const std::string robot_interaction::KinematicOptionsMap::DEFAULT = "";
00043 const std::string robot_interaction::KinematicOptionsMap::ALL = "";
00044 
00045 robot_interaction::KinematicOptionsMap::KinematicOptionsMap()
00046 {
00047 }
00048 
00049 // Returns a copy of the KinematicOptions so that the caller does not need to
00050 // worry about locking.
00051 robot_interaction::KinematicOptions robot_interaction::KinematicOptionsMap::getOptions(const std::string& key) const
00052 {
00053   boost::mutex::scoped_lock lock(lock_);
00054 
00055   if (&key == &DEFAULT)
00056     return defaults_;
00057 
00058   M_options::const_iterator it = options_.find(key);
00059   if (it == options_.end())
00060     return defaults_;
00061   return it->second;
00062 }
00063 
00064 void robot_interaction::KinematicOptionsMap::setOptions(const std::string& key, const KinematicOptions& options_delta,
00065                                                         KinematicOptions::OptionBitmask fields)
00066 {
00067   boost::mutex::scoped_lock lock(lock_);
00068 
00069   if (&key == &ALL)
00070   {
00071     if (fields == KinematicOptions::ALL)
00072     {
00073       // setting ALL fields for ALL keys
00074       // so just clear all key-specific fields and set the defaults.
00075       defaults_ = options_delta;
00076       options_.clear();
00077       return;
00078     }
00079 
00080     defaults_.setOptions(options_delta, fields);
00081     for (M_options::iterator it = options_.begin(); it != options_.end(); ++it)
00082     {
00083       it->second.setOptions(options_delta, fields);
00084     }
00085     return;
00086   }
00087 
00088   if (&key == &DEFAULT)
00089   {
00090     defaults_.setOptions(options_delta, fields);
00091     return;
00092   }
00093 
00094   M_options::iterator it = options_.find(key);
00095   KinematicOptions* opts;
00096   if (it == options_.end())
00097   {
00098     // create new entry for key and initialize to defaults_
00099     opts = &options_[key];
00100     *opts = defaults_;
00101   }
00102   else
00103   {
00104     opts = &it->second;
00105   }
00106 
00107   opts->setOptions(options_delta, fields);
00108 }
00109 
00110 // merge other into this.  All options in other take precedence over this.
00111 void robot_interaction::KinematicOptionsMap::merge(const KinematicOptionsMap& other)
00112 {
00113   if (&other == this)
00114     return;
00115 
00116   // need to lock in consistent order to avoid deadlock.
00117   // Lock the one with lower address first.
00118   boost::mutex* m1 = &lock_;
00119   boost::mutex* m2 = &other.lock_;
00120   if (m2 < m1)
00121     std::swap(m1, m2);
00122   boost::mutex::scoped_lock lock1(*m1);
00123   boost::mutex::scoped_lock lock2(*m2);
00124 
00125   defaults_ = other.defaults_;
00126   for (M_options::const_iterator it = other.options_.begin(); it != other.options_.end(); ++it)
00127   {
00128     options_[it->first] = it->second;
00129   }
00130 }
00131 
00132 // This is intended to be called as a ModifyStateFunction to modify the state
00133 // maintained by a LockedRobotState in place.
00134 bool robot_interaction::KinematicOptionsMap::setStateFromIK(robot_state::RobotState& state, const std::string& key,
00135                                                             const std::string& group, const std::string& tip,
00136                                                             const geometry_msgs::Pose& pose) const
00137 {
00138   // copy options so lock is not needed during IK solve.
00139   KinematicOptions options = getOptions(key);
00140   return options.setStateFromIK(state, group, tip, pose);
00141 }


robot_interaction
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:21:55