4 std::string filename =
"dummy.yaml";
7 XmlRpc::XmlRpcValue resources;
8 nh_->getParam(
"/objects", resources);
10 std::vector<object_data> robot_middle;
11 std::vector<object_data>* robot_pointer =
nullptr;
14 std::regex rx(
"table([0-9]+)_table_top");
17 ROS_INFO(
"--- TASK_SPACE_READER ---");
18 std::map<const std::string, std::vector<object_data>> map;
20 for(
int i = 0; i < resources.size(); i++){
21 std::string str = resources[i][
"id"];
22 if (std::regex_match(str, match, rx)){
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>()));
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");
44 tf2::Vector3 pos(0,0,0);
45 tf2::Quaternion rot(0,0,0,1);
46 tf2::Vector3 size(0,0,0);
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);
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);
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);
61 robot_pointer->push_back({
"", tf2::Transform(rot, pos), size});
64 robot_pointer =
nullptr;
67 for(
auto& robot: map){
69 robot.second.push_back(tf);