task_space_reader.cpp
Go to the documentation of this file.
2 
3 
5  std::string filename = "dummy.yaml";
6  //nh_->getParam("/resource_name", filename);
7 
8  XmlRpc::XmlRpcValue resources;
9  nh_->getParam("/objects", resources);
10 
11  std::vector<tf2::Vector3> robot_middle;
12  std::vector<tf2::Vector3>* robot_pointer = nullptr;
13 
14 
15  std::regex rx("table([0-9]+)_table_top");
16  std::smatch match;
17 
18  ROS_INFO("--- TASK_SPACE_READER ---");
19  std::map<const std::string, std::vector<tf2::Vector3>> map;
20 
21  for(int i = 0; i < resources.size(); i++){
22  std::string str = resources[i]["id"];
23  if (std::regex_match(str, match, rx)){
24  std::stringstream ss;
25  ss << "panda_arm" << match[1].str().c_str();
26  map.insert(std::make_pair<const std::string, std::vector<tf2::Vector3>>(ss.str().c_str(), std::vector<tf2::Vector3>()));
27  }
28  }
29 
30  tf2::Vector3 pos(0,0,0);
31  for (int i = 0; i < resources.size(); i++){
32  if (resources[i]["type"] == "DROP_OFF_LOCATION"){
33  std::string str = resources[i]["id"];
34  if(str[0] == 'A') robot_pointer = &map.at("panda_arm1");
35  if(str[0] == 'B') robot_pointer = &map.at("panda_arm1");
36  if(str[0] == 'C') robot_pointer = &map.at("panda_arm1");
37  if(str[0] == 'D') robot_pointer = &map.at("panda_arm1");
38  if(str[0] == 'E') robot_pointer = &robot_middle;
39  if(str[0] == 'F') robot_pointer = &robot_middle;
40  if(str[0] == 'G') robot_pointer = &map.at("panda_arm2");
41  if(str[0] == 'H') robot_pointer = &map.at("panda_arm2");
42  if(str[0] == 'I') robot_pointer = &map.at("panda_arm2");
43  if(str[0] == 'J') robot_pointer = &map.at("panda_arm2");
44 
45  if (robot_pointer){
46  tf2::Vector3 pos(0,0,0);
47  (resources[i]["pos"].hasMember("x")) ? pos.setX(floatOf(resources[i]["pos"]["x"])) :pos.setX(0);
48  (resources[i]["pos"].hasMember("y")) ? pos.setY(floatOf(resources[i]["pos"]["y"])) :pos.setY(0);
49  (resources[i]["pos"].hasMember("z")) ? pos.setZ(floatOf(resources[i]["pos"]["z"])) :pos.setZ(0);
50  robot_pointer->push_back(pos);
51  }
52  }
53  robot_pointer = nullptr;
54  }
55 
56  // lets make some result data
57  ROS_INFO("writing file...");
58  std::ofstream o(ros::package::getPath("multi_cell_builder") + "/descriptions/" + filename);
59  YAML::Node node;
60 
61  for(auto robot: map){
62  for(tf2::Vector3& vec : robot.second){
63  node["task"]["groups"][robot.first].push_back(YAML::Load(std::string("{ 'pos': {'x': ") + std::to_string(vec.getX()) + ", 'y': " + std::to_string(vec.getY()) + ", 'z':" + std::to_string(vec.getZ()) + std::string("}}")));
64  }
65  }
66 
67  for(auto robot: map){
68  for(tf2::Vector3& vec : robot_middle){
69  node["task"]["groups"][robot.first].push_back(YAML::Load(std::string("{ 'pos': {'x': ") + std::to_string(vec.getX()) + ", 'y': " + std::to_string(vec.getY()) + ", 'z':" + std::to_string(vec.getZ()) + std::string("}}")));
70  }
71  }
72  o << node;
73  o.close();
74 }
AbstractParamReader::nh_
std::shared_ptr< ros::NodeHandle > nh_
ROS Nodehandle.
Definition: abstract_param_reader.h:31
AbstractParamReader::floatOf
float floatOf(XmlRpc::XmlRpcValue &val)
Xmlrpc parser.
Definition: abstract_param_reader.cpp:3
TaskSpaceReader::read
void read() override
read implementatin
Definition: task_space_reader.cpp:4
task_space_reader.h


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