wing_reader.cpp
Go to the documentation of this file.
1 #include "reader/wing_reader.h"
2 
4  XmlRpc::XmlRpcValue resources;
5  nh_->getParam("/objects", resources);
6 
7  std::regex rx("table([0-9]+)_table_top");
8  std::smatch match;
9 
10  std::map<const std::string, std::pair<std::vector<object_data>, int>> map;
11 
12  ROS_INFO("--- WING_READER ---");
13  try{
14  for(int i = 0; i < resources.size(); i++){
15  std::string str = resources[i]["id"];
16  if(std::regex_match(str, match, rx)){
17  std::stringstream ss;
18  ss << "panda_arm" << match[1].str().c_str();
19  std::vector<object_data> wd;
20  wd.push_back({"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)});
21  wd.push_back({"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)});
22  wd.push_back({"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)});
23  map.insert(std::pair<const std::string, std::pair<std::vector<object_data>, int>>(ss.str(), {wd, 0}));
24  }
25  }
26  } catch (const XmlRpc::XmlRpcException& xre) {
27  ROS_INFO("Iteration error in Blueprint initialization");
28  ros::shutdown();
29  }
30 
31 
32  rx.assign("table([0-9]+)_right_panel");
33  try{
34  for(int i = 0; i < resources.size(); i++){
35  std::string str = resources[i]["id"];
36  if (std::regex_match(str, match, rx)){
37  std::stringstream ss;
38  ss << "panda_arm" << match[1].str().c_str();
39 
40  ROS_INFO("=> WING: description found, loading...");
41  tf2::Vector3 pos;
42  tf2::Vector3 size;
43  tf2::Quaternion rot;
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"]) + size.getZ()/2) :pos.setZ(0);
52  (resources[i]["orientation"].hasMember("x")) ? rot.setX(floatOf(resources[i]["orientation"]["x"])) :rot.setX(0);
53  (resources[i]["orientation"].hasMember("y")) ? rot.setY(floatOf(resources[i]["orientation"]["y"])) :rot.setY(0);
54  (resources[i]["orientation"].hasMember("z")) ? rot.setZ(floatOf(resources[i]["orientation"]["z"])) :rot.setZ(0);
55  (resources[i]["orientation"].hasMember("w")) ? rot.setW(floatOf(resources[i]["orientation"]["w"])) :rot.setW(0);
56 
57  ROS_INFO("--- Wing %s ---", str.c_str());
58  ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
59  ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
60  ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
61 
62  try {
63  map.at(ss.str()).first[0].name_ = str.c_str();
64  map.at(ss.str()).first[0].pose_ = tf2::Transform(rot.normalized(), pos);
65  map.at(ss.str()).first[0].size_ = size;
66  map.at(ss.str()).second += std::pow(2, 0);
67  } catch (const std::out_of_range& oor) {
68  ROS_INFO("Found panel for unknown robot");
69  }
70  }
71  }
72  } catch (const XmlRpc::XmlRpcException& xre){
73  ROS_INFO("Iteration error in right panel search");
74  ros::shutdown();
75  }
76 
77 
78  rx.assign("table([0-9]+)_front_panel");
79  try {
80  for(int i = 0; i < resources.size(); i++){
81  std::string str = resources[i]["id"];
82  if (std::regex_match(str, match, rx)){
83  std::stringstream ss;
84  ss << "panda_arm" << match[1].str().c_str();
85 
86  ROS_INFO("=> WING: description found, loading...");
87  tf2::Vector3 pos;
88  tf2::Vector3 size;
89  tf2::Quaternion rot;
90 
91 
92  (resources[i]["size"].hasMember("length")) ? size.setX(floatOf(resources[i]["size"]["length"])) :size.setX(0);
93  (resources[i]["size"].hasMember("width")) ? size.setY(floatOf(resources[i]["size"]["width"])) :size.setY(0);
94  (resources[i]["size"].hasMember("height")) ? size.setZ(floatOf(resources[i]["size"]["height"])) :size.setZ(0);
95 
96  (resources[i]["pos"].hasMember("x")) ? pos.setX(floatOf(resources[i]["pos"]["x"])) :pos.setX(0);
97  (resources[i]["pos"].hasMember("y")) ? pos.setY(floatOf(resources[i]["pos"]["y"])) :pos.setY(0);
98  (resources[i]["pos"].hasMember("z")) ? pos.setZ(floatOf(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
99  (resources[i]["orientation"].hasMember("x")) ? rot.setX(floatOf(resources[i]["orientation"]["x"])) :rot.setX(0);
100  (resources[i]["orientation"].hasMember("y")) ? rot.setY(floatOf(resources[i]["orientation"]["y"])) :rot.setY(0);
101  (resources[i]["orientation"].hasMember("z")) ? rot.setZ(floatOf(resources[i]["orientation"]["z"])) :rot.setZ(0);
102  (resources[i]["orientation"].hasMember("w")) ? rot.setW(floatOf(resources[i]["orientation"]["w"])) :rot.setW(0);
103 
104  ROS_INFO("--- Wing %s ---", str.c_str());
105  ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
106  ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
107  ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
108 
109  try {
110  map.at(ss.str()).first[1].name_ = str.c_str();
111  map.at(ss.str()).first[1].pose_ = tf2::Transform(rot.normalized(), pos);
112  map.at(ss.str()).first[1].size_ = size;
113  map.at(ss.str()).second += std::pow(2, 1);
114  } catch (const std::out_of_range& oor) {
115  ROS_INFO("Found panel for unknown robot");
116  }
117  }
118  }
119  } catch(const XmlRpc::XmlRpcException& xre){
120  ROS_INFO("Iteration error in front panel search");
121  ros::shutdown();
122  }
123 
124 
125  rx.assign("table([0-9]+)_left_panel");
126  try{
127  for(int i = 0; i < resources.size(); i++){
128  std::string str = resources[i]["id"];
129  if (std::regex_match(str, match, rx)){
130  std::stringstream ss;
131  ss << "panda_arm" << match[1].str().c_str();
132 
133  ROS_INFO("=> WING: description found, loading...");
134  tf2::Vector3 pos;
135  tf2::Vector3 size;
136  tf2::Quaternion rot;
137 
138 
139  (resources[i]["size"].hasMember("length")) ? size.setX(floatOf(resources[i]["size"]["length"])) :size.setX(0);
140  (resources[i]["size"].hasMember("width")) ? size.setY(floatOf(resources[i]["size"]["width"])) :size.setY(0);
141  (resources[i]["size"].hasMember("height")) ? size.setZ(floatOf(resources[i]["size"]["height"])) :size.setZ(0);
142 
143  (resources[i]["pos"].hasMember("x")) ? pos.setX(floatOf(resources[i]["pos"]["x"])) :pos.setX(0);
144  (resources[i]["pos"].hasMember("y")) ? pos.setY(floatOf(resources[i]["pos"]["y"])) :pos.setY(0);
145  (resources[i]["pos"].hasMember("z")) ? pos.setZ(floatOf(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
146  (resources[i]["orientation"].hasMember("x")) ? rot.setX(floatOf(resources[i]["orientation"]["x"])) :rot.setX(0);
147  (resources[i]["orientation"].hasMember("y")) ? rot.setY(floatOf(resources[i]["orientation"]["y"])) :rot.setY(0);
148  (resources[i]["orientation"].hasMember("z")) ? rot.setZ(floatOf(resources[i]["orientation"]["z"])) :rot.setZ(0);
149  (resources[i]["orientation"].hasMember("w")) ? rot.setW(floatOf(resources[i]["orientation"]["w"])) :rot.setW(0);
150 
151  ROS_INFO("--- Wing %s ---", str.c_str());
152  ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
153  ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
154  ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
155 
156  try {
157  map.at(ss.str()).first[2].name_ = str.c_str();
158  map.at(ss.str()).first[2].pose_ = tf2::Transform(rot.normalized(), pos);
159  map.at(ss.str()).first[2].size_ = size;
160  map.at(ss.str()).second += std::pow(2, 2);
161  } catch (const std::out_of_range& oor) {
162  ROS_INFO("Found panel for unknown robot");
163  }
164  }
165  }
166  } catch(const XmlRpc::XmlRpcException& xre){
167  ROS_INFO("Iteration error in left panel search");
168  ros::shutdown();
169  }
170  setWingData(map);
171 }
AbstractParamReader::nh_
std::shared_ptr< ros::NodeHandle > nh_
ROS Nodehandle.
Definition: abstract_param_reader.h:31
WingReader::read
void read() override
read implementatin
Definition: wing_reader.cpp:3
wing_reader.h
AbstractParamReader::floatOf
float floatOf(XmlRpc::XmlRpcValue &val)
Xmlrpc parser.
Definition: abstract_param_reader.cpp:3
WingReader::setWingData
void setWingData(std::map< const std::string, std::pair< std::vector< object_data >, int >> &wing_data)
Set wing data.
Definition: wing_reader.h:30


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