int QTshotWidget::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QMAINWINDOW::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: doHelp(); break;
case 1: doQuit(); break;
case 2: doOpen(); break;
case 3: doNew(); break;
case 4: doSave(); break;
case 5: doData(); break;
case 6: doExport(); break;
case 7: doExportOK(); break;
case 8: doOptions(); break;
case 9: doToggle(); break;
case 10: doCollapse(); break;
case 11: doPlan(); break;
case 12: doExtended(); break;
case 13: do3D(); break;
case 14: doCrossSection((*reinterpret_cast< DBlock*(*)>(_a[1])),(*reinterpret_cast< bool(*)>(_a[2])),(*reinterpret_cast< bool(*)>(_a[3]))); break;
case 15: doCrossSection((*reinterpret_cast< DBlock*(*)>(_a[1])),(*reinterpret_cast< bool(*)>(_a[2]))); break;
case 16: value_changed((*reinterpret_cast< int(*)>(_a[1])),(*reinterpret_cast< int(*)>(_a[2]))); break;
case 17: double_clicked((*reinterpret_cast< int(*)>(_a[1])),(*reinterpret_cast< int(*)>(_a[2])),(*reinterpret_cast< int(*)>(_a[3])),(*reinterpret_cast< const QPoint(*)>(_a[4]))); break;
}
_id -= 18;
}
return _id;
}
int main(int argc, char **argv)
{
int motion = 0;
std::string initialpath = "";
if (argc >= 2)
{
motion = atoi(argv[1]);
if(argc >=3)
{
initialpath = argv[2];
}
}
printf("%d Motion %d\n", argc, motion);
ros::init(argc, argv, "move_itomp");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle("~");
robot_model_loader::RobotModelLoader robot_model_loader(
"robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
planning_scene::PlanningScenePtr planning_scene(
new planning_scene::PlanningScene(robot_model));
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name;
if (!node_handle.getParam("planning_plugin", planner_plugin_name))
ROS_FATAL_STREAM("Could not find planner plugin name");
try
{
planner_plugin_loader.reset(
new pluginlib::ClassLoader<planning_interface::PlannerManager>(
"moveit_core", "planning_interface::PlannerManager"));
} catch (pluginlib::PluginlibException& ex)
{
ROS_FATAL_STREAM(
"Exception while creating planning plugin loader " << ex.what());
}
try
{
planner_instance.reset(
planner_plugin_loader->createUnmanagedInstance(
planner_plugin_name));
if (!planner_instance->initialize(robot_model,
node_handle.getNamespace()))
ROS_FATAL_STREAM("Could not initialize planner instance");
ROS_INFO_STREAM(
"Using planning interface '" << planner_instance->getDescription() << "'");
} catch (pluginlib::PluginlibException& ex)
{
const std::vector<std::string> &classes =
planner_plugin_loader->getDeclaredClasses();
std::stringstream ss;
for (std::size_t i = 0; i < classes.size(); ++i)
ss << classes[i] << " ";
ROS_ERROR_STREAM(
"Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str());
}
loadStaticScene(node_handle, planning_scene, robot_model);
/* Sleep a little to allow time to startup rviz, etc. */
ros::WallDuration sleep_time(1.0);
sleep_time.sleep();
renderStaticScene(node_handle, planning_scene, robot_model);
// We will now create a motion plan request
// specifying the desired pose of the end-effector as input.
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
std::vector<robot_state::RobotState> robot_states;
int state_index = 0;
robot_states.push_back(planning_scene->getCurrentStateNonConst());
robot_states.push_back(robot_states.back());
Eigen::VectorXd start_trans, goal_trans;
// set trajectory constraints
std::vector<Eigen::VectorXd> waypoints;
std::vector<std::string> hierarchy;
// internal waypoints between start and goal
if(initialpath.empty())
{
hierarchy.push_back("base_prismatic_joint_x");
hierarchy.push_back("base_prismatic_joint_y");
hierarchy.push_back("base_prismatic_joint_z");
hierarchy.push_back("base_revolute_joint_z");
hierarchy.push_back("base_revolute_joint_y");
hierarchy.push_back("base_revolute_joint_x");
Eigen::VectorXd vec1;
start_trans = Eigen::VectorXd(6); start_trans << 0.0, 1.0, 0.0,0,0,0;
goal_trans = Eigen::VectorXd(6); goal_trans << 0.0, 2.5, 0.0,0,0,0;
vec1 = Eigen::VectorXd(6); vec1 << 0.0, 1.5, 1.0,0,0,0;
waypoints.push_back(vec1);
vec1 = Eigen::VectorXd(6); vec1 << 0.0, 2.0, 2.0,0,0,0;
waypoints.push_back(vec1);
vec1 = Eigen::VectorXd(6); vec1 << 0.0, 2.5, 1.0,0,0,0;
waypoints.push_back(vec1);
}
else
{
hierarchy = InitTrajectoryFromFile(waypoints, initialpath);
start_trans = waypoints.front();
goal_trans = waypoints.back();
}
setWalkingStates(robot_states[state_index], robot_states[state_index + 1],
start_trans, goal_trans, hierarchy);
for (int i = 0; i < waypoints.size(); ++i)
{
moveit_msgs::Constraints configuration_constraint =
setRootJointConstraint(hierarchy, waypoints[i]);
req.trajectory_constraints.constraints.push_back(
configuration_constraint);
}
displayStates(robot_states[state_index], robot_states[state_index + 1],
node_handle, robot_model);
doPlan("whole_body", req, res, robot_states[state_index],
robot_states[state_index + 1], planning_scene, planner_instance);
visualizeResult(res, node_handle, 0, 1.0);
ROS_INFO("Done");
planner_instance.reset();
return 0;
}