base_routine.cpp
Go to the documentation of this file.
3 #include "impl/abstract_robot.h"
7 #include "impl/mediator.h"
10 #include "impl/wing.h"
11 #include "impl/field.h"
12 #include "impl/robot.h"
13 #include "impl/map_loader.h"
14 #include "impl/base_by_rotation.h"
15 #include <xmlrpcpp/XmlRpc.h>
16 #include <filesystem>
18 #include "reader/robot_reader.h"
19 #include "reader/wing_reader.h"
20 
21 #include <moveit_grasps/two_finger_grasp_generator.h>
22 #include <moveit_grasps/two_finger_grasp_data.h>
23 #include <moveit_grasps/two_finger_grasp_filter.h>
24 #include <moveit_grasps/grasp_planner.h>
25 
26 
27 
28 
29 
30 int main(int argc, char **argv){
31  ROS_INFO("penis");
32  ros::init(argc, argv, "base_routine");
33  std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
34 
35  XmlRpc::XmlRpcValue map, task, resources;
36 
37  n->getParam("/data",map);
38  n->getParam("/objects",resources);
39  n->getParam("/task/groups",task);
40 
41  std::string filename;
42  n->getParam("/resource_name", filename);
43 
44  if(std::filesystem::exists(ros::package::getPath("multi_cell_builder") + "/results/" + filename)) std::filesystem::remove_all(ros::package::getPath("multi_cell_builder") + "/results/" + filename);
45  std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" + filename);
46 
47  Abstract_map_loader* map_loader = new Map_loader(map, task);
48  Abstract_strategy* strategy = new Base_by_rotation();
49 
50  map_loader->set_strategy(strategy);
51  std::vector<std::vector<pcl::PointXYZ>> results = map_loader->base_calculation();
52 
53  Abstract_mediator* mediator = new Mediator(map_loader->task_grasps(), new ros::Publisher(n->advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)));
54  mediator->set_result_vector(results);
55  mediator->set_dirname(filename);
56 
57 
58  std::unique_ptr<Robot_reader> robot_reader(new Robot_reader(n));
59  auto rd = robot_reader->robot_data();
60  for (int i = 0; i < rd.size() ;i++) mediator->connect_robots(new Robot(rd[i].name_, rd[i].pose_, rd[i].size_));
61 
62  /*
63  std::unique_ptr<Wing_reader> wing_reader(new Wing_reader(n));
64  auto wd = wing_reader->wing_data();
65 
66  for (int i = 0; i < mediator->robots().size(); i++){
67  for(object_data& w : wd[i].first){
68  w.pose_ = mediator->robots()[i]->tf().inverse() * w.pose_;
69  }
70  mediator->robots()[i]->set_observer_mask(static_cast<wing_config>(wd[i].second));
71  }
72  mediator->set_wings(wd);
73  */
74 
75  mediator->mediate();
76 
77 
78 
79 
80  //map_loader->write_task(robo);
81 
82  //free(rviz_right);
83  //free(rviz_mid);
84  //free(rviz_left);
85 
86 
87  free(map_loader);
88  free(strategy);
89 
90  //free(robo);
91 
92  //free(robo2);
93 
94  return 0;
95 }
robot.h
abstract_robot_element.h
Abstract_mediator
Abstract mediator.
Definition: impl/abstract_mediator.h:33
abstract_map_loader.h
Abstract_mediator::set_dirname
void set_dirname(std::string &dirn)
Definition: impl/abstract_mediator.h:57
abstract_param_reader.h
Mediator
Definition: mediator.h:12
field.h
mediator.h
main
int main(int argc, char **argv)
Definition: base_routine.cpp:30
Abstract_map_loader::base_calculation
virtual std::vector< std::vector< pcl::PointXYZ > > base_calculation()=0
Abstract_map_loader::task_grasps
std::vector< std::vector< tf2::Transform > > & task_grasps()
Definition: abstract_map_loader.h:46
Abstract_map_loader::set_strategy
void set_strategy(Abstract_strategy *some_stratergy)
Definition: abstract_map_loader.h:38
abstract_mediator.h
map_loader.h
abstract_robot.h
Abstract_strategy
Definition: abstract_strategy.h:8
Abstract_map_loader
Definition: abstract_map_loader.h:24
Robot
Definition: robot.h:19
Abstract_mediator::mediate
virtual void mediate()=0
Abstract_mediator::connect_robots
virtual void connect_robots(Abstract_robot *robot)=0
wing_reader.h
robot_reader.h
Map_loader
Definition: map_loader.h:10
wing_rviz_decorator.h
Abstract_mediator::set_result_vector
void set_result_vector(std::vector< std::vector< pcl::PointXYZ >> &res)
Definition: impl/abstract_mediator.h:51
base_by_rotation.h
field_rviz_decorator.h
wing.h
Base_by_rotation
Definition: base_by_rotation.h:9
abstract_strategy.h
Robot_reader
Robot reader.
Definition: robot_reader.h:15
abstract_robot_element_decorator.h


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