5 std::string filename =
"dummy.yaml";
8 XmlRpc::XmlRpcValue resources;
9 nh_->getParam(
"/objects", resources);
11 std::vector<tf2::Vector3> robot_middle;
12 std::vector<tf2::Vector3>* robot_pointer =
nullptr;
15 std::regex rx(
"table([0-9]+)_table_top");
18 ROS_INFO(
"--- TASK_SPACE_READER ---");
19 std::map<const std::string, std::vector<tf2::Vector3>> map;
21 for(
int i = 0; i < resources.size(); i++){
22 std::string str = resources[i][
"id"];
23 if (std::regex_match(str, match, rx)){
25 ss <<
"panda_arm" << match[1].str().c_str();
26 map.insert(std::make_pair<
const std::string, std::vector<tf2::Vector3>>(ss.str().c_str(), std::vector<tf2::Vector3>()));
30 tf2::Vector3 pos(0,0,0);
31 for (
int i = 0; i < resources.size(); i++){
32 if (resources[i][
"type"] ==
"DROP_OFF_LOCATION"){
33 std::string str = resources[i][
"id"];
34 if(str[0] ==
'A') robot_pointer = &map.at(
"panda_arm1");
35 if(str[0] ==
'B') robot_pointer = &map.at(
"panda_arm1");
36 if(str[0] ==
'C') robot_pointer = &map.at(
"panda_arm1");
37 if(str[0] ==
'D') robot_pointer = &map.at(
"panda_arm1");
38 if(str[0] ==
'E') robot_pointer = &robot_middle;
39 if(str[0] ==
'F') robot_pointer = &robot_middle;
40 if(str[0] ==
'G') robot_pointer = &map.at(
"panda_arm2");
41 if(str[0] ==
'H') robot_pointer = &map.at(
"panda_arm2");
42 if(str[0] ==
'I') robot_pointer = &map.at(
"panda_arm2");
43 if(str[0] ==
'J') robot_pointer = &map.at(
"panda_arm2");
46 tf2::Vector3 pos(0,0,0);
47 (resources[i][
"pos"].hasMember(
"x")) ? pos.setX(
floatOf(resources[i][
"pos"][
"x"])) :pos.setX(0);
48 (resources[i][
"pos"].hasMember(
"y")) ? pos.setY(
floatOf(resources[i][
"pos"][
"y"])) :pos.setY(0);
49 (resources[i][
"pos"].hasMember(
"z")) ? pos.setZ(
floatOf(resources[i][
"pos"][
"z"])) :pos.setZ(0);
50 robot_pointer->push_back(pos);
53 robot_pointer =
nullptr;
57 ROS_INFO(
"writing file...");
58 std::ofstream o(ros::package::getPath(
"multi_cell_builder") +
"/descriptions/" + filename);
62 for(tf2::Vector3& vec : robot.second){
63 node[
"task"][
"groups"][robot.first].push_back(YAML::Load(std::string(
"{ 'pos': {'x': ") + std::to_string(vec.getX()) +
", 'y': " + std::to_string(vec.getY()) +
", 'z':" + std::to_string(vec.getZ()) + std::string(
"}}")));
68 for(tf2::Vector3& vec : robot_middle){
69 node[
"task"][
"groups"][robot.first].push_back(YAML::Load(std::string(
"{ 'pos': {'x': ") + std::to_string(vec.getX()) +
", 'y': " + std::to_string(vec.getY()) +
", 'z':" + std::to_string(vec.getZ()) + std::string(
"}}")));