4 ROS_INFO(
"--- CUBOID_READER ---");
5 XmlRpc::XmlRpcValue resources;
6 nh_->getParam(
"/objects", resources);
9 for (
int i = 0; i < resources.size(); i++){
10 std::string
id = resources[i][
"id"];
11 if(resources[i][
"type"] ==
"BOX"){
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);
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);
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();
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());
49 if(resources[i][
"type"] ==
"BIN" || std::regex_match(
id, match, std::regex(
"obstacle([0-9]+)"))){
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);
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);
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();
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());