3 #include "boost/circular_buffer.hpp"
6 ROS_INFO(
"--- JOB_READER ---");
7 XmlRpc::XmlRpcValue resources;
8 nh_->getParam(
"/tasks/groups", resources);
11 for (
int i = 0; i < resources.size(); i++){
12 count+= resources[i][
"jobs"].size();
15 boost::circular_buffer<std::pair<std::string, job_data>> cb_jd(count);
17 for (
int i = 0; i < resources.size(); i++){
18 std::string robot = resources[i][
"name"];
20 for (
int j = 0; j < resources[i][
"jobs"].size(); j++){
21 ROS_INFO(
"--- %s --- JOB_%i ---", robot.c_str(), j);
23 for(
int k = 0; k < resources[i][
"jobs"][j].size(); k++){
24 XmlRpc::XmlRpcValue
job_data = resources[i][
"jobs"][j][k];
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));
42 cb_jd.push_back(std::pair<std::string, job_data>(robot, jd));
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());