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


multi_cell_builder
Author(s): MA
autogenerated on Thu Jan 12 2023 23:45:43