job_reader.cpp
Go to the documentation of this file.
1 #include "reader/job_reader.h"
2 #include <queue>
3 #include "boost/circular_buffer.hpp"
4 
6  ROS_INFO("--- JOB_READER ---");
7  XmlRpc::XmlRpcValue resources;
8  nh_->getParam("/tasks/groups", resources);
9 
10  int count =0;
11  for (int i = 0; i < resources.size(); i++){
12  count+= resources[i]["jobs"].size();
13  }
14 
15  boost::circular_buffer<std::pair<std::string, job_data>> cb_jd(count);
16 
17  for (int i = 0; i < resources.size(); i++){
18  std::string robot = resources[i]["name"];
19  //nh_->getParam("/tasks/groups/" + robot, jobs);
20  for (int j = 0; j < resources[i]["jobs"].size(); j++){
21  ROS_INFO("--- %s --- JOB_%i ---", robot.c_str(), j);
22  job_data jd;
23  for(int k = 0; k < resources[i]["jobs"][j].size(); k++){
24  XmlRpc::XmlRpcValue job_data = resources[i]["jobs"][j][k];
25  tf2::Vector3 pos;
26  tf2::Quaternion rot;
27 
28 
29  (job_data["pos"].hasMember("x")) ? pos.setX(floatOf(job_data["pos"]["x"])) :pos.setX(0);
30  (job_data["pos"].hasMember("y")) ? pos.setY(floatOf(job_data["pos"]["y"])) :pos.setY(0);
31  (job_data["pos"].hasMember("z")) ? pos.setZ(floatOf(job_data["pos"]["z"])) :pos.setZ(0);
32  (job_data["orientation"].hasMember("x")) ? rot.setX(floatOf(job_data["orientation"]["x"])) :rot.setX(0);
33  (job_data["orientation"].hasMember("y")) ? rot.setY(floatOf(job_data["orientation"]["y"])) :rot.setY(0);
34  (job_data["orientation"].hasMember("z")) ? rot.setZ(floatOf(job_data["orientation"]["z"])) :rot.setZ(0);
35  (job_data["orientation"].hasMember("w")) ? rot.setW(floatOf(job_data["orientation"]["w"])) :rot.setW(0);
36 
37 
38  ROS_INFO("=> Grab: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
39  ROS_INFO("=> Grab: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
40  jd.jobs_.push_back(tf2::Transform(rot, pos));
41  }
42  cb_jd.push_back(std::pair<std::string, job_data>(robot, jd));
43  }
44  }
45 
46  setJobData(cb_jd);
47 
48 
49  // validate
50  for(auto& s_jd : job_data_){
51  for(auto& pose : s_jd.second.jobs_) {
52  ROS_INFO("%s %f %f %f %f %f %f %f", s_jd.first.c_str(), pose.getOrigin().getX(), pose.getOrigin().getY(), pose.getOrigin().getZ(), pose.getRotation().getX(), pose.getRotation().getY(), pose.getRotation().getZ(), pose.getRotation().getW());
53  }
54  }
55 }
JobReader::read
void read() override
read implementation
Definition: job_reader.cpp:5
job_reader.h
JobReader::job_data_
boost::circular_buffer< std::pair< std::string, job_data > > job_data_
FIFO job information.
Definition: job_reader.h:18
job_data::jobs_
std::vector< tf2::Transform > jobs_
Definition: abstract_param_reader.h:22
AbstractParamReader::nh_
std::shared_ptr< ros::NodeHandle > nh_
ROS Nodehandle.
Definition: abstract_param_reader.h:31
JobReader::setJobData
void setJobData(boost::circular_buffer< std::pair< std::string, job_data >> &robot_data)
Set Job_data.
Definition: job_reader.h:35
job_data
Job data.
Definition: abstract_param_reader.h:21
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