#include <pluginlib/class_loader.h>
#include <ros/ros.h>
// MoveIt!
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <boost/scoped_ptr.hpp>
int main(int argc, char** argv)
{
const std::string node_name = "motion_planning_tutorial";
ros::init(argc, argv, node_name);
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle("~");
// BEGIN_TUTORIAL
// Start
// ^^^^^
// Setting up to start using a planner is pretty easy. Planners are
// setup as plugins in MoveIt! and you can use the ROS pluginlib
// interface to load any planner that you want to use. Before we
// can load the planner, we need two objects, a RobotModel and a
// PlanningScene. We will start by instantiating a `RobotModelLoader`_
// object, which will look up the robot description on the ROS
// parameter server and construct a :moveit_core:`RobotModel` for us
// to use.
//
// .. _RobotModelLoader:
// http://docs.ros.org/indigo/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
const std::string PLANNING_GROUP = "panda_arm";
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
/* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/
robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
// Using the :moveit_core:`RobotModel`, we can construct a :planning_scene:`PlanningScene`
// that maintains the state of the world (including the robot).
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
// Configure a valid robot state
planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");
// We will now construct a loader to load a planner, by name.
// Note that we are using the ROS pluginlib library here.
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name;
// We will get the name of planning plugin we want to load
// from the ROS parameter ser