4 XmlRpc::XmlRpcValue resources;
5 nh_->getParam(
"/objects", resources);
7 std::regex rx(
"table([0-9]+)_table_top");
10 std::map<const std::string, std::pair<std::vector<object_data>,
int>> map;
12 ROS_INFO(
"--- WING_READER ---");
14 for(
int i = 0; i < resources.size(); i++){
15 std::string str = resources[i][
"id"];
16 if(std::regex_match(str, match, rx)){
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}));
26 }
catch (
const XmlRpc::XmlRpcException& xre) {
27 ROS_INFO(
"Iteration error in Blueprint initialization");
32 rx.assign(
"table([0-9]+)_right_panel");
34 for(
int i = 0; i < resources.size(); i++){
35 std::string str = resources[i][
"id"];
36 if (std::regex_match(str, match, rx)){
38 ss <<
"panda_arm" << match[1].str().c_str();
40 ROS_INFO(
"=> WING: description found, loading...");
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);
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);
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());
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");
72 }
catch (
const XmlRpc::XmlRpcException& xre){
73 ROS_INFO(
"Iteration error in right panel search");
78 rx.assign(
"table([0-9]+)_front_panel");
80 for(
int i = 0; i < resources.size(); i++){
81 std::string str = resources[i][
"id"];
82 if (std::regex_match(str, match, rx)){
84 ss <<
"panda_arm" << match[1].str().c_str();
86 ROS_INFO(
"=> WING: description found, loading...");
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);
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);
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());
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");
119 }
catch(
const XmlRpc::XmlRpcException& xre){
120 ROS_INFO(
"Iteration error in front panel search");
125 rx.assign(
"table([0-9]+)_left_panel");
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();
133 ROS_INFO(
"=> WING: description found, loading...");
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);
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);
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());
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");
166 }
catch(
const XmlRpc::XmlRpcException& xre){
167 ROS_INFO(
"Iteration error in left panel search");