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/move_group_capability.h>
00040 #include <boost/algorithm/string/join.hpp>
00041 #include <boost/tokenizer.hpp>
00042 #include <moveit/macros/console_colors.h>
00043 #include <moveit/move_group/node_name.h>
00044
00045 static const std::string ROBOT_DESCRIPTION = "robot_description";
00046
00047 namespace move_group
00048 {
00049
00050 class MoveGroupExe
00051 {
00052 public:
00053
00054 MoveGroupExe(const planning_scene_monitor::PlanningSceneMonitorPtr& psm, bool debug) :
00055 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 "\nAll is well but no capabilities are loaded. There will be no party :(\n\n" MOVEIT_CONSOLE_COLOR_RESET);
00082 else
00083 printf(MOVEIT_CONSOLE_COLOR_GREEN "\nAll is well! Everyone is happy! You can start planning now!\n\n" MOVEIT_CONSOLE_COLOR_RESET);
00084 fflush(stdout);
00085 }
00086 }
00087 else
00088 ROS_ERROR("No MoveGroup context created. Nothing will work.");
00089 }
00090
00091 private:
00092
00093 void configureCapabilities()
00094 {
00095 try
00096 {
00097 capability_plugin_loader_.reset(new pluginlib::ClassLoader<MoveGroupCapability>("moveit_ros_move_group", "move_group::MoveGroupCapability"));
00098 }
00099 catch(pluginlib::PluginlibException& ex)
00100 {
00101 ROS_FATAL_STREAM("Exception while creating plugin loader for move_group capabilities: " << ex.what());
00102 return;
00103 }
00104
00105
00106 std::string capability_plugins;
00107 if (node_handle_.getParam("capabilities", capability_plugins))
00108 {
00109 boost::char_separator<char> sep(" ");
00110 boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
00111 for(boost::tokenizer<boost::char_separator<char> >::iterator beg = tok.begin() ; beg != tok.end(); ++beg)
00112 {
00113 std::string plugin = *beg;
00114 try
00115 {
00116 printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'...\n" MOVEIT_CONSOLE_COLOR_RESET, plugin.c_str());
00117 MoveGroupCapability *cap = capability_plugin_loader_->createUnmanagedInstance(plugin);
00118 cap->setContext(context_);
00119 cap->initialize();
00120 capabilities_.push_back(boost::shared_ptr<MoveGroupCapability>(cap));
00121 }
00122 catch(pluginlib::PluginlibException& ex)
00123 {
00124 ROS_ERROR_STREAM("Exception while loading move_group capability '" << plugin << "': " << ex.what() << std::endl
00125 << "Available capabilities: " << boost::algorithm::join(capability_plugin_loader_->getDeclaredClasses(), ", "));
00126 }
00127 }
00128 }
00129 std::stringstream ss;
00130 ss << std::endl;
00131 ss << std::endl;
00132 ss << "********************************************************" << std::endl;
00133 ss << "* MoveGroup using: " << std::endl;
00134 for (std::size_t i = 0 ; i < capabilities_.size() ; ++i)
00135 ss << "* - " << capabilities_[i]->getName() << std::endl;
00136 ss << "********************************************************" << std::endl;
00137 ROS_INFO_STREAM(ss.str());
00138 }
00139
00140 ros::NodeHandle node_handle_;
00141 MoveGroupContextPtr context_;
00142 boost::shared_ptr<pluginlib::ClassLoader<MoveGroupCapability> > capability_plugin_loader_;
00143 std::vector<boost::shared_ptr<MoveGroupCapability> > capabilities_;
00144 };
00145
00146 }
00147
00148 int main(int argc, char **argv)
00149 {
00150 ros::init(argc, argv, move_group::NODE_NAME);
00151
00152 ros::AsyncSpinner spinner(1);
00153 spinner.start();
00154
00155 boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener(ros::Duration(10.0)));
00156
00157 planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION, tf));
00158
00159 if (planning_scene_monitor->getPlanningScene())
00160 {
00161 bool debug = false;
00162 for (int i = 1 ; i < argc ; ++i)
00163 if (strncmp(argv[i], "--debug", 7) == 0)
00164 {
00165 debug = true;
00166 break;
00167 }
00168 if (debug)
00169 ROS_INFO("MoveGroup debug mode is ON");
00170 else
00171 ROS_INFO("MoveGroup debug mode is OFF");
00172
00173 printf(MOVEIT_CONSOLE_COLOR_CYAN "Starting context monitors...\n" MOVEIT_CONSOLE_COLOR_RESET);
00174 planning_scene_monitor->startSceneMonitor();
00175 planning_scene_monitor->startWorldGeometryMonitor();
00176 planning_scene_monitor->startStateMonitor();
00177 printf(MOVEIT_CONSOLE_COLOR_CYAN "Context monitors started.\n" MOVEIT_CONSOLE_COLOR_RESET);
00178
00179 move_group::MoveGroupExe mge(planning_scene_monitor, debug);
00180
00181 planning_scene_monitor->publishDebugInformation(debug);
00182
00183 mge.status();
00184
00185 ros::waitForShutdown();
00186 }
00187 else
00188 ROS_ERROR("Planning scene not configured");
00189
00190 return 0;
00191 }