robot_reader.cpp
Go to the documentation of this file.
1 #include "reader/robot_reader.h"
2 
4  ROS_INFO("--- ROBOT_READER ---");
5  XmlRpc::XmlRpcValue resources;
6  nh_->getParam("/objects", resources);
7  std::regex rx("table([0-9]+)_table_top");
8  std::smatch match;
9 
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...");
14  std::stringstream ss;
15  ss << "panda_arm" << match[1];
16  tf2::Vector3 pos;
17  tf2::Vector3 size;
18  tf2::Quaternion rot;
19 
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);
23 
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);
31 
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());
36 
37  robot_data_.push_back({ss.str().c_str(), tf2::Transform(rot,pos), size});
38  }
39  }
40 }
RobotReader::read
void read() override
read implementatin
Definition: robot_reader.cpp:3
AbstractParamReader::nh_
std::shared_ptr< ros::NodeHandle > nh_
ROS Nodehandle.
Definition: abstract_param_reader.h:31
robot_reader.h
RobotReader::robot_data_
std::vector< object_data > robot_data_
Map of object_data to robot.
Definition: robot_reader.h:17
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