Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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";
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
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
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 "\nmove_group is running but no capabilities are "
00082 "loaded.\n\n" MOVEIT_CONSOLE_COLOR_RESET);
00083 else
00084 printf(MOVEIT_CONSOLE_COLOR_GREEN "\nYou can start planning now!\n\n" MOVEIT_CONSOLE_COLOR_RESET);
00085 fflush(stdout);
00086 }
00087 }
00088 else
00089 ROS_ERROR("No MoveGroup context created. Nothing will work.");
00090 }
00091
00092 private:
00093 void configureCapabilities()
00094 {
00095 try
00096 {
00097 capability_plugin_loader_.reset(
00098 new pluginlib::ClassLoader<MoveGroupCapability>("moveit_ros_move_group", "move_group::MoveGroupCapability"));
00099 }
00100 catch (pluginlib::PluginlibException& ex)
00101 {
00102 ROS_FATAL_STREAM("Exception while creating plugin loader for move_group capabilities: " << ex.what());
00103 return;
00104 }
00105
00106 std::set<std::string> capabilities;
00107
00108
00109 for (size_t i = 0; i < sizeof(DEFAULT_CAPABILITIES) / sizeof(DEFAULT_CAPABILITIES[0]); ++i)
00110 capabilities.insert(DEFAULT_CAPABILITIES[i]);
00111
00112
00113 std::string capability_plugins;
00114 if (node_handle_.getParam("capabilities", capability_plugins))
00115 {
00116 boost::char_separator<char> sep(" ");
00117 boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
00118 capabilities.insert(tok.begin(), tok.end());
00119 }
00120
00121
00122 if (node_handle_.getParam("disable_capabilities", capability_plugins))
00123 {
00124 boost::char_separator<char> sep(" ");
00125 boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
00126 for (boost::tokenizer<boost::char_separator<char> >::iterator cap_name = tok.begin(); cap_name != tok.end();
00127 ++cap_name)
00128 capabilities.erase(*cap_name);
00129 }
00130
00131 for (std::set<std::string>::iterator plugin = capabilities.begin(); plugin != capabilities.end(); ++plugin)
00132 {
00133 try
00134 {
00135 printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'...\n" MOVEIT_CONSOLE_COLOR_RESET, plugin->c_str());
00136 MoveGroupCapability* cap = capability_plugin_loader_->createUnmanagedInstance(*plugin);
00137 cap->setContext(context_);
00138 cap->initialize();
00139 capabilities_.push_back(MoveGroupCapabilityPtr(cap));
00140 }
00141 catch (pluginlib::PluginlibException& ex)
00142 {
00143 ROS_ERROR_STREAM("Exception while loading move_group capability '"
00144 << *plugin << "': " << ex.what() << std::endl
00145 << "Available capabilities: "
00146 << boost::algorithm::join(capability_plugin_loader_->getDeclaredClasses(), ", "));
00147 }
00148 }
00149
00150 std::stringstream ss;
00151 ss << std::endl;
00152 ss << std::endl;
00153 ss << "********************************************************" << std::endl;
00154 ss << "* MoveGroup using: " << std::endl;
00155 for (std::size_t i = 0; i < capabilities_.size(); ++i)
00156 ss << "* - " << capabilities_[i]->getName() << std::endl;
00157 ss << "********************************************************" << std::endl;
00158 ROS_INFO_STREAM(ss.str());
00159 }
00160
00161 ros::NodeHandle node_handle_;
00162 MoveGroupContextPtr context_;
00163 boost::shared_ptr<pluginlib::ClassLoader<MoveGroupCapability> > capability_plugin_loader_;
00164 std::vector<MoveGroupCapabilityPtr> capabilities_;
00165 };
00166 }
00167
00168 int main(int argc, char** argv)
00169 {
00170 ros::init(argc, argv, move_group::NODE_NAME);
00171
00172 ros::AsyncSpinner spinner(1);
00173 spinner.start();
00174
00175 boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener(ros::Duration(10.0)));
00176
00177 planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(
00178 new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION, tf));
00179
00180 if (planning_scene_monitor->getPlanningScene())
00181 {
00182 bool debug = false;
00183 for (int i = 1; i < argc; ++i)
00184 if (strncmp(argv[i], "--debug", 7) == 0)
00185 {
00186 debug = true;
00187 break;
00188 }
00189 if (debug)
00190 ROS_INFO("MoveGroup debug mode is ON");
00191 else
00192 ROS_INFO("MoveGroup debug mode is OFF");
00193
00194 printf(MOVEIT_CONSOLE_COLOR_CYAN "Starting context monitors...\n" MOVEIT_CONSOLE_COLOR_RESET);
00195 planning_scene_monitor->startSceneMonitor();
00196 planning_scene_monitor->startWorldGeometryMonitor();
00197 planning_scene_monitor->startStateMonitor();
00198 printf(MOVEIT_CONSOLE_COLOR_CYAN "Context monitors started.\n" MOVEIT_CONSOLE_COLOR_RESET);
00199
00200 move_group::MoveGroupExe mge(planning_scene_monitor, debug);
00201
00202 planning_scene_monitor->publishDebugInformation(debug);
00203
00204 mge.status();
00205
00206 ros::waitForShutdown();
00207 }
00208 else
00209 ROS_ERROR("Planning scene not configured");
00210
00211 return 0;
00212 }