12 #include <xmlrpcpp/XmlRpc.h>
18 #include <moveit_grasps/two_finger_grasp_generator.h>
19 #include <moveit_grasps/two_finger_grasp_data.h>
20 #include <moveit_grasps/two_finger_grasp_filter.h>
21 #include <moveit_grasps/grasp_planner.h>
25 int main(
int argc,
char **argv){
26 ros::init(argc, argv,
"cell_routine");
27 std::shared_ptr<ros::NodeHandle> nh = std::make_shared<ros::NodeHandle>();
28 ros::AsyncSpinner spinner(1);
31 XmlRpc::XmlRpcValue map, task;
33 nh->getParam(
"/data",map);
34 nh->getParam(
"/task/groups",task);
40 ros::Publisher* pub =
new ros::Publisher(nh->advertise< visualization_msgs::MarkerArray >(
"visualization_marker_array", 1));
45 std::unique_ptr<Robot_reader> robot_reader = std::make_unique<Robot_reader>(nh);
46 auto rd = robot_reader->robot_data();
49 std::unique_ptr<Wing_reader> wing_reader(
new Wing_reader(nh));
50 auto wd =
static_cast<Wing_reader*
>(wing_reader.get())->wing_data();
52 std::unique_ptr<Cuboid_reader> cuboid_reader = std::make_unique<Cuboid_reader>(nh);
53 auto cbd = cuboid_reader->cuboid_bin();
54 auto cod = cuboid_reader->cuboid_obstacle();