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;
}
int main(int argc, char **argv)
{
std::string image_filename;
std::string config_filename;
if (argc < 3)
{
std::cout << "Please provide --file or --config" << std::endl;
return 1;
}
for(unsigned int i = 1 ; i + 1 < argc; i += 2)
{
std::string arg = argv[i];
std::cout << arg << std::endl;
if (arg[0] == '_')
break;
if (arg == "--file")
image_filename = argv[i + 1];
else if (arg == "--config")
config_filename = argv[i + 1];
else
{
std::cerr << "Unknown option: " << arg << std::endl;
return 1;
}
}
tue::Configuration config;
if (!config_filename.empty())
{
tue::config::loadFromYAMLFile(config_filename, config);
fr.configure(config);
}
if (!image_filename.empty())
{
rgbd::ImagePtr image = loadImage(image_filename);
if (!image)
{
std::cerr << "Image could not be loaded" << std::endl;
return 1;
}
fr.configure(config);
if (config.hasError())
{
std::cerr << "Error in configuration: " << config.error().c_str() << std::endl;
return 1;
}
std::vector<FaceRecognitionResult> detections;
fr.find(image->getRGBImage(), detections);
visualizeResult(*image, detections);
}
else
{
// No image filename given, so run as rosnode
ros::init(argc, argv, "face_recognition");
ros::NodeHandle nh;
if (argc == 1)
{
ROS_ERROR("[FACE RECOGNITION] Please provide config file");
return 1;
}
if (!config.hasError())
{
std::string topic;
if (config.value("camera_topic", topic))
rgbd_client.intialize(topic);
fr.configure(config);
}
if (config.hasError())
{
ROS_ERROR("[FACE RECOGNITION] %s", config.error().c_str());
return 1;
}
ros::ServiceServer srv_learn_person = nh.advertiseService("learn_person", srvLearnPerson);
ros::ServiceServer srv_recognize_person = nh.advertiseService("recognize_person", srvRecognizePerson);
ros::ServiceServer srv_clear_persons = nh.advertiseService("clear_persons", clearPersons);
ros::spin();
}
if (argc == 1 || std::string(argv[1]) != "--file")
{
}
else
{
// Filename as argument, so run as test
if (argc < 3)
{
std::cout << "Please provide rgbd filename" << std::endl;
return 1;
}
std::string image_filename = argv[2];
}
return 0;
}