cell_routine.cpp
Go to the documentation of this file.
1 #include "impl/abstract_robot.h"
3 #include "impl/map_loader.h"
7 #include "impl/moveit_mediator.h"
9 #include "impl/wing.h"
10 #include "impl/moveit_robot.h"
11 #include "impl/collision_helper.h"
12 #include <xmlrpcpp/XmlRpc.h>
14 #include "reader/robot_reader.h"
15 #include "reader/wing_reader.h"
16 #include "reader/cuboid_reader.h"
17 
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>
22 
23 
24 
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);
29  spinner.start();
30 
31  XmlRpc::XmlRpcValue map, task;
32 
33  nh->getParam("/data",map);
34  nh->getParam("/task/groups",task);
35 
36 
37  Abstract_map_loader* map_loader = new Map_loader(map, task);
38 
39 
40  ros::Publisher* pub = new ros::Publisher(nh->advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)); //refractor
41 
42  Abstract_mediator* mediator = new Moveit_mediator(map_loader->task_grasps(), pub, nh);
43  Moveit_mediator* moveit_mediator = dynamic_cast<Moveit_mediator*>(mediator);
44 
45  std::unique_ptr<Robot_reader> robot_reader = std::make_unique<Robot_reader>(nh);
46  auto rd = robot_reader->robot_data();
47  for (int i = 0; i < rd.size() ;i++) mediator->connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_));
48 
49  std::unique_ptr<Wing_reader> wing_reader(new Wing_reader(nh));
50  auto wd = static_cast<Wing_reader*>(wing_reader.get())->wing_data();
51 
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();
55 
56  /*
57  for (int i = 0; i < mediator->robots().size(); i++){
58  for(object_data& w : wd[i].first){
59  w.pose_ = mediator->robots()[i]->tf().inverse() * w.pose_;
60  }
61  mediator->robots()[i]->set_observer_mask(static_cast<wing_config>(wd[i].second));
62  }
63  moveit_mediator->set_wings(wd);
64  */
65 
66 
67  Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(mediator->robots()[0]);
68  Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(mediator->robots()[1]);
69 
70 
71  /*
72  for (int i = 0; i < mediator->robots().size(); i++){
73  std::bitset<3> bs = wd[i].second;
74  moveit_mediator->build_wings(bs, i);
75  }
76  */
77 
78 
79  //moveit::planning_interface::PlanningSceneInterface::applyCollisionObject()
80  ceti1->notify();
81  ceti2->notify();
82 
83  moveit_mediator->mediate();
84 
85 
86  while (ros::ok()){
87  ros::spinOnce();
88  }
89 }
moveit_robot.h
Abstract_mediator::robots
std::vector< Abstract_robot * > robots()
Definition: impl/abstract_mediator.h:54
abstract_robot_element.h
cuboid_reader.h
main
int main(int argc, char **argv)
Definition: cell_routine.cpp:25
Abstract_mediator
Abstract mediator.
Definition: impl/abstract_mediator.h:33
Wing_reader
Wing reader.
Definition: wing_reader.h:14
abstract_map_loader.h
Robot::notify
void notify() override
Definition: robot.cpp:125
abstract_param_reader.h
Abstract_map_loader::task_grasps
std::vector< std::vector< tf2::Transform > > & task_grasps()
Definition: abstract_map_loader.h:46
wing_moveit_decorator.h
abstract_mediator.h
map_loader.h
abstract_robot.h
Abstract_map_loader
Definition: abstract_map_loader.h:24
Abstract_mediator::connect_robots
virtual void connect_robots(Abstract_robot *robot)=0
collision_helper.h
wing_reader.h
robot_reader.h
Map_loader
Definition: map_loader.h:10
Moveit_robot
Definition: moveit_robot.h:12
Moveit_mediator::mediate
void mediate() override
Definition: moveit_mediator.cpp:67
Moveit_mediator
Definition: moveit_mediator.h:54
moveit_mediator.h
wing.h
abstract_robot_element_decorator.h


multi_cell_builder
Author(s): MA
autogenerated on Thu Jan 12 2023 23:45:43