39 #include <tf2_ros/transform_listener.h>
42 #include <boost/tokenizer.hpp>
49 static const std::string ROBOT_DESCRIPTION =
64 static const char*
const DEFAULT_CAPABILITIES[] = {
65 "move_group/GetUrdfService",
66 "move_group/MoveGroupCartesianPathService",
67 "move_group/MoveGroupKinematicsService",
68 "move_group/MoveGroupExecuteTrajectoryAction",
69 "move_group/MoveGroupMoveAction",
70 "move_group/MoveGroupPlanService",
71 "move_group/MoveGroupQueryPlannersService",
72 "move_group/MoveGroupStateValidationService",
73 "move_group/MoveGroupGetPlanningSceneService",
74 "move_group/ApplyPlanningSceneService",
75 "move_group/ClearOctomapService",
85 bool allow_trajectory_execution;
86 moveit_cpp->getNode()->get_parameter_or(
"allow_trajectory_execution", allow_trajectory_execution,
true);
89 std::make_shared<MoveGroupContext>(
moveit_cpp, default_planning_pipeline, allow_trajectory_execution, debug);
92 configureCapabilities();
97 capabilities_.clear();
99 capability_plugin_loader_.reset();
106 if (context_->status())
108 if (capabilities_.empty())
121 RCLCPP_ERROR(
getLogger(),
"No MoveGroup context created. Nothing will work.");
130 void configureCapabilities()
134 capability_plugin_loader_ = std::make_shared<pluginlib::ClassLoader<MoveGroupCapability>>(
135 "moveit_ros_move_group",
"move_group::MoveGroupCapability");
137 catch (pluginlib::PluginlibException& ex)
140 "Exception while creating plugin loader for move_group capabilities: " << ex.what());
144 std::set<std::string> capabilities;
147 for (
const char* capability : DEFAULT_CAPABILITIES)
148 capabilities.insert(capability);
151 std::string capability_plugins;
152 if (context_->moveit_cpp_->getNode()->get_parameter(
"capabilities", capability_plugins))
154 boost::char_separator<char> sep(
" ");
155 boost::tokenizer<boost::char_separator<char>> tok(capability_plugins, sep);
156 capabilities.insert(tok.begin(), tok.end());
160 for (
const auto& pipeline_entry : context_->moveit_cpp_->getPlanningPipelines())
162 const auto& pipeline_name = pipeline_entry.first;
163 std::string pipeline_capabilities;
164 if (context_->moveit_cpp_->getNode()->get_parameter(pipeline_name +
".capabilities", pipeline_capabilities))
166 boost::char_separator<char> sep(
" ");
167 boost::tokenizer<boost::char_separator<char>> tok(pipeline_capabilities, sep);
168 capabilities.insert(tok.begin(), tok.end());
173 if (context_->moveit_cpp_->getNode()->get_parameter(
"disable_capabilities", capability_plugins))
175 boost::char_separator<char> sep(
" ");
176 boost::tokenizer<boost::char_separator<char>> tok(capability_plugins, sep);
177 for (boost::tokenizer<boost::char_separator<char>>::iterator cap_name = tok.begin(); cap_name != tok.end();
179 capabilities.erase(*cap_name);
182 for (
const std::string& capability : capabilities)
187 MoveGroupCapabilityPtr cap = capability_plugin_loader_->createUniqueInstance(capability);
188 cap->setContext(context_);
190 capabilities_.push_back(cap);
192 catch (pluginlib::PluginlibException& ex)
195 "Exception while loading move_group capability '" << capability <<
"': " << ex.what());
199 std::stringstream ss;
202 ss <<
"********************************************************" <<
'\n';
203 ss <<
"* MoveGroup using: " <<
'\n';
204 for (
const MoveGroupCapabilityPtr& cap : capabilities_)
205 ss <<
"* - " << cap->getName() <<
'\n';
206 ss <<
"********************************************************" <<
'\n';
207 RCLCPP_INFO(
getLogger(),
"%s", ss.str().c_str());
210 MoveGroupContextPtr context_;
211 std::shared_ptr<pluginlib::ClassLoader<MoveGroupCapability>> capability_plugin_loader_;
212 std::vector<MoveGroupCapabilityPtr> capabilities_;
216 int main(
int argc,
char** argv)
218 rclcpp::init(argc, argv);
220 rclcpp::NodeOptions opt;
221 opt.allow_undeclared_parameters(
true);
222 opt.automatically_declare_parameters_from_overrides(
true);
223 rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared(
"move_group", opt);
229 std::vector<std::string> planning_pipeline_configs;
230 if (nh->get_parameter(
"planning_pipelines", planning_pipeline_configs))
232 if (planning_pipeline_configs.empty())
234 RCLCPP_ERROR(nh->get_logger(),
"Failed to read parameter 'move_group.planning_pipelines'");
238 for (
const auto& config : planning_pipeline_configs)
247 std::string default_planning_pipeline;
248 if (nh->get_parameter(
"default_planning_pipeline", default_planning_pipeline))
251 if (std::find(pipeline_names.begin(), pipeline_names.end(), default_planning_pipeline) == pipeline_names.end())
253 RCLCPP_WARN(nh->get_logger(),
254 "MoveGroup launched with ~default_planning_pipeline '%s' not configured in ~planning_pipelines",
255 default_planning_pipeline.c_str());
256 default_planning_pipeline =
"";
259 else if (pipeline_names.size() > 1)
262 RCLCPP_WARN(nh->get_logger(),
263 "MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default "
264 "planning pipeline configuration");
268 if (default_planning_pipeline.empty())
270 if (!pipeline_names.empty())
272 RCLCPP_WARN(nh->get_logger(),
"Using default pipeline '%s'", pipeline_names[0].c_str());
273 default_planning_pipeline = pipeline_names[0];
277 RCLCPP_WARN(nh->get_logger(),
"Falling back to using the the move_group node namespace (deprecated behavior).");
278 default_planning_pipeline =
"move_group";
284 nh->set_parameter(rclcpp::Parameter(
"default_planning_pipeline", default_planning_pipeline));
288 const auto moveit_cpp = std::make_shared<moveit_cpp::MoveItCpp>(nh, moveit_cpp_options);
294 for (
int i = 1; i < argc; ++i)
296 if (strncmp(argv[i],
"--debug", 7) == 0)
305 RCLCPP_INFO(nh->get_logger(),
"MoveGroup debug mode is ON");
309 RCLCPP_INFO(nh->get_logger(),
"MoveGroup debug mode is OFF");
312 rclcpp::executors::MultiThreadedExecutor executor;
316 bool monitor_dynamics;
317 if (nh->get_parameter(
"monitor_dynamics", monitor_dynamics) && monitor_dynamics)
319 RCLCPP_INFO(nh->get_logger(),
"MoveGroup monitors robot dynamics (higher load)");
326 executor.add_node(nh);
332 RCLCPP_ERROR(nh->get_logger(),
"Planning scene not configured");
MoveGroupContextPtr getContext()
MoveGroupExe(const moveit_cpp::MoveItCppPtr &moveit_cpp, const std::string &default_planning_pipeline, bool debug)
#define MOVEIT_CONSOLE_COLOR_BLUE
#define MOVEIT_CONSOLE_COLOR_GREEN
#define MOVEIT_CONSOLE_COLOR_RESET
#define MOVEIT_CONSOLE_COLOR_CYAN
int main(int argc, char **argv)
rclcpp::Logger getLogger()
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
void setNodeLoggerName(const std::string &name)
Call once after creating a node to initialize logging namespaces.
Parameter container for initializing MoveItCpp.
PlanningPipelineOptions planning_pipeline_options
std::vector< std::string > pipeline_names
std::string parent_namespace