1 #include <xmlrpcpp/XmlRpc.h>
12 #include <moveit_grasps/two_finger_grasp_generator.h>
13 #include <moveit_grasps/two_finger_grasp_data.h>
14 #include <moveit_grasps/two_finger_grasp_filter.h>
15 #include <moveit_grasps/grasp_planner.h>
19 int main(
int argc,
char **argv){
20 ros::init(argc, argv,
"cell_routine");
21 std::shared_ptr<ros::NodeHandle> n(
new ros::NodeHandle);
23 ros::AsyncSpinner spinner(1);
26 std::shared_ptr<MoveitMediator> mediator = std::make_shared<MoveitMediator>(n);
28 auto rd = mediator->robotReader()->robotData();
29 for (
int i = 0; i < rd.size() ;i++){
30 std::unique_ptr<AbstractRobot> ceti = std::make_unique<CetiRobot>(rd[i].name_, rd[i].pose_, rd[i].size_);
31 std::unique_ptr<PandaDecorator> ceti_panda = std::make_unique<PandaDecorator>(std::move(ceti));
32 mediator->connectRobots(std::move(ceti_panda));