move_group.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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 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: Ioan Sucan */
00036 
00037 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00038 #include <tf/transform_listener.h>
00039 #include <moveit/move_group/capability_names.h>
00040 #include <moveit/move_group/move_group_capability.h>
00041 #include <boost/algorithm/string/join.hpp>
00042 #include <boost/tokenizer.hpp>
00043 #include <moveit/macros/console_colors.h>
00044 #include <moveit/move_group/node_name.h>
00045 #include <set>
00046 
00047 static const std::string ROBOT_DESCRIPTION =
00048     "robot_description";  // name of the robot description (a param name, so it can be changed externally)
00049 
00050 namespace move_group
00051 {
00052 class MoveGroupExe
00053 {
00054 public:
00055   MoveGroupExe(const planning_scene_monitor::PlanningSceneMonitorPtr& psm, bool debug) : node_handle_("~")
00056   {
00057     // if the user wants to be able to disable execution of paths, they can just set this ROS param to false
00058     bool allow_trajectory_execution;
00059     node_handle_.param("allow_trajectory_execution", allow_trajectory_execution, true);
00060 
00061     context_.reset(new MoveGroupContext(psm, allow_trajectory_execution, debug));
00062 
00063     // start the capabilities
00064     configureCapabilities();
00065   }
00066 
00067   ~MoveGroupExe()
00068   {
00069     capabilities_.clear();
00070     context_.reset();
00071     capability_plugin_loader_.reset();
00072   }
00073 
00074   void status()
00075   {
00076     if (context_)
00077     {
00078       if (context_->status())
00079       {
00080         if (capabilities_.empty())
00081           printf(MOVEIT_CONSOLE_COLOR_BLUE "\nAll is well but no capabilities are loaded. There will be no party "
00082                                            ":(\n\n" MOVEIT_CONSOLE_COLOR_RESET);
00083         else
00084           printf(MOVEIT_CONSOLE_COLOR_GREEN "\nAll is well! Everyone is happy! You can start planning "
00085                                             "now!\n\n" MOVEIT_CONSOLE_COLOR_RESET);
00086         fflush(stdout);
00087       }
00088     }
00089     else
00090       ROS_ERROR("No MoveGroup context created. Nothing will work.");
00091   }
00092 
00093 private:
00094   void configureCapabilities()
00095   {
00096     try
00097     {
00098       capability_plugin_loader_.reset(
00099           new pluginlib::ClassLoader<MoveGroupCapability>("moveit_ros_move_group", "move_group::MoveGroupCapability"));
00100     }
00101     catch (pluginlib::PluginlibException& ex)
00102     {
00103       ROS_FATAL_STREAM("Exception while creating plugin loader for move_group capabilities: " << ex.what());
00104       return;
00105     }
00106 
00107     std::set<std::string> capabilities;
00108 
00109     // add default capabilities
00110     for (size_t i = 0; i < sizeof(DEFAULT_CAPABILITIES) / sizeof(DEFAULT_CAPABILITIES[0]); ++i)
00111       capabilities.insert(DEFAULT_CAPABILITIES[i]);
00112 
00113     // add capabilities listed in ROS parameter
00114     std::string capability_plugins;
00115     if (node_handle_.getParam("capabilities", capability_plugins))
00116     {
00117       boost::char_separator<char> sep(" ");
00118       boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
00119       capabilities.insert(tok.begin(), tok.end());
00120     }
00121 
00122     // drop capabilities that have been explicitly disabled
00123     if (node_handle_.getParam("disable_capabilities", capability_plugins))
00124     {
00125       boost::char_separator<char> sep(" ");
00126       boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
00127       for (boost::tokenizer<boost::char_separator<char> >::iterator cap_name = tok.begin(); cap_name != tok.end();
00128            ++cap_name)
00129         capabilities.erase(*cap_name);
00130     }
00131 
00132     for (std::set<std::string>::iterator plugin = capabilities.begin(); plugin != capabilities.end(); ++plugin)
00133     {
00134       try
00135       {
00136         printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'...\n" MOVEIT_CONSOLE_COLOR_RESET, plugin->c_str());
00137         MoveGroupCapability* cap = capability_plugin_loader_->createUnmanagedInstance(*plugin);
00138         cap->setContext(context_);
00139         cap->initialize();
00140         capabilities_.push_back(MoveGroupCapabilityPtr(cap));
00141       }
00142       catch (pluginlib::PluginlibException& ex)
00143       {
00144         ROS_ERROR_STREAM("Exception while loading move_group capability '"
00145                          << *plugin << "': " << ex.what() << std::endl
00146                          << "Available capabilities: "
00147                          << boost::algorithm::join(capability_plugin_loader_->getDeclaredClasses(), ", "));
00148       }
00149     }
00150 
00151     std::stringstream ss;
00152     ss << std::endl;
00153     ss << std::endl;
00154     ss << "********************************************************" << std::endl;
00155     ss << "* MoveGroup using: " << std::endl;
00156     for (std::size_t i = 0; i < capabilities_.size(); ++i)
00157       ss << "*     - " << capabilities_[i]->getName() << std::endl;
00158     ss << "********************************************************" << std::endl;
00159     ROS_INFO_STREAM(ss.str());
00160   }
00161 
00162   ros::NodeHandle node_handle_;
00163   MoveGroupContextPtr context_;
00164   boost::shared_ptr<pluginlib::ClassLoader<MoveGroupCapability> > capability_plugin_loader_;
00165   std::vector<MoveGroupCapabilityPtr> capabilities_;
00166 };
00167 }
00168 
00169 int main(int argc, char** argv)
00170 {
00171   ros::init(argc, argv, move_group::NODE_NAME);
00172 
00173   ros::AsyncSpinner spinner(1);
00174   spinner.start();
00175 
00176   boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener(ros::Duration(10.0)));
00177 
00178   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(
00179       new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION, tf));
00180 
00181   if (planning_scene_monitor->getPlanningScene())
00182   {
00183     bool debug = false;
00184     for (int i = 1; i < argc; ++i)
00185       if (strncmp(argv[i], "--debug", 7) == 0)
00186       {
00187         debug = true;
00188         break;
00189       }
00190     if (debug)
00191       ROS_INFO("MoveGroup debug mode is ON");
00192     else
00193       ROS_INFO("MoveGroup debug mode is OFF");
00194 
00195     printf(MOVEIT_CONSOLE_COLOR_CYAN "Starting context monitors...\n" MOVEIT_CONSOLE_COLOR_RESET);
00196     planning_scene_monitor->startSceneMonitor();
00197     planning_scene_monitor->startWorldGeometryMonitor();
00198     planning_scene_monitor->startStateMonitor();
00199     printf(MOVEIT_CONSOLE_COLOR_CYAN "Context monitors started.\n" MOVEIT_CONSOLE_COLOR_RESET);
00200 
00201     move_group::MoveGroupExe mge(planning_scene_monitor, debug);
00202 
00203     planning_scene_monitor->publishDebugInformation(debug);
00204 
00205     mge.status();
00206 
00207     ros::waitForShutdown();
00208   }
00209   else
00210     ROS_ERROR("Planning scene not configured");
00211 
00212   return 0;
00213 }


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Nov 26 2018 03:23:11