cuboid_reader.cpp
Go to the documentation of this file.
1 #include "reader/cuboid_reader.h"
2 
4  ROS_INFO("--- CUBOID_READER ---");
5  XmlRpc::XmlRpcValue resources;
6  nh_->getParam("/objects", resources);
7  std::smatch match;
8 
9  for (int i = 0; i < resources.size(); i++){
10  std::string id = resources[i]["id"];
11  if(resources[i]["type"] == "BOX"){
12  tf2::Vector3 pos;
13  tf2::Vector3 size;
14  tf2::Quaternion rot;
15 
16  (resources[i]["size"].hasMember("length")) ? size.setX(floatOf(resources[i]["size"]["length"])) :size.setX(0);
17  (resources[i]["size"].hasMember("width")) ? size.setY(floatOf(resources[i]["size"]["width"])) :size.setY(0);
18  (resources[i]["size"].hasMember("height")) ? size.setZ(floatOf(resources[i]["size"]["height"])) :size.setZ(0);
19 
20  (resources[i]["pos"].hasMember("x")) ? pos.setX(floatOf(resources[i]["pos"]["x"])) :pos.setX(0);
21  (resources[i]["pos"].hasMember("y")) ? pos.setY(floatOf(resources[i]["pos"]["y"])) :pos.setY(0);
22  (resources[i]["pos"].hasMember("z")) ? pos.setZ(floatOf(resources[i]["pos"]["z"])) :pos.setZ(0);
23  (resources[i]["orientation"].hasMember("x")) ? rot.setX(floatOf(resources[i]["orientation"]["x"])) :rot.setX(0);
24  (resources[i]["orientation"].hasMember("y")) ? rot.setY(floatOf(resources[i]["orientation"]["y"])) :rot.setY(0);
25  (resources[i]["orientation"].hasMember("z")) ? rot.setZ(floatOf(resources[i]["orientation"]["z"])) :rot.setZ(0);
26  (resources[i]["orientation"].hasMember("w")) ? rot.setW(floatOf(resources[i]["orientation"]["w"])) :rot.setW(0);
27 
28  Cuboid o;
29  o.Name = id.c_str();
30  o.Pose.position.x = pos.getX();
31  o.Pose.position.y = pos.getY();
32  o.Pose.position.z = pos.getZ();
33  o.Pose.orientation.x = rot.getX();
34  o.Pose.orientation.y = rot.getY();
35  o.Pose.orientation.z = rot.getZ();
36  o.Pose.orientation.w = rot.getW();
37  o.x_depth = size.getX();
38  o.y_width = size.getY();
39  o.z_heigth = size.getZ();
40 
41 
42  cuboid_box_.push_back(o);
43  ROS_INFO("--- Object: %s --- Type: BOX ---", o.Name.c_str());
44  ROS_INFO("=> Object: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
45  ROS_INFO("=> Object: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
46  ROS_INFO("=> Object: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
47  }
48 
49  if(resources[i]["type"] == "BIN" || std::regex_match(id, match, std::regex("obstacle([0-9]+)"))){
50  tf2::Vector3 pos;
51  tf2::Vector3 size;
52  tf2::Quaternion rot;
53 
54  (resources[i]["size"].hasMember("length")) ? size.setX(floatOf(resources[i]["size"]["length"])) :size.setX(0);
55  (resources[i]["size"].hasMember("width")) ? size.setY(floatOf(resources[i]["size"]["width"])) :size.setY(0);
56  (resources[i]["size"].hasMember("height")) ? size.setZ(floatOf(resources[i]["size"]["height"])) :size.setZ(0);
57 
58  (resources[i]["pos"].hasMember("x")) ? pos.setX(floatOf(resources[i]["pos"]["x"])) :pos.setX(0);
59  (resources[i]["pos"].hasMember("y")) ? pos.setY(floatOf(resources[i]["pos"]["y"])) :pos.setY(0);
60  (resources[i]["pos"].hasMember("z")) ? pos.setZ(floatOf(resources[i]["pos"]["z"])) :pos.setZ(0);
61  (resources[i]["orientation"].hasMember("x")) ? rot.setX(floatOf(resources[i]["orientation"]["x"])) :rot.setX(0);
62  (resources[i]["orientation"].hasMember("y")) ? rot.setY(floatOf(resources[i]["orientation"]["y"])) :rot.setY(0);
63  (resources[i]["orientation"].hasMember("z")) ? rot.setZ(floatOf(resources[i]["orientation"]["z"])) :rot.setZ(0);
64  (resources[i]["orientation"].hasMember("w")) ? rot.setW(floatOf(resources[i]["orientation"]["w"])) :rot.setW(0);
65 
66  Cuboid o;
67  o.Name = id.c_str();
68  o.Pose.position.x = pos.getX();
69  o.Pose.position.y = pos.getY();
70  o.Pose.position.z = pos.getZ();
71  o.Pose.orientation.x = rot.getX();
72  o.Pose.orientation.y = rot.getY();
73  o.Pose.orientation.z = rot.getZ();
74  o.Pose.orientation.w = rot.getW();
75  o.x_depth = size.getX();
76  o.y_width = size.getY();
77  o.z_heigth = size.getZ();
78 
79  cuboid_obstacle_.push_back(o);
80  ROS_INFO("--- Object: %s --- Type: BIN ---", o.Name.c_str());
81  ROS_INFO("=> Object: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
82  ROS_INFO("=> Object: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
83  ROS_INFO("=> Object: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
84 
85  }
86  }
87 }
cuboid_reader.h
AbstractParamReader::nh_
std::shared_ptr< ros::NodeHandle > nh_
ROS Nodehandle.
Definition: abstract_param_reader.h:31
CuboidReader::read
void read() override
read implementatin
Definition: cuboid_reader.cpp:3
CuboidReader::cuboid_box_
std::vector< Cuboid > cuboid_box_
As box defined object.
Definition: cuboid_reader.h:19
CuboidReader::cuboid_obstacle_
std::vector< Cuboid > cuboid_obstacle_
As obstacle defined objects.
Definition: cuboid_reader.h:20
AbstractParamReader::floatOf
float floatOf(XmlRpc::XmlRpcValue &val)
Xmlrpc parser.
Definition: abstract_param_reader.cpp:3


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