41 #include <boost/tokenizer.hpp> 55 "move_group/MoveGroupCartesianPathService",
56 "move_group/MoveGroupKinematicsService",
57 "move_group/MoveGroupExecuteTrajectoryAction",
58 "move_group/MoveGroupMoveAction",
59 "move_group/MoveGroupPickPlaceAction",
60 "move_group/MoveGroupPlanService",
61 "move_group/MoveGroupQueryPlannersService",
62 "move_group/MoveGroupStateValidationService",
63 "move_group/MoveGroupGetPlanningSceneService",
64 "move_group/ApplyPlanningSceneService",
65 "move_group/ClearOctomapService",
75 bool allow_trajectory_execution;
76 node_handle_.
param(
"allow_trajectory_execution", allow_trajectory_execution,
true);
106 ROS_ERROR(
"No MoveGroup context created. Nothing will work.");
119 ROS_FATAL_STREAM(
"Exception while creating plugin loader for move_group capabilities: " << ex.what());
123 std::set<std::string> capabilities;
127 capabilities.insert(DEFAULT_CAPABILITIES[i]);
130 std::string capability_plugins;
133 boost::char_separator<char> sep(
" ");
134 boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
135 capabilities.insert(tok.begin(), tok.end());
141 boost::char_separator<char> sep(
" ");
142 boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
143 for (boost::tokenizer<boost::char_separator<char> >::iterator cap_name = tok.begin(); cap_name != tok.end();
145 capabilities.erase(*cap_name);
148 for (std::set<std::string>::iterator plugin = capabilities.cbegin(); plugin != capabilities.cend(); ++plugin)
160 ROS_ERROR_STREAM(
"Exception while loading move_group capability '" << *plugin <<
"': " << ex.what());
164 std::stringstream ss;
167 ss <<
"********************************************************" << std::endl;
168 ss <<
"* MoveGroup using: " << std::endl;
171 ss <<
"********************************************************" << std::endl;
182 int main(
int argc,
char** argv)
194 if (planning_scene_monitor->getPlanningScene())
197 for (
int i = 1; i < argc; ++i)
198 if (strncmp(argv[i],
"--debug", 7) == 0)
204 ROS_INFO(
"MoveGroup debug mode is ON");
206 ROS_INFO(
"MoveGroup debug mode is OFF");
209 planning_scene_monitor->startSceneMonitor();
210 planning_scene_monitor->startWorldGeometryMonitor();
211 planning_scene_monitor->startStateMonitor();
216 planning_scene_monitor->publishDebugInformation(debug);
223 ROS_ERROR(
"Planning scene not configured");
MoveGroupContextPtr context_
#define MOVEIT_CONSOLE_COLOR_RESET
static const std::string NODE_NAME
static const char * DEFAULT_CAPABILITIES[]
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define MOVEIT_CONSOLE_COLOR_CYAN
std::string getName(void *handle)
MoveGroupExe(const planning_scene_monitor::PlanningSceneMonitorPtr &psm, bool debug)
void configureCapabilities()
std::shared_ptr< pluginlib::ClassLoader< MoveGroupCapability > > capability_plugin_loader_
int main(int argc, char **argv)
#define ROS_FATAL_STREAM(args)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
ros::NodeHandle node_handle_
std::vector< MoveGroupCapabilityPtr > capabilities_
static const std::string ROBOT_DESCRIPTION
#define ROS_INFO_STREAM(args)
#define MOVEIT_CONSOLE_COLOR_GREEN
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_STREAM(args)
#define MOVEIT_CONSOLE_COLOR_BLUE
ROSCPP_DECL void waitForShutdown()