grasp_mediator.cpp
Go to the documentation of this file.
2 
3 GraspMediator::GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh)
4  : AbstractMediator(nh)
5  , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
6  , grasp_pipeline_demo_(std::make_unique<moveit_grasps_demo::GraspPipelineDemo>())
7  {
8  nh->getParam("/referenceRobot", referenceRobot_);
9  nh->getParam("/resultPath", resultPath_);
10 
11  }
12 
13 void GraspMediator::connectRobots(std::unique_ptr<AbstractRobotDecorator> robot) {
14  ROS_INFO("connecting %s...", robot->name().c_str());
15  robots_.insert_or_assign(robot->name(), std::move(robot));
16 }
17 
19  auto wd = wing_reader_->wingData();
20 
21  for (const auto& s_r : robots_){
22  try{
23  auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
24  ceti_bot->setObserverMask(static_cast<wing_config>(wd.at(s_r.first).second));
25  } catch(const std::out_of_range& oor) {
26  ROS_INFO("No mask data for %s", s_r.first.c_str());
27  }
28  }
29 
30  for (const auto& s_r : robots_){
31  auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
32  std::vector<std::unique_ptr<AbstractRobotElementDecorator>> panels(3);
33 
34  for (std::size_t j = 0; j < ceti_bot->observerMask().size(); j++){
35  if (ceti_bot->observerMask()[j]){
36  try{
37  tf2::Transform relative_tf = ceti_bot->tf().inverse() * wd.at(ceti_bot->name()).first[j].pose_;
38 
39  std::unique_ptr<AbstractRobotElement> panel = std::make_unique<MoveitPanel>(wd.at(s_r.first).first[j].name_, relative_tf, wd.at(s_r.first).first[j].size_);
40  std::unique_ptr<AbstractRobotElementDecorator> log = std::make_unique<LogDecorator>(std::move(panel));
41  panels[j] = std::move(log);
42  } catch (std::out_of_range &oor){
43  ROS_INFO("OOR in set_panel");
44  }
45  }
46  }
47  ceti_bot->setObservers(panels);
48  }
49 
50  for (const auto& s_r : robots_){
51  s_r.second->notify();
52  }
53 }
54 
56  setPanel();
57  //if(autoMapping_)mapReconfigure();
58  ros::Duration(2).sleep();
59  std::vector<Cuboid> relevant_box;
60  std::vector<Cuboid> relevant_obstacle;
61 
62  Cuboid rob(referenceRobot_);
63  rob.Pose.position.x = robots_.at(referenceRobot_)->tf().getOrigin().getX();
64  rob.Pose.position.y = robots_.at(referenceRobot_)->tf().getOrigin().getY();
65  rob.Pose.position.z = robots_.at(referenceRobot_)->tf().getOrigin().getZ();
66  rob.Pose.orientation.x = robots_.at(referenceRobot_)->tf().getRotation().getX();
67  rob.Pose.orientation.y = robots_.at(referenceRobot_)->tf().getRotation().getY();
68  rob.Pose.orientation.z = robots_.at(referenceRobot_)->tf().getRotation().getZ();
69  rob.Pose.orientation.w = robots_.at(referenceRobot_)->tf().getRotation().getW();
70  rob.x_depth = robots_.at(referenceRobot_)->size().getX();
71  rob.y_width = robots_.at(referenceRobot_)->size().getY();
72  rob.z_heigth = robots_.at(referenceRobot_)->tf().getOrigin().getZ()*2;
73  relevant_obstacle.push_back(rob);
74 
75  auto ceti_bot = dynamic_cast<CetiRobot*>(robots_.at(referenceRobot_)->next());
76 
77  for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){
78  if(ceti_bot->observerMask()[k]){
79  Cuboid wing(ceti_bot->observers()[k]->name());
80  wing.Pose.position.x = ceti_bot->observers()[k]->worldTf().getOrigin().getX();
81  wing.Pose.position.y = ceti_bot->observers()[k]->worldTf().getOrigin().getY();
82  wing.Pose.position.z = ceti_bot->observers()[k]->worldTf().getOrigin().getZ() - ceti_bot->observers()[k]->size().getZ()/2;
83  wing.Pose.orientation.x = ceti_bot->observers()[k]->worldTf().getRotation().getX();
84  wing.Pose.orientation.y = ceti_bot->observers()[k]->worldTf().getRotation().getY();
85  wing.Pose.orientation.z = ceti_bot->observers()[k]->worldTf().getRotation().getZ();
86  wing.Pose.orientation.w = ceti_bot->observers()[k]->worldTf().getRotation().getW();
87  wing.x_depth = ceti_bot->observers()[k]->size().getX();
88  wing.y_width = ceti_bot->observers()[k]->size().getY();
89  wing.z_heigth = ceti_bot->observers()[k]->size().getZ();
90  relevant_obstacle.push_back(wing);
91  }
92  }
93 
94  for (auto& ro :relevant_obstacle){
95  grasp_pipeline_demo_->sceneObstacles().push_back(ro);
96  }
97 
98  for (const auto& b : cuboid_reader_->cuboidBox()){
99  std::string temp;
100  std::bitset<3> temp2 = ceti_bot->observerMask();
101  tf2::Transform pos(tf2::Quaternion(b.Pose.orientation.x, b.Pose.orientation.y, b.Pose.orientation.z, b.Pose.orientation.w), tf2::Vector3(b.Pose.position.x, b.Pose.position.y, b.Pose.position.z));
102  if (robots_.at(referenceRobot_)->checkSingleObjectCollision(pos, temp, temp2)){
103  grasp_pipeline_demo_->sceneBoxes().push_back(b);
104  relevant_boxes_.push_back(b);
105  }
106  }
107 
108  for (const auto& b : cuboid_reader_->cuboidObstacle()){
109  std::string temp;
110  std::bitset<3> temp2 = ceti_bot->observerMask();
111  tf2::Transform pos(tf2::Quaternion(b.Pose.orientation.x, b.Pose.orientation.y, b.Pose.orientation.z, b.Pose.orientation.w), tf2::Vector3(b.Pose.position.x, b.Pose.position.y, b.Pose.position.z));
112  if (robots_.at(referenceRobot_)->checkSingleObjectCollision(pos, temp, temp2)){
113  grasp_pipeline_demo_->sceneObstacles().push_back(b);
114  }
115  }
116 
117  srand(ros::Time::now().toSec());
118  grasp_pipeline_demo_->publishConfigScene();
119  grasp_pipeline_demo_->generateGraspMap();
120  grasp_pipeline_demo_->getVoxelizedEnv();
121  grasp_pipeline_demo_->computeConfig();
122  rewriteResult();
123 }
124 
126  std::ifstream input_file(ros::package::getPath("multi_cell_builder") + "/results/" + resultPath_);
127  if (!input_file) {
128  ROS_INFO("file not found");
129  ros::shutdown();
130  }
131 
132  std::ofstream output_file("input.txt.tmp");
133  if (!output_file) {
134  ROS_INFO("temp file not open");
135  ros::shutdown();
136  }
137 
138  for (auto& c: grasp_pipeline_demo_->sortConfig().objects){
139  ROS_INFO("%s", c.first.c_str());
140  }
141 
142 
143  // std::string line;
144  // while (std::getline(input_file, line)) {
145  // for (auto& c: grasp_pipeline_demo_->sortConfig().objects){
146  // std::string str = ".*" + c.first + ".*";
147  // std::smatch match;
148  // if (std::regex_match(line, match, std::regex(str))){
149  // std::stringstream ss;
150  // ss << "{ 'id': '"<< c.second->Pose<<"', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': -0.7, 'z': 0.9355 },'size': { 'length': 0.0318, 'width': 0.0636, 'height': 0.091 },'orientation': { 'x':0, 'y':0, 'z':0, 'w':1},'color': { 'b': 1 } }, ";
151  // }
152  // }
153  // std::transform(line.begin(), line.end(), line.begin(), ::toupper);
154 
155  // output_file << line << '\n';
156  // }
157 
158 }
GraspMediator::setPanel
void setPanel() override
pure virtual Sets panels for robots
Definition: grasp_mediator.cpp:18
GraspMediator::rewriteResult
void rewriteResult()
Definition: grasp_mediator.cpp:125
CetiRobot::setObserverMask
void setObserverMask(int i)
Definition: ceti_robot.h:83
GraspMediator::relevant_boxes_
std::vector< Cuboid > relevant_boxes_
Definition: grasp_mediator.h:22
wing_config
wing_config
Definition: abstract_robot.h:10
AbstractMediator::wing_reader_
std::unique_ptr< WingReader > wing_reader_
Wing_reader which collects panel information of robots.
Definition: abstract_mediator.h:54
AbstractMediator
AbstractMediator.
Definition: abstract_mediator.h:43
CetiRobot
Concrete Ceti-Robot.
Definition: ceti_robot.h:14
GraspMediator::grasp_pipeline_demo_
std::unique_ptr< moveit_grasps_demo::GraspPipelineDemo > grasp_pipeline_demo_
Auerswald grasp pipeline.
Definition: grasp_mediator.h:19
GraspMediator::connectRobots
void connectRobots(std::unique_ptr< AbstractRobotDecorator > robot) override
pure virtual robot connecting methode
Definition: grasp_mediator.cpp:13
grasp_mediator.h
AbstractMediator::robots_
std::map< std::string, std::unique_ptr< AbstractRobotDecorator > > robots_
Robots agents.
Definition: abstract_mediator.h:47
GraspMediator::referenceRobot_
std::string referenceRobot_
Reference Robot.
Definition: grasp_mediator.h:20
AbstractMediator::cuboid_reader_
std::unique_ptr< CuboidReader > cuboid_reader_
coboidReader instance that distinguishes between scene objects of type bin and box
Definition: abstract_mediator.h:56
GraspMediator::mediate
void mediate() override
pure virtual mediate methode
Definition: grasp_mediator.cpp:55
GraspMediator::resultPath_
std::string resultPath_
path to the result
Definition: grasp_mediator.h:21
GraspMediator::GraspMediator
GraspMediator(std::shared_ptr< ros::NodeHandle > const &nh)
Definition: grasp_mediator.cpp:3


multi_cell_builder
Author(s): Matteo Anedda
autogenerated on Sun Apr 9 2023 23:59:51