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


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