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


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