4 ROS_INFO(
"--- ROBOT_READER ---");
5 XmlRpc::XmlRpcValue resources;
6 nh_->getParam(
"/objects", resources);
7 std::regex rx(
"table([0-9]+)_table_top");
10 for (
int i = 0; i < resources.size(); i++){
11 std::string str = resources[i][
"id"];
12 if(std::regex_match(str, match, rx)){
13 ROS_INFO(
"=> Robot: description found, loading...");
15 ss <<
"panda_arm" << match[1];
20 (resources[i][
"size"].hasMember(
"length")) ? size.setX(
floatOf(resources[i][
"size"][
"length"])) :size.setX(0);
21 (resources[i][
"size"].hasMember(
"width")) ? size.setY(
floatOf(resources[i][
"size"][
"width"])) :size.setY(0);
22 (resources[i][
"size"].hasMember(
"height")) ? size.setZ(
floatOf(resources[i][
"size"][
"height"])) :size.setZ(0);
24 (resources[i][
"pos"].hasMember(
"x")) ? pos.setX(
floatOf(resources[i][
"pos"][
"x"])) :pos.setX(0);
25 (resources[i][
"pos"].hasMember(
"y")) ? pos.setY(
floatOf(resources[i][
"pos"][
"y"])) :pos.setY(0);
26 (resources[i][
"pos"].hasMember(
"z")) ? pos.setZ((
floatOf(resources[i][
"pos"][
"z"]) + size.getZ()/2) /2) :pos.setZ(0);
27 (resources[i][
"orientation"].hasMember(
"x")) ? rot.setX(
floatOf(resources[i][
"orientation"][
"x"])) :rot.setX(0);
28 (resources[i][
"orientation"].hasMember(
"y")) ? rot.setY(
floatOf(resources[i][
"orientation"][
"y"])) :rot.setY(0);
29 (resources[i][
"orientation"].hasMember(
"z")) ? rot.setZ(
floatOf(resources[i][
"orientation"][
"z"])) :rot.setZ(0);
30 (resources[i][
"orientation"].hasMember(
"w")) ? rot.setW(
floatOf(resources[i][
"orientation"][
"w"])) :rot.setW(0);
32 ROS_INFO(
"--- Robot: %s ---", ss.str().c_str());
33 ROS_INFO(
"=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
34 ROS_INFO(
"=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
35 ROS_INFO(
"=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
37 robot_data_.push_back({ss.str().c_str(), tf2::Transform(rot,pos), size});