simple_base.cpp
Go to the documentation of this file.
1 #include "bridge/simple_base.h"
3 
4 Simple_base::Simple_base(std::shared_ptr<ros::NodeHandle> const& d)
5  : Abstract_base(d)
6  , task_space_reader_(std::make_unique<Ts_reader>(d))
7  , map_reader_(std::make_unique<Map_reader>(d))
8  {
9  map_= map_reader_->map_data();
10  ROS_INFO("=> RM is known");
11 
12  for (auto& s_vod:task_space_reader_->drop_off_data()){
13  std::vector<std::pair<object_data, std::vector<tf2::Quaternion>>> vod_q;
14  for(auto& od: s_vod.second) vod_q.push_back(std::pair<object_data, std::vector<tf2::Quaternion>>(od, std::vector<tf2::Quaternion>()));
15  task_space_.insert(std::pair< const std::string, std::vector<std::pair<object_data, std::vector<tf2::Quaternion>>>>(s_vod.first,vod_q));
16  }
17  ROS_INFO("=> TS is known");
18  }
19 
20 std::map<const std::string, std::vector<pcl::PointXYZ>> Simple_base::base_calculation(){
21 
22  implementation_->set_grasp_orientations(task_space_);
23  ROS_INFO("=> grasp rotations from implenentation object set");
24 
25 
26  implementation_->inv_map_creation(map_, inv_map_, task_space_);
27  ROS_INFO("=> inverse map is set...");
28 
29  std::map<const std::string, std::vector<pcl::PointXYZ>> resulting;
30  return resulting;
31 
32 
33  /*
34  ROS_INFO("init voxel...");
35  std::vector<pcl::PointXYZ> voxelization = this->create_pcl_box();
36  std::vector<std::vector<std::vector<int>>> base_target_map;
37  base_target_map.resize(task_grasps_.size());
38  for(long unsigned int i = 0; i < base_target_map.size();i++) base_target_map[i].resize(voxelization.size());
39 
40  ROS_INFO("forming base clouds...");
41  strategy_->cloud_calculation(this);
42 
43  // OpenMP
44  ROS_INFO("start cloud quantization...");
45  for(long unsigned int i = 0; i < target_cloud_.size();i++){
46  for(long unsigned int j = 0; j < target_cloud_[i].size();j++){
47  pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.2f);
48  octree.setInputCloud(target_cloud_[i][j]);
49  octree.addPointsFromInputCloud();
50  double min_x, min_y, min_z, max_x, max_y, max_z;
51  octree.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
52  for(long unsigned int k = 0; k < voxelization.size(); k++) {
53  pcl::PointXYZ p = voxelization[k];
54  bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);
55  if(isInBox && octree.isVoxelOccupiedAtPoint(p)) {
56  std::vector< int > pointIdxVec;
57  if(octree.voxelSearch(p, pointIdxVec)) if(!pointIdxVec.empty()) base_target_map[i][k].push_back(j);
58  }
59 
60  }
61  }
62  }
63 
64 
65 
66 
67  std::vector<std::vector<pcl::PointXYZ>> resulting;
68  for(long unsigned int i = 0; i < base_target_map.size(); i++) {
69  std::vector<pcl::PointXYZ> points_per_robot;
70  for(int j = 0; j < base_target_map[i].size(); j++){
71  if (base_target_map[i][j].size() == task_grasps_[i].size()) {
72  points_per_robot.push_back(voxelization[j]);
73  }
74  }
75  if (!points_per_robot.empty()) resulting.push_back(points_per_robot);
76  }
77 
78  for (long unsigned int i = 0; i < resulting.size(); i++) {
79  ROS_INFO("Robot %li got %li base positions to ckeck", i, resulting[i].size());
80  }
81  return resulting;
82 }
83 
84 
85 std::vector<pcl::PointXYZ> Abstract_map_loader::create_pcl_box(){
86  tf2::Vector3 origin(0,0,0);
87  float resolution = 0.4f;
88  float diameter = 3.0f;
89  unsigned char depth = 16;
90  std::vector<pcl::PointXYZ> box;
91  octomap::OcTree* tree = new octomap::OcTree(resolution/2);
92  for (float x = origin.getX() - diameter * 5 ; x <= origin.getX() + diameter * 5 ; x += resolution){
93  for (float y = origin.getY() - diameter * 5 ; y <= origin.getY() + diameter * 5 ; y += resolution){
94  for (float z = origin.getZ() - diameter * 1.5 ; z <= origin.getZ() + diameter * 1.5 ; z += resolution){
95  octomap::point3d point(x,y,z);
96  tree->updateNode(point, true);
97  }
98  }
99  }
100 
101  for (octomap::OcTree::leaf_iterator it = tree->begin_leafs(depth), end = tree->end_leafs(); it != end; ++it){
102  pcl::PointXYZ searchPoint(it.getCoordinate().x(), it.getCoordinate().y(), it.getCoordinate().z());
103  box.push_back(searchPoint);
104  }
105 
106  return box;
107 }
108 
109 void Map_loader::write_task(Abstract_robot* robot){
110 
111  std::ofstream o(ros::package::getPath("multi_cell_builder") + "/mtc_task_file/dummy.yaml");
112  tf2::Transform target_start = task_grasps_[0].front();
113  tf2::Transform target_end = task_grasps_[0].back();
114  float x, y, z;
115  x = target_end.getOrigin().getX();
116  y = target_end.getOrigin().getY();
117  z = target_end.getOrigin().getZ();
118 
119  std::stringstream ss;
120  ss << "hand_" << robot->name().back();
121  std::string hand = ss.str();
122  std::stringstream sss;
123  sss << "panda_" << robot->name().back() << "_link8";
124  std::string last_link = sss.str();
125 
126 
127 
128  YAML::Node node;
129  //YAML::Comment("yaml-language-server: $schema=/home/matteo/reachability/src/yaml_to_mtc/config/yaml_to_mtc_schema.json");
130  YAML::Node planner_node;
131  planner_node["id"] = "cartesian";
132  planner_node["type"] = "CartesianPath";
133  node["planners"].push_back(planner_node);
134  planner_node.reset();
135 
136  planner_node["id"] = "sampling";
137  planner_node["type"] = "PipelinePlanner";
138  planner_node["properties"]["step_size"] = 0.005f;
139  planner_node["properties"]["goal_joint_tolerance"] = static_cast<double>(0.00001f);
140  node["planners"].push_back(planner_node);
141  planner_node.reset();
142 
143  planner_node["id"] = "interpolation";
144  planner_node["type"] = "JointInterpolationPlanner";
145  node["planners"].push_back(planner_node);
146  planner_node.reset();
147 
148 
149  node["task"]["name"] = "Pick and Place test";
150  node["task"]["properties"]["group"] = robot->name();
151  node["task"]["properties"]["eef"] = hand;
152  node["task"]["properties"]["hand_grasping_frame"] = last_link;
153  node["task"]["properties"]["ik_frame"] = last_link;
154  node["task"]["properties"]["hand"] = hand;
155 
156  YAML::Node stage;
157  stage["name"] = "current";
158  stage["type"] = "CurrentState";
159  node["task"]["stages"].push_back(stage);
160  stage.reset();
161 
162  stage["name"] = "move to ready";
163  stage["type"] = "MoveTo";
164  stage["id"] = "ready";
165  stage["planner"] = "sampling";
166  stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
167  stage["propertiesConfigureInitFrom"]["values"].push_back("group");
168  stage["set"]["goal"] = "ready";
169  node["task"]["stages"].push_back(stage);
170  stage.reset();
171 
172  stage["type"] = "MoveTo";
173  stage["planner"] = "sampling";
174  stage["id"] = "hand_open";
175  stage["properties"]["group"] = hand;
176  stage["set"]["goal"] = "open";
177  node["task"]["stages"].push_back(stage);
178  stage.reset();
179 
180  stage["type"] = "Connect";
181  stage["group_planner_vector"][robot->name()] = "sampling";
182  stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
183  node["task"]["stages"].push_back(stage);
184  stage.reset();
185 
186  stage["type"] = "SerialContainer";
187  stage["name"] = "grasp";
188  stage["properties_exposeTo"]["source"] = "task";
189  stage["properties_exposeTo"]["values"] = YAML::Load("[eef, hand, group, ik_frame]");
190  stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
191  stage["propertiesConfigureInitFrom"]["values"] = YAML::Load("[eef, hand, group, ik_frame]");
192  //node["task"]["stages"].push_back(stage);
193 
194  YAML::Node stage_in_stage;
195  stage_in_stage["type"] = "MoveRelative";
196  stage_in_stage["planner"] = "cartesian";
197  stage_in_stage["properties"]["link"] = last_link;
198  stage_in_stage["properties"]["min_distance"] = 0.07f;
199  stage_in_stage["properties"]["max_distance"] = 0.2f;
200  stage_in_stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
201  stage_in_stage["propertiesConfigureInitFrom"]["values"] = YAML::Load("[group]");
202  stage_in_stage["set"]["direction"]["vector"] = YAML::Load("{x: 0.0, y: 0.0, z: 1.0, header: { frame_id: " + last_link +" }}");
203  stage["stages"].push_back(stage_in_stage);
204  stage_in_stage.reset();
205 
206  stage_in_stage["type"] = "ComputeIK";
207  stage_in_stage["properties"] = YAML::Load("{max_ik_solutions: 5}");
208  stage_in_stage["set"]["ik_frame"]["isometry"]["translation"] = YAML::Load("{ x: 0.1, y: 0.0, z: 0.0 }");
209  stage_in_stage["set"]["ik_frame"]["isometry"]["quaternion"] = YAML::Load("{ r: 1.571, p: 0.785, y: 1.571 }");
210  stage_in_stage["set"]["ik_frame"]["link"] = last_link;
211 
212  YAML::Node properties_in_stage_in_stage;
213  properties_in_stage_in_stage["source"]= "PARENT";
214  properties_in_stage_in_stage["values"]= YAML::Load("[eef, group]");
215  stage_in_stage["propertiesConfigureInitFrom"].push_back(properties_in_stage_in_stage);
216  properties_in_stage_in_stage.reset();
217 
218  properties_in_stage_in_stage["source"]= "INTERFACE";
219  properties_in_stage_in_stage["values"]= YAML::Load("[target_pose]");
220  stage_in_stage["propertiesConfigureInitFrom"].push_back(properties_in_stage_in_stage);
221  properties_in_stage_in_stage.reset();
222 
223  stage_in_stage["stage"]["type"] = "GenerateGraspPose";
224  stage_in_stage["stage"]["propertiesConfigureInitFrom"]["source"] = "PARENT";
225  stage_in_stage["stage"]["properties"]["object"] = "bottle";
226  stage_in_stage["stage"]["properties"]["angle_delta"] = 1.571f;
227  stage_in_stage["stage"]["properties"]["pregrasp"] = "open";
228  stage_in_stage["stage"]["set"]["monitored_stage"] = "ready";
229  stage["stages"].push_back(stage_in_stage);
230  stage_in_stage.reset();
231 
232  stage_in_stage["type"] = "ModifyPlanningScene";
233  stage_in_stage["set"]["allow_collisions"]["first"] = "bottle";
234  stage_in_stage["set"]["allow_collisions"]["second"]["joint_model_group_name"] = hand;
235  stage_in_stage["set"]["allow_collisions"]["allow"] = true;
236  stage["stages"].push_back(stage_in_stage);
237  stage_in_stage.reset();
238 
239  stage_in_stage["type"] = "MoveTo";
240  stage_in_stage["planner"] = "sampling";
241  stage_in_stage["properties"]["group"] = hand;
242  stage_in_stage["set"]["goal"] = "close";
243  stage["stages"].push_back(stage_in_stage);
244  stage_in_stage.reset();
245 
246  stage_in_stage["type"] = "ModifyPlanningScene";
247  stage_in_stage["set"]["attach_object"]["object"] = "bottle";
248  stage_in_stage["set"]["attach_object"]["link"] = last_link;
249  stage["stages"].push_back(stage_in_stage);
250  stage_in_stage.reset();
251 
252  stage_in_stage["type"] = "MoveRelative";
253  stage_in_stage["planner"] = "cartesian";
254  stage_in_stage["id"] = "pick_up";
255  stage_in_stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
256  stage_in_stage["propertiesConfigureInitFrom"]["values"] = YAML::Load("[group]");
257  stage_in_stage["properties"]["min_distance"] = 0.1f;
258  stage_in_stage["properties"]["max_distance"] = 0.2f;
259  stage_in_stage["set"]["ik_frame"]["link"] = last_link;
260  stage_in_stage["set"]["direction"]["vector"] = YAML::Load("{ x: 0.0, y: 0.0, z: 1.0 }");
261  stage["stages"].push_back(stage_in_stage);
262  stage_in_stage.reset();
263  node["task"]["stages"].push_back(stage);
264 
265  stage.reset();
266 
267  stage["type"] = "Connect";
268  stage["group_planner_vector"][robot->name()] = "sampling";
269  stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
270  node["task"]["stages"].push_back(stage);
271  stage.reset();
272 
273  // satrtet here
274  stage["type"] = "SerialContainer";
275  stage["name"] = "place";
276  stage["properties_exposeTo"]["source"] = "task";
277  stage["properties_exposeTo"]["values"] = YAML::Load("[eef, hand, group, ik_frame]");
278  stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
279 
280  stage_in_stage["type"] = "MoveRelative";
281  stage_in_stage["planner"] = "cartesian";
282  stage_in_stage["properties"]["link"] = last_link;
283  stage_in_stage["properties"]["min_distance"] = 0.1f;
284  stage_in_stage["properties"]["max_distance"] = 0.2f;
285  stage_in_stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
286  stage_in_stage["propertiesConfigureInitFrom"]["values"] = YAML::Load("[group]");
287  stage_in_stage["set"]["direction"]["vector"] = YAML::Load("{ x: 0.0, y: 0.0, z: -1.0}");
288  stage["stages"].push_back(stage_in_stage);
289  stage_in_stage.reset();
290 
291  stage_in_stage["type"] = "ComputeIK";
292  stage_in_stage["properties"] = YAML::Load("{ max_ik_solutions: 5 }");
293  stage_in_stage["set"]["ik_frame"]["isometry"]["translation"] = YAML::Load("{ x: 0.1, y: 0.0, z: 0.0 }");
294  stage_in_stage["set"]["ik_frame"]["isometry"]["quaternion"] = YAML::Load("{ r: 1.571, p: 0.785, y: 1.571 }");
295  stage_in_stage["set"]["ik_frame"]["link"] = last_link;
296 
297  properties_in_stage_in_stage["source"]= "PARENT";
298  properties_in_stage_in_stage["values"]= YAML::Load("[eef, group]");
299  stage_in_stage["propertiesConfigureInitFrom"].push_back(properties_in_stage_in_stage);
300  properties_in_stage_in_stage.reset();
301 
302  properties_in_stage_in_stage["source"]= "INTERFACE";
303  properties_in_stage_in_stage["values"]= YAML::Load("[target_pose]");
304  stage_in_stage["propertiesConfigureInitFrom"].push_back(properties_in_stage_in_stage);
305  properties_in_stage_in_stage.reset();
306 
307  stage_in_stage["stage"]["type"] = "GeneratePose";
308  stage_in_stage["stage"]["set"]["monitored_stage"] = "pick_up";
309  stage_in_stage["stage"]["set"]["pose"]["point"] = YAML::Load("{ x: " + std::to_string(x) + ", y: " + std::to_string(y) + ", z: 0.9305 }"); // Hier objekt
310  stage_in_stage["stage"]["set"]["pose"]["orientation"] = YAML::Load("{ x: 0.0, y: 0.0, z: 0.0, w: 1.0}");
311  stage["stages"].push_back(stage_in_stage);
312  stage_in_stage.reset();
313 
314  stage_in_stage["type"] = "MoveTo";
315  stage_in_stage["planner"] = "sampling";
316  stage_in_stage["properties"]["group"] = hand;
317  stage_in_stage["set"]["goal"] = "open";
318  stage["stages"].push_back(stage_in_stage);
319  stage_in_stage.reset();
320 
321  stage_in_stage["type"] = "ModifyPlanningScene";
322  stage_in_stage["set"]["detach_object"]["object"]= "bottle";
323  stage_in_stage["set"]["detach_object"]["link"]= last_link;
324  stage_in_stage["set"]["allow_collisions"]["first"]= "bottle";
325  stage_in_stage["set"]["allow_collisions"]["second"]["joint_model_group_name"]= hand;
326  stage_in_stage["set"]["allow_collisions"]["allow"] = false;
327  stage["stages"].push_back(stage_in_stage);
328  stage_in_stage.reset();
329 
330  stage_in_stage["type"] = "MoveRelative";
331  stage_in_stage["planner"] = "cartesian";
332  stage_in_stage["properties"]["link"] = last_link;
333  stage_in_stage["properties"]["min_distance"] = 0.07f;
334  stage_in_stage["properties"]["max_distance"] = 0.2f;
335  stage_in_stage["propertiesConfigureInitFrom"]["source"] = "PARENT";
336  stage_in_stage["propertiesConfigureInitFrom"]["values"] = YAML::Load("[group]");
337  stage_in_stage["set"]["direction"]["vector"] = YAML::Load("{x: 0.0, y: 0.0, z: -1.0, header: { frame_id: " + last_link+ " }}");
338  stage["stages"].push_back(stage_in_stage);
339  stage_in_stage.reset();
340 
341  stage_in_stage["type"] = "MoveTo";
342  stage_in_stage["planner"] = "sampling";
343  stage_in_stage["properties"]["group"] = hand;
344  stage_in_stage["set"]["goal"] = "close";
345  stage["stages"].push_back(stage_in_stage);
346  stage_in_stage.reset();
347 
348  stage_in_stage["name"] = "move to ready";
349  stage_in_stage["type"] = "MoveTo";
350  stage_in_stage["planner"] = "sampling";
351  stage_in_stage["propertiesConfigureInitFrom"]["source"]= "PARENT";
352  stage_in_stage["propertiesConfigureInitFrom"]["values"].push_back("group");
353  stage_in_stage["set"]["goal"] = "ready";
354  stage["stages"].push_back(stage_in_stage);
355  stage_in_stage.reset();
356  node["task"]["stages"].push_back(stage);
357  stage.reset();
358 
359  node["max_planning_solutions"] = 10;
360  o << node;
361  o.close();
362 
363 */
364 }
365 
Abstract_base::task_space_
std::map< const std::string, std::vector< std::pair< object_data, std::vector< tf2::Quaternion > > > > task_space_
Drop-off locations with their grasp orientations, mapped to a robot.
Definition: abstract_base.h:35
Abstract_base::map_
std::vector< tf2::Transform > map_
Reachability map structure of a robot.
Definition: abstract_base.h:33
Simple_base::write_task
void write_task(Abstract_robot *robot) override
write trask
Definition: simple_base.cpp:366
Abstract_base
Abstract base class.
Definition: abstract_base.h:29
Simple_base::Simple_base
Simple_base(std::shared_ptr< ros::NodeHandle > const &d)
Simple base constructor.
Definition: simple_base.cpp:4
object_data
Definition: abstract_param_reader.h:13
Map_reader
Map reader.
Definition: map_reader.h:15
Simple_base::base_calculation
std::map< const std::string, std::vector< pcl::PointXYZ > > base_calculation() override
refined Template methode
Definition: simple_base.cpp:20
Abstract_base::implementation_
std::shared_ptr< Abstract_base_implementation > implementation_
refined implementation
Definition: abstract_base.h:38
Ts_reader
TS reader.
Definition: ts_reader.h:15
Abstract_base::inv_map_
std::vector< tf2::Transform > inv_map_
Inversion of reachability map structure.
Definition: abstract_base.h:34
Simple_base::task_space_reader_
std::unique_ptr< Ts_reader > task_space_reader_
Definition: simple_base.h:28
Simple_base::map_reader_
std::unique_ptr< Map_reader > map_reader_
Definition: simple_base.h:29
Abstract_robot
Definition: impl/abstract_robot.h:25
simple_base.h
abstract_base_implementation.h


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