Skip to content
Snippets Groups Projects
Commit 053500d1 authored by KingMaZito's avatar KingMaZito
Browse files

trajetory for n robots

parent 97007c06
No related branches found
No related tags found
No related merge requests found
Expand all Show whitespace changes
Inline Side-by-side
Showing
with 458 additions and 102 deletions
.vscode/settings.json
+ 74
− 1
View file @ 053500d1
...@@ -2,5 +2,78 @@ ...@@ -2,5 +2,78 @@
"python.autoComplete.extraPaths": [ "python.autoComplete.extraPaths": [
"/home/matteo/ccf_sim_workspace/devel/lib/python3/dist-packages", "/home/matteo/ccf_sim_workspace/devel/lib/python3/dist-packages",
"/opt/ros/noetic/lib/python3/dist-packages" "/opt/ros/noetic/lib/python3/dist-packages"
] ],
"files.associations": {
"cctype": "cpp",
"clocale": "cpp",
"cmath": "cpp",
"csignal": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"any": "cpp",
"array": "cpp",
"atomic": "cpp",
"hash_map": "cpp",
"hash_set": "cpp",
"strstream": "cpp",
"bit": "cpp",
"*.tcc": "cpp",
"bitset": "cpp",
"chrono": "cpp",
"codecvt": "cpp",
"complex": "cpp",
"condition_variable": "cpp",
"cstdint": "cpp",
"deque": "cpp",
"forward_list": "cpp",
"list": "cpp",
"map": "cpp",
"set": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"functional": "cpp",
"iterator": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"numeric": "cpp",
"optional": "cpp",
"random": "cpp",
"ratio": "cpp",
"regex": "cpp",
"string": "cpp",
"string_view": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp",
"fstream": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"mutex": "cpp",
"new": "cpp",
"ostream": "cpp",
"shared_mutex": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"thread": "cpp",
"cfenv": "cpp",
"cinttypes": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"variant": "cpp"
}
} }
\ No newline at end of file
CMakeLists.txt
+ 1
− 0
View file @ 053500d1
...@@ -81,6 +81,7 @@ src/reader/abstract_param_reader.cpp ...@@ -81,6 +81,7 @@ src/reader/abstract_param_reader.cpp
src/reader/robot_reader.cpp src/reader/robot_reader.cpp
src/reader/wing_reader.cpp src/reader/wing_reader.cpp
src/reader/cuboid_reader.cpp src/reader/cuboid_reader.cpp
src/reader/job_reader.cpp
......
include/impl/moveit_mediator.h
+ 10
− 0
View file @ 053500d1
...@@ -45,6 +45,9 @@ ...@@ -45,6 +45,9 @@
#include <moveit/planning_scene_monitor/current_state_monitor.h> #include <moveit/planning_scene_monitor/current_state_monitor.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h> #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/moveit_cpp/moveit_cpp.h> #include <moveit/moveit_cpp/moveit_cpp.h>
#include "reader/job_reader.h"
#include "reader/cuboid_reader.h"
#include <stdint.h> #include <stdint.h>
...@@ -67,7 +70,11 @@ class Moveit_mediator : public Abstract_mediator{ ...@@ -67,7 +70,11 @@ class Moveit_mediator : public Abstract_mediator{
std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_; std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_; std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;
std::unique_ptr<Job_reader> job_reader_;
std::unique_ptr<Cuboid_reader> cuboid_reader_;
std::map<std::string, std::vector<uint8_t>> acm_; std::map<std::string, std::vector<uint8_t>> acm_;
std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> tasks_;
...@@ -90,11 +97,14 @@ class Moveit_mediator : public Abstract_mediator{ ...@@ -90,11 +97,14 @@ class Moveit_mediator : public Abstract_mediator{
void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr); void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
void merge_acm(moveit_msgs::PlanningScene& in); void merge_acm(moveit_msgs::PlanningScene& in);
void task_planner();
void publish_tables(); void publish_tables();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps); void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps);
moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;}; inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;};
inline void set_tasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {tasks_ = tasks;};
}; };
......
include/reader/job_reader.h
+ 5
− 3
View file @ 053500d1
...@@ -4,21 +4,23 @@ ...@@ -4,21 +4,23 @@
#include "ros/ros.h" #include "ros/ros.h"
#include <ros/package.h> #include <ros/package.h>
#include <xmlrpcpp/XmlRpc.h> #include <xmlrpcpp/XmlRpc.h>
#include "boost/circular_buffer.hpp"
#include "reader/abstract_param_reader.h" #include "reader/abstract_param_reader.h"
class Job_reader : public Abstract_param_reader{ class Job_reader : public Abstract_param_reader{
protected: protected:
std::map<std::string, std::vector<job_data>> job_data_; boost::circular_buffer<std::pair<std::string, job_data>> job_data_;
public: public:
Job_reader(std::shared_ptr<ros::NodeHandle> const& d) Job_reader(std::shared_ptr<ros::NodeHandle> const& d)
: Abstract_param_reader(d) : Abstract_param_reader(d)
{read();} {read();}
inline void set_job_data(std::map<std::string, std::vector<job_data>>& robot_data) {job_data_ = robot_data;} inline void set_job_data(boost::circular_buffer<std::pair<std::string, job_data>>& robot_data) {job_data_ = robot_data;}
inline std::map<std::string, std::vector<job_data>>& robot_data() {return job_data_;} inline boost::circular_buffer<std::pair<std::string, job_data>>& robot_data() {return job_data_;}
void read() override; void read() override;
}; };
......
jobs/dummy.yaml
+ 48
− 48
View file @ 053500d1
{ 'task': { 'tasks':
{'groups' : [ {'groups' : [
{ 'name': 'panda_arm1', 'jobs':[[ { 'name': 'panda_arm1', 'jobs':[[
{ 'pos': { 'x': -0.300000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': -0.300000 ,'y': -0.700000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.300000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': -0.300000 ,'y': -0.600000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.200000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': -0.200000 ,'y': -0.700000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.200000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': -0.200000 ,'y': -0.600000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': -0.700000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': -0.600000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': -0.300000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': -0.200000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': -0.100000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.200000 ,'y': -0.300000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.200000 ,'y': -0.200000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.200000 ,'y': -0.100000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.300000 ,'y': -0.300000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.300000 ,'y': -0.200000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.300000 ,'y': -0.100000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': 0.100000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': 0.200000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': 0.300000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.200000 ,'y': 0.100000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.200000 ,'y': 0.200000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.200000 ,'y': 0.300000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.300000 ,'y': 0.100000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.300000 ,'y': 0.200000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } } { 'pos': { 'x': 0.300000 ,'y': 0.300000,'z': 0.935500 }, 'orientation': { 'w': 1 } }
]]}, ]]},
{ 'name': 'panda_arm2', 'jobs':[[ { 'name': 'panda_arm2', 'jobs':[[
{ 'pos': { 'x': 0.100000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': 1.110000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': 1.210000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 1.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.200000 ,'y': 1.010000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.200000 ,'y': 1.110000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.200000 ,'y': 1.210000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 1.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.300000 ,'y': 1.010000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.300000 ,'y': 1.110000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.300000 ,'y': 1.210000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': 1.410000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': 1.510000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': 1.610000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.200000 ,'y': 1.410000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.200000 ,'y': 1.510000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.200000 ,'y': 1.610000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.300000 ,'y': 1.410000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.300000 ,'y': 1.510000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.300000 ,'y': 1.610000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.300000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': -0.300000 ,'y': 1.910000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.300000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': -0.300000 ,'y': 2.010000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.200000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': -0.200000 ,'y': 1.910000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.200000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': -0.200000 ,'y': 2.010000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, { 'pos': { 'x': 0.100000 ,'y': 1.910000,'z': 0.935500 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } } { 'pos': { 'x': 0.100000 ,'y': 2.010000,'z': 0.935500 }, 'orientation': { 'w': 1 } }
]]} ]]}
] ]
} }
......
launch/cell_routine.launch
+ 1
− 1
View file @ 053500d1
...@@ -9,7 +9,7 @@ ...@@ -9,7 +9,7 @@
<rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" /> <rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/> <rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
<rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/> <rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
<!-- <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/> --> <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/>
......
launch/mtc_auerswald.launch
+ 1
− 0
View file @ 053500d1
...@@ -66,6 +66,7 @@ ...@@ -66,6 +66,7 @@
<rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/> <rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
<rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/> <rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/> <rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/grasp_map_config.yaml"/> <rosparam command="load" file="$(find gb_grasp)/config/grasp_map_config.yaml"/>
......
results/dummy/-562289182.yaml
+ 4
− 8
View file @ 053500d1
...@@ -22,11 +22,7 @@ ...@@ -22,11 +22,7 @@
{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': .1, 'y': -.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B { 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': -0.3, 'y': -0.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': .2, 'y': .3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B { 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.1, 'y': 1.11, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } } # target B
{ 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # impossible for the target B
{ 'id': 'green1', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
{ 'id': 'green2', 'type': 'BOX', 'pos': { 'x': .1, 'y': 1.91, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
{ 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
{ 'id': 'red2', 'type': 'BOX', 'pos': { 'x': .3, 'y': 1.41, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } }
]} ]}
src/impl/moveit_mediator.cpp
+ 293
− 37
View file @ 053500d1
This diff is collapsed.
src/reader/job_reader.cpp
+ 21
− 4
View file @ 053500d1
#include "reader/job_reader.h" #include "reader/job_reader.h"
#include <queue>
#include "boost/circular_buffer.hpp"
void Job_reader::read(){ void Job_reader::read(){
ROS_INFO("--- JOB_READER ---"); ROS_INFO("--- JOB_READER ---");
XmlRpc::XmlRpcValue resources; XmlRpc::XmlRpcValue resources;
nh_->getParam("/task/groups", resources); nh_->getParam("/tasks/groups", resources);
int count =0;
for (int i = 0; i < resources.size(); i++){
count+= resources[i]["jobs"].size();
}
boost::circular_buffer<std::pair<std::string, job_data>> cb_jd(count);
for (int i = 0; i < resources.size(); i++){ for (int i = 0; i < resources.size(); i++){
std::string robot = resources[i]["name"]; std::string robot = resources[i]["name"];
//nh_->getParam("/tasks/groups/" + robot, jobs); //nh_->getParam("/tasks/groups/" + robot, jobs);
std::vector<job_data> v_jd;
for (int j = 0; j < resources[i]["jobs"].size(); j++){ for (int j = 0; j < resources[i]["jobs"].size(); j++){
ROS_INFO("--- %s --- JOB_%i ---", robot.c_str(), j); ROS_INFO("--- %s --- JOB_%i ---", robot.c_str(), j);
job_data jd; job_data jd;
...@@ -31,8 +39,17 @@ void Job_reader::read(){ ...@@ -31,8 +39,17 @@ void Job_reader::read(){
ROS_INFO("=> Grab: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW()); ROS_INFO("=> Grab: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
jd.jobs_.push_back(tf2::Transform(rot, pos)); jd.jobs_.push_back(tf2::Transform(rot, pos));
} }
v_jd.push_back(jd); cb_jd.push_back(std::pair<std::string, job_data>(robot, jd));
}
}
set_job_data(cb_jd);
// validate
for(auto& s_jd : job_data_){
for(auto& pose : s_jd.second.jobs_) {
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());
} }
job_data_.insert(std::pair<std::string, std::vector<job_data>>(robot, v_jd));
} }
} }
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment