ts_reader.cpp
Go to the documentation of this file.
1 #include "reader/ts_reader.h"
2 
4  std::string filename = "dummy.yaml";
5  //nh_->getParam("/resource_name", filename);
6 
7  XmlRpc::XmlRpcValue resources;
8  nh_->getParam("/objects", resources);
9 
10  std::vector<object_data> robot_middle;
11  std::vector<object_data>* robot_pointer = nullptr;
12 
13 
14  std::regex rx("table([0-9]+)_table_top");
15  std::smatch match;
16 
17  ROS_INFO("--- TASK_SPACE_READER ---");
18  std::map<const std::string, std::vector<object_data>> map;
19 
20  for(int i = 0; i < resources.size(); i++){
21  std::string str = resources[i]["id"];
22  if (std::regex_match(str, match, rx)){
23  std::stringstream ss;
24  ss << "panda_arm" << match[1].str().c_str();
25  map.insert(std::make_pair<const std::string, std::vector<object_data>>(ss.str().c_str(), std::vector<object_data>()));
26  }
27  }
28 
29  for (int i = 0; i < resources.size(); i++){
30  if (resources[i]["type"] == "DROP_OFF_LOCATION"){
31  std::string str = resources[i]["id"];
32  if(str[0] == 'A') robot_pointer = &map.at("panda_arm1");
33  if(str[0] == 'B') robot_pointer = &map.at("panda_arm1");
34  if(str[0] == 'C') robot_pointer = &map.at("panda_arm1");
35  if(str[0] == 'D') robot_pointer = &map.at("panda_arm1");
36  if(str[0] == 'E') robot_pointer = &robot_middle;
37  if(str[0] == 'F') robot_pointer = &robot_middle;
38  if(str[0] == 'G') robot_pointer = &map.at("panda_arm2");
39  if(str[0] == 'H') robot_pointer = &map.at("panda_arm2");
40  if(str[0] == 'I') robot_pointer = &map.at("panda_arm2");
41  if(str[0] == 'J') robot_pointer = &map.at("panda_arm2");
42 
43  if (robot_pointer){
44  tf2::Vector3 pos(0,0,0);
45  tf2::Quaternion rot(0,0,0,1);
46  tf2::Vector3 size(0,0,0);
47 
48  (resources[i]["size"].hasMember("length")) ? size.setX(float_of(resources[i]["size"]["length"])) :size.setX(0);
49  (resources[i]["size"].hasMember("width")) ? size.setY(float_of(resources[i]["size"]["width"])) :size.setY(0);
50  (resources[i]["size"].hasMember("height")) ? size.setZ(float_of(resources[i]["size"]["height"])) :size.setZ(0);
51 
52  (resources[i]["pos"].hasMember("x")) ? pos.setX(float_of(resources[i]["pos"]["x"])) :pos.setX(0);
53  (resources[i]["pos"].hasMember("y")) ? pos.setY(float_of(resources[i]["pos"]["y"])) :pos.setY(0);
54  (resources[i]["pos"].hasMember("z")) ? pos.setZ(float_of(resources[i]["pos"]["z"])) :pos.setZ(0);
55 
56  (resources[i]["orientation"].hasMember("x")) ? pos.setZ(float_of(resources[i]["orientation"]["x"])) :pos.setX(0);
57  (resources[i]["orientation"].hasMember("y")) ? pos.setZ(float_of(resources[i]["orientation"]["y"])) :pos.setY(0);
58  (resources[i]["orientation"].hasMember("z")) ? pos.setZ(float_of(resources[i]["orientation"]["z"])) :pos.setZ(0);
59  (resources[i]["orientation"].hasMember("w")) ? pos.setZ(float_of(resources[i]["orientation"]["w"])) :pos.setW(0);
60 
61  robot_pointer->push_back({"", tf2::Transform(rot, pos), size});
62  }
63  }
64  robot_pointer = nullptr;
65  }
66 
67  for(auto& robot: map){
68  for(object_data& tf : robot_middle){
69  robot.second.push_back(tf);
70  }
71  }
72 
73  set_drop_off_data(map);
74 }
Ts_reader::read
void read() override
read implementatin
Definition: ts_reader.cpp:3
Abstract_param_reader::nh_
std::shared_ptr< ros::NodeHandle > nh_
ROS Nodehandle.
Definition: abstract_param_reader.h:29
object_data
Definition: abstract_param_reader.h:13
ts_reader.h
Ts_reader::set_drop_off_data
void set_drop_off_data(std::map< const std::string, std::vector< object_data >> &dod)
Set drop off data.
Definition: ts_reader.h:37
Abstract_param_reader::float_of
float float_of(XmlRpc::XmlRpcValue &val)
Xmlrpc parser.
Definition: abstract_param_reader.cpp:3


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