4 XmlRpc::XmlRpcValue resources;
5 nh_->getParam(
"/objects", resources);
7 std::vector<object_data> robot_middle;
8 std::vector<object_data>* robot_pointer =
nullptr;
11 std::regex rx(
"table([0-9]+)_table_top");
14 ROS_INFO(
"--- TASK_SPACE_READER ---");
15 std::map<const std::string, std::vector<object_data>> map;
17 for(
int i = 0; i < resources.size(); i++){
18 std::string str = resources[i][
"id"];
19 if (std::regex_match(str, match, rx)){
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>()));
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");
41 tf2::Vector3 pos(0,0,0);
42 tf2::Quaternion rot(0,0,0,1);
43 tf2::Vector3 size(0,0,0);
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"])) :pos.setZ(0);
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);
58 robot_pointer->push_back({
"", tf2::Transform(rot, pos), size});
61 robot_pointer =
nullptr;
64 for(
auto& robot: map){
66 robot.second.push_back(tf);