New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add list of default capabilities #359

Merged
merged 1 commit into from Nov 16, 2016
Jump to file or symbol
Failed to load files and symbols.
+72 −31
Diff settings

Always

Just for now

@@ -41,6 +41,25 @@
namespace move_group
{
// These capabilities are loaded unless listed in disable_capabilities
// clang-format off
static const char* DEFAULT_CAPABILITIES[] = {

This comment has been minimized.

@davetcoleman

davetcoleman Nov 15, 2016

Member

is char* really the best option here? MoveIt! almost always uses std::string - and this structure gets inserted into an std::string set

@davetcoleman

davetcoleman Nov 15, 2016

Member

is char* really the best option here? MoveIt! almost always uses std::string - and this structure gets inserted into an std::string set

This comment has been minimized.

@v4hn

v4hn Nov 15, 2016

Member

I call bike shed. I don't see how this makes any difference.

Incidentally, I used vector<string> before, but this requires braced-initializer-lists from C++11 and I decided to write this standard-agnostic because I'm unsure whether to backport it.
So I changed it to full C-style structure instead.

@v4hn

v4hn Nov 15, 2016

Member

I call bike shed. I don't see how this makes any difference.

Incidentally, I used vector<string> before, but this requires braced-initializer-lists from C++11 and I decided to write this standard-agnostic because I'm unsure whether to backport it.
So I changed it to full C-style structure instead.

This comment has been minimized.

"move_group/MoveGroupCartesianPathService",
"move_group/MoveGroupKinematicsService",
"move_group/MoveGroupCartesianPathService",
"move_group/MoveGroupExecuteTrajectoryAction",
"move_group/MoveGroupKinematicsService",
"move_group/MoveGroupMoveAction",
"move_group/MoveGroupPickPlaceAction",
"move_group/MoveGroupPlanService",
"move_group/MoveGroupQueryPlannersService",
"move_group/MoveGroupStateValidationService",
"move_group/MoveGroupGetPlanningSceneService",
"move_group/ApplyPlanningSceneService",
"move_group/ClearOctomapService",
};
// clang-format on
static const std::string PLANNER_SERVICE_NAME =
"plan_kinematic_path"; // name of the advertised service (within the ~ namespace)
static const std::string EXECUTE_SERVICE_NAME =
@@ -36,12 +36,14 @@
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <tf/transform_listener.h>
#include <moveit/move_group/capability_names.h>
#include <moveit/move_group/move_group_capability.h>
#include <boost/algorithm/string/join.hpp>
#include <boost/tokenizer.hpp>
#include <moveit/macros/console_colors.h>
#include <moveit/move_group/node_name.h>
#include <memory>
#include <set>
static const std::string ROBOT_DESCRIPTION =
"robot_description"; // name of the robot description (a param name, so it can be changed externally)
@@ -102,32 +104,50 @@ class MoveGroupExe
return;
}
// add individual capabilities move_group supports
std::set<std::string> capabilities;
// add default capabilities
for (size_t i = 0; i < sizeof(DEFAULT_CAPABILITIES) / sizeof(DEFAULT_CAPABILITIES[0]); ++i)
capabilities.insert(DEFAULT_CAPABILITIES[i]);
// add capabilities listed in ROS parameter
std::string capability_plugins;
if (node_handle_.getParam("capabilities", capability_plugins))
{
boost::char_separator<char> sep(" ");
boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
for (boost::tokenizer<boost::char_separator<char> >::iterator beg = tok.begin(); beg != tok.end(); ++beg)
capabilities.insert(tok.begin(), tok.end());
}
// drop capabilities that have been explicitly disabled
if (node_handle_.getParam("disable_capabilities", capability_plugins))
{
boost::char_separator<char> sep(" ");
boost::tokenizer<boost::char_separator<char> > tok(capability_plugins, sep);
for (boost::tokenizer<boost::char_separator<char> >::iterator cap_name = tok.begin(); cap_name != tok.end();
++cap_name)
capabilities.erase(*cap_name);
}
for (std::set<std::string>::iterator plugin = capabilities.cbegin(); plugin != capabilities.cend(); ++plugin)
{
try
{
std::string plugin = *beg;
try
{
printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'...\n" MOVEIT_CONSOLE_COLOR_RESET, plugin.c_str());
MoveGroupCapability* cap = capability_plugin_loader_->createUnmanagedInstance(plugin);
cap->setContext(context_);
cap->initialize();
capabilities_.push_back(MoveGroupCapabilityPtr(cap));
}
catch (pluginlib::PluginlibException& ex)
{
ROS_ERROR_STREAM("Exception while loading move_group capability '"
<< plugin << "': " << ex.what() << std::endl
<< "Available capabilities: "
<< boost::algorithm::join(capability_plugin_loader_->getDeclaredClasses(), ", "));
}
printf(MOVEIT_CONSOLE_COLOR_CYAN "Loading '%s'...\n" MOVEIT_CONSOLE_COLOR_RESET, plugin->c_str());
MoveGroupCapability* cap = capability_plugin_loader_->createUnmanagedInstance(*plugin);
cap->setContext(context_);
cap->initialize();
capabilities_.push_back(MoveGroupCapabilityPtr(cap));
}
catch (pluginlib::PluginlibException& ex)
{
ROS_ERROR_STREAM("Exception while loading move_group capability '"
<< *plugin << "': " << ex.what() << std::endl
<< "Available capabilities: "
<< boost::algorithm::join(capability_plugin_loader_->getDeclaredClasses(), ", "));
}
}
std::stringstream ss;
ss << std::endl;
ss << std::endl;
@@ -46,19 +46,21 @@
<param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
<!-- MoveGroup capabilities to load -->
<param name="capabilities" value="move_group/MoveGroupCartesianPathService
move_group/MoveGroupExecuteTrajectoryAction
move_group/MoveGroupKinematicsService
move_group/MoveGroupMoveAction
move_group/MoveGroupPickPlaceAction
move_group/MoveGroupPlanService
move_group/MoveGroupQueryPlannersService
move_group/MoveGroupStateValidationService
move_group/MoveGroupGetPlanningSceneService
move_group/ApplyPlanningSceneService
move_group/ClearOctomapService
" />
<!-- load these non-default MoveGroup capabilities -->
<!--
<param name="capabilities" value="
a_package/AwsomeMotionPlanningCapability
another_package/GraspPlanningPipeline
" />
-->
<!-- inhibit these default MoveGroup capabilities -->
<!--
<param name="disable_capabilities" value="
move_group/MoveGroupKinematicsService
move_group/ClearOctomapService
" />
-->
<!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
<param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />