diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000000000000000000000000000000000000..473dc3fcfc36899ca2e29b524ae01585630bc7de --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,39 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/home/matteo/ccf_sim_workspace/devel/include/**", + "/opt/ros/noetic/include/**", + "/home/matteo/ccf_sim_workspace/src/BehaviorTree.CPP/include/**", + "/home/matteo/ccf_sim_workspace/src/ccf/include/**", + "/home/matteo/ccf_sim_workspace/src/ccf_immersive_sorting/include/**", + "/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_grasp_plugin/include/**", + "/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_state_plugins/include/**", + "/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_test_tools/include/**", + "/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_version_helpers/include/**", + "/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_world_plugin_loader/include/**", + "/home/matteo/ccf_sim_workspace/src/gb-grasp-auerswald/include/**", + "/home/matteo/ccf_sim_workspace/src/moveit-grasps-reduced/include/**", + "/home/matteo/ccf_sim_workspace/src/moveit_task_constructor/core/include/**", + "/home/matteo/ccf_sim_workspace/src/moveit_task_constructor/demo/include/**", + "/home/matteo/ccf_sim_workspace/src/multi_cell_builder/include/**", + "/home/matteo/ccf_sim_workspace/src/general_message_packages/object_msgs_tools/include/**", + "/home/matteo/ccf_sim_workspace/src/panda_grasping/include/**", + "/home/matteo/ccf_sim_workspace/src/panda_util/include/**", + "/home/matteo/ccf_sim_workspace/src/moveit_task_constructor/rviz_marker_tools/include/**", + "/home/matteo/ccf_sim_workspace/src/simulation_util/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000000000000000000000000000000000000..6ffb51377a2b5bbeb48424690bec9a959c6f4746 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "python.autoComplete.extraPaths": [ + "/home/matteo/ccf_sim_workspace/devel/lib/python3/dist-packages", + "/opt/ros/noetic/lib/python3/dist-packages" + ] +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 04ba4b8b98cce952f01cfbc30a483dac8c5a9e27..c5ab33ec691b43a7cb9882ab645f73da0015f928 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -103,6 +103,8 @@ src/reader/abstract_param_reader.cpp src/reader/robot_reader.cpp src/reader/wing_reader.cpp src/reader/cuboid_reader.cpp +src/reader/job_reader.cpp + ) add_executable(grasp_cell_routine src/grasp_cell_routine.cpp @@ -122,6 +124,8 @@ src/reader/abstract_param_reader.cpp src/reader/robot_reader.cpp src/reader/wing_reader.cpp src/reader/cuboid_reader.cpp +src/reader/job_reader.cpp + ) @@ -129,6 +133,8 @@ add_executable(mtc2taskspace src/mtc2taskspace.cpp src/reader/abstract_param_reader.cpp src/reader/task_space_reader.cpp src/reader/robot_reader.cpp +src/reader/job_reader.cpp + ) diff --git a/include/impl/moveit_grasp_mediator.h b/include/impl/moveit_grasp_mediator.h index 09f727b3a24f522bea77f6a0d2beab8de8b1be46..ae363e20bad13f69d17921107e6a8376148e946c 100644 --- a/include/impl/moveit_grasp_mediator.h +++ b/include/impl/moveit_grasp_mediator.h @@ -15,6 +15,8 @@ #include "reader/abstract_param_reader.h" #include "reader/robot_reader.h" #include "reader/cuboid_reader.h" +#include "reader/job_reader.h" + @@ -28,9 +30,11 @@ class Moveit_grasp_mediator : public Moveit_mediator{ // Readers - std::unique_ptr<Abstract_param_reader> robot_reader_; + std::unique_ptr<Robot_reader> robot_reader_; std::unique_ptr<Abstract_param_reader> wing_reader_; std::unique_ptr<Abstract_param_reader> cuboid_reader_; + std::unique_ptr<Job_reader> job_reader_; + public: Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh); diff --git a/include/reader/abstract_param_reader.h b/include/reader/abstract_param_reader.h index 5cbf34d499d8c35e096217029c2df4ed614de4bf..1191a7a29eaeaa5bb1e06a640fbc98282953859a 100644 --- a/include/reader/abstract_param_reader.h +++ b/include/reader/abstract_param_reader.h @@ -17,7 +17,6 @@ struct object_data { }; struct job_data { - std::string name_; std::vector<tf2::Transform> jobs_; }; diff --git a/include/reader/job_reader.h b/include/reader/job_reader.h index 3e75feef365c0f769c3928832a10417959d9178c..fedfd87d23b75243437a5287bbaf5b59f31fc328 100644 --- a/include/reader/job_reader.h +++ b/include/reader/job_reader.h @@ -10,17 +10,15 @@ class Job_reader : public Abstract_param_reader{ protected: - std::vector<job_data> job_data_; - std::string name_; + std::map<std::string, std::vector<job_data>> job_data_; public: - Job_reader(std::shared_ptr<ros::NodeHandle> const& d, std::string const& name) + Job_reader(std::shared_ptr<ros::NodeHandle> const& d) : Abstract_param_reader(d) - , name_(name) {read();} - inline void set_job_data(std::vector<job_data>& robot_data) {job_data_ = robot_data;} - inline std::vector<job_data> robot_data() {return job_data_;} + inline void set_job_data(std::map<std::string, std::vector<job_data>>& robot_data) {job_data_ = robot_data;} + inline std::map<std::string, std::vector<job_data>>& robot_data() {return job_data_;} void read() override; }; diff --git a/jobs/dummy.yaml b/jobs/dummy.yaml new file mode 100644 index 0000000000000000000000000000000000000000..7a5ef0fb28f1d2554570fb5ddc2d4c3111ee5110 --- /dev/null +++ b/jobs/dummy.yaml @@ -0,0 +1,56 @@ +{ 'task': + {'groups' : [ + { 'name': 'panda_arm1', 'jobs':[[ + { 'pos': { 'x': -0.300000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': -0.300000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': -0.200000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': -0.200000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.200000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.200000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.200000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.300000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.300000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.300000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.200000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.200000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.200000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.300000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.300000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.300000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } } + ]]}, + { 'name': 'panda_arm2', 'jobs':[[ + { 'pos': { 'x': 0.100000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.200000 ,'y': 1.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.200000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.200000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.300000 ,'y': 1.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.300000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.300000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.200000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.200000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.200000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.300000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.300000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.300000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': -0.300000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': -0.300000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': -0.200000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': -0.200000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } }, + { 'pos': { 'x': 0.100000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } } + ]]} + ] + } +} diff --git a/launch/cell_routine.launch b/launch/cell_routine.launch index 6f14f4bf36fb17af310af801652b09ba108dd5a7..7a516c1d0823ab2d3e91b40b47cf344c1fb01a8f 100644 --- a/launch/cell_routine.launch +++ b/launch/cell_routine.launch @@ -1,5 +1,7 @@ <launch> <arg name="result" default="dummy/-562289182.yaml" /> + <arg name="jobs" default="jobs/dummy.yaml" /> + <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> --> <!-- this is to change--> @@ -7,6 +9,8 @@ <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)/descriptions/dummy2.yaml"/> + <!-- <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/> --> + <rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam> diff --git a/launch/mtc2taskspace.launch b/launch/mtc2taskspace.launch new file mode 100644 index 0000000000000000000000000000000000000000..f3c8dd0759a5b150d7f21bd551e0adcc8fbaea37 --- /dev/null +++ b/launch/mtc2taskspace.launch @@ -0,0 +1,8 @@ +<launch> + <arg name="resource" default="jobs/dummy.yaml" /> + + <rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam> + + <rosparam command="load" file="$(find multi_cell_builder)/$(arg resource)"/> + <node pkg="multi_cell_builder" type="mtc2taskspace" name="mtc2taskspace" output="screen" required="true"/> +</launch> diff --git a/src/impl/moveit_grasp_mediator.cpp b/src/impl/moveit_grasp_mediator.cpp index f02e0ef1f0215681c23d7b3cbf07e5ee4e49acb8..c5decb4b21bf88cbf9b8274eae8978b8d7833b9c 100644 --- a/src/impl/moveit_grasp_mediator.cpp +++ b/src/impl/moveit_grasp_mediator.cpp @@ -527,7 +527,7 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf , robot_reader_(std::make_unique<Robot_reader>(nh)) , wing_reader_(std::make_unique<Wing_reader>(nh)) , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) { - + // , job_reader_(std::make_unique<Job_reader>(nh)) { visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>( robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_); visual_tools_->loadMarkerPub(); @@ -551,7 +551,7 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf planning_pipeline_ = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, *nh_, "planning_plugin", "request_adapter"); voxel_manager_ = std::make_shared<VoxelManager>(*nh_, grasp_ns_.c_str() ,visual_tools_,planning_scene_monitor_); - auto rd = static_cast<Robot_reader*>(robot_reader_.get())->robot_data(); + auto rd = robot_reader_->robot_data(); for (int i = 0; i < rd.size() ;i++) connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_)); auto wd = static_cast<Wing_reader*>(wing_reader_.get())->wing_data(); diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp index 43d9901150650899154103e7b57c1655a962c6b0..9133caf89dd6934770322765098046bb6e46598a 100644 --- a/src/impl/moveit_mediator.cpp +++ b/src/impl/moveit_mediator.cpp @@ -244,6 +244,7 @@ void Moveit_mediator::setup_task(){ for(auto& a: acm_) a.second.resize(acm_.size(), 0); + while (!tasks[0].empty() && !tasks[1].empty()){ for (int i =0; i < std::max(tasks[0].front().solution.sub_trajectory.size(), tasks[1].front().solution.sub_trajectory.size()); i++){ moveit_msgs::RobotTrajectory* t1 = nullptr; @@ -253,166 +254,16 @@ void Moveit_mediator::setup_task(){ moveit_msgs::PlanningScene ps_m; std::vector<moveit_msgs::PlanningScene::_allowed_collision_matrix_type> acm_m; - for(auto entry: acm_) ROS_INFO("%s", entry.first.c_str()); + //for(auto entry: acm_) ROS_INFO("%s", entry.first.c_str()); if (i < tasks[0].front().solution.sub_trajectory.size()){ t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory; ps1 = &tasks[0].front().solution.sub_trajectory[i].scene_diff; - - ROS_INFO("[Robot 1] acm"); - ROS_INFO("default entry names"); - for (auto den : ps1->allowed_collision_matrix.default_entry_names){ - ROS_INFO("%s", den.c_str()); - } - ROS_INFO("default entry values"); - for (auto den : ps1->allowed_collision_matrix.default_entry_values){ - ROS_INFO("%i", den); - } - ROS_INFO("entry names"); - for (auto den : ps1->allowed_collision_matrix.entry_names){ - ROS_INFO("%s", den.c_str()); - ROS_INFO("count %i", ps1->allowed_collision_matrix.entry_names.size()); - } - ROS_INFO("entry values"); - for (auto den : ps1->allowed_collision_matrix.entry_values){ - for (auto ene : den.enabled) ROS_INFO("%i", ene); - ROS_INFO("count %i", den.enabled.size()); - - } - ROS_INFO("[Robot 1] fft"); - for (auto ft : ps1->fixed_frame_transforms){ - ROS_INFO("child frame id: %s, header id: %s, seq %i, time", ft.child_frame_id.c_str(), ft.header.frame_id.c_str(), ft.header.seq); - } - ROS_INFO("[Robot 1] is_diff"); - ROS_INFO("is_diff: %i", ps1->is_diff); - ROS_INFO("[Robot 1] link padding"); - for (auto lp : ps1->link_padding){ - ROS_INFO("link padding name: %s, padding %f", lp.link_name.c_str(), lp.padding); - } - ROS_INFO("[Robot 1] link scale"); - for (auto ls : ps1->link_scale){ - ROS_INFO("link scale name: %s, padding %f", ls.link_name.c_str(), ls.scale); - } - ROS_INFO("[Robot 1] name"); - ROS_INFO("name: %s", ps1->name.c_str()); - ROS_INFO("[Robot 1] object color"); - for (auto oc : ps1->object_colors){ - ROS_INFO("object color %f %f %f %f, id %s", oc.color.r, oc.color.g, oc.color.b, oc.color.a, oc.id.c_str()); - } - ROS_INFO("[Robot 1] rmn"); - ROS_INFO("rmn %s", ps1->robot_model_name.c_str()); - ROS_INFO("[Robot 1] rs"); - for (auto aco : ps1->robot_state.attached_collision_objects){ - ROS_INFO("aco dp header frame id: %s, seq %i, time", aco.detach_posture.header.frame_id.c_str(), aco.detach_posture.header.seq); - for (auto jn : aco.detach_posture.joint_names){ - ROS_INFO("aco dp joint name: %s", jn.c_str()); - } - ROS_INFO("aco dp Points"); - for (auto p : aco.detach_posture.points){ - for (auto pa : p.accelerations) ROS_INFO("aco dp points acc: %f", pa); - for (auto pa : p.effort) ROS_INFO("aco dp points effort: %f", pa); - ROS_INFO("aco dp points time from start"); - for (auto pa : p.velocities) ROS_INFO("aco dp points vel: %f", pa); - for (auto pa : p.positions) ROS_INFO("aco dp points tra: %f", pa); - } - ROS_INFO("aco link names %s", aco.link_name.c_str()); - ROS_INFO("aco collis obj id %s", aco.object.id.c_str()); - for (auto p : aco.touch_links) ROS_INFO("aco touch link %s", p.c_str()); - ROS_INFO("aco collis obj weight %f", aco.weight); - } - - for (auto d : ps1->robot_state.joint_state.position) ROS_INFO("js position %f ", d); - for (auto d : ps1->robot_state.joint_state.name) ROS_INFO(" js name %s", d.c_str()); - for (auto d : ps1->robot_state.joint_state.velocity) ROS_INFO(" js vel %f", d); - for (auto d : ps1->robot_state.joint_state.effort) ROS_INFO(" js e %f", d); - ROS_INFO ("%s, %i", ps1->robot_state.joint_state.header.frame_id.c_str(), ps1->robot_state.joint_state.header.seq); - - for (auto p : ps1->world.collision_objects) ROS_INFO("world co id %s", p.id); - ROS_INFO("world om origin %f %f %f, %f %f %f %f", ps1->world.octomap.origin.position.x, ps1->world.octomap.origin.position.y, ps1->world.octomap.origin.position.z, ps1->world.octomap.origin.orientation.x, ps1->world.octomap.origin.orientation.y, ps1->world.octomap.origin.orientation.z, ps1->world.octomap.origin.orientation.w); - ROS_INFO("world om header time, id %s, seq %i", ps1->world.octomap.header.frame_id.c_str(), ps1->world.octomap.header.seq); - ROS_INFO("world om Data"); - } if (i < tasks[1].front().solution.sub_trajectory.size()){ t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory; ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff; - - - ROS_INFO("[Robot 2] acm"); - ROS_INFO("default entry names"); - for (auto den : ps2->allowed_collision_matrix.default_entry_names){ - ROS_INFO("%s", den.c_str()); - } - ROS_INFO("default entry values"); - for (auto den : ps2->allowed_collision_matrix.default_entry_values){ - ROS_INFO("%i", den); - } - ROS_INFO("entry names"); - for (auto den : ps2->allowed_collision_matrix.entry_names){ - ROS_INFO("%s", den.c_str()); - ROS_INFO("count %i", ps2->allowed_collision_matrix.entry_names.size()); - - } - ROS_INFO("entry values"); - for (auto den : ps2->allowed_collision_matrix.entry_values){ - for (auto ene : den.enabled) ROS_INFO("%i", ene); - ROS_INFO("count %i", den.enabled.size()); - } - ROS_INFO("[Robot 2] fft"); - for (auto ft : ps2->fixed_frame_transforms){ - ROS_INFO("child frame id: %s, header id: %s, seq %i, time", ft.child_frame_id.c_str(), ft.header.frame_id.c_str(), ft.header.seq); - } - ROS_INFO("[Robot 2] is_diff"); - ROS_INFO("is_diff: %i", ps2->is_diff); - - ROS_INFO("[Robot 2] link padding"); - for (auto lp : ps2->link_padding){ - ROS_INFO("link padding name: %s, padding %f", lp.link_name.c_str(), lp.padding); - } - ROS_INFO("[Robot 2] link scale"); - for (auto ls : ps2->link_scale){ - ROS_INFO("link scale name: %s, padding %f", ls.link_name.c_str(), ls.scale); - } - ROS_INFO("[Robot 2] name"); - ROS_INFO("name: %s", ps2->name.c_str()); - ROS_INFO("[Robot 2] object color"); - for (auto oc : ps2->object_colors){ - ROS_INFO("object color %f %f %f %f, id %s", oc.color.r, oc.color.g, oc.color.b, oc.color.a, oc.id.c_str()); - } - ROS_INFO("[Robot 2] rmn"); - ROS_INFO("rmn %s", ps2->robot_model_name.c_str()); - ROS_INFO("[Robot 2] rs"); - for (auto aco : ps2->robot_state.attached_collision_objects){ - ROS_INFO("aco dp header frame id: %s, seq %i, time", aco.detach_posture.header.frame_id.c_str(), aco.detach_posture.header.seq); - for (auto jn : aco.detach_posture.joint_names){ - ROS_INFO("aco dp joint name: %s", jn.c_str()); - } - ROS_INFO("aco dp Points"); - for (auto p : aco.detach_posture.points){ - for (auto pa : p.accelerations) ROS_INFO("aco dp points acc: %f", pa); - for (auto pa : p.effort) ROS_INFO("aco dp points effort: %f", pa); - ROS_INFO("aco dp points time from start"); - for (auto pa : p.velocities) ROS_INFO("aco dp points vel: %f", pa); - for (auto pa : p.positions) ROS_INFO("aco dp points tra: %f", pa); - } - ROS_INFO("aco link names %s", aco.link_name.c_str()); - ROS_INFO("aco collis obj id %s", aco.object.id.c_str()); - for (auto p : aco.touch_links) ROS_INFO("aco touch link %s", p.c_str()); - ROS_INFO("aco collis obj weight %f", aco.weight); - } - for (auto d : ps2->robot_state.joint_state.position) ROS_INFO("js position %f ", d); - for (auto d : ps2->robot_state.joint_state.name) ROS_INFO(" js name %s", d.c_str()); - for (auto d : ps2->robot_state.joint_state.velocity) ROS_INFO(" js vel %f", d); - for (auto d : ps2->robot_state.joint_state.effort) ROS_INFO(" js e %f", d); - ROS_INFO ("%s, %i", ps2->robot_state.joint_state.header.frame_id.c_str(), ps2->robot_state.joint_state.header.seq); - - - for (auto p : ps2->world.collision_objects) ROS_INFO("world co id %s", p.id.c_str()); - ROS_INFO("world om origin %f %f %f, %f %f %f %f", ps2->world.octomap.origin.position.x, ps2->world.octomap.origin.position.y, ps2->world.octomap.origin.position.z, ps2->world.octomap.origin.orientation.x, ps2->world.octomap.origin.orientation.y, ps2->world.octomap.origin.orientation.z, ps2->world.octomap.origin.orientation.w); - ROS_INFO("world om header time, id %s, seq %i", ps2->world.octomap.header.frame_id.c_str(), ps2->world.octomap.header.seq); - ROS_INFO("world om Data"); - } std::vector<std::thread> th; @@ -428,53 +279,125 @@ void Moveit_mediator::setup_task(){ // build panda link regex std::stringstream ss; - ss << "panda_" << match[1] << "_*"; - std::regex rx_panda_links(ss.str().c_str()); - std::regex rx_box("bottle*"); - std::regex rx_table("table*"); + ss << "panda_" << match[1] << "_.*"; + std::regex rx_panda_links(ss.str()); + std::regex rx_box("bottle.*"); + std::regex rx_table("table.*"); - ROS_INFO("start merging"); // Iterate task collsion matrix for(int j = 0; j < ps1->allowed_collision_matrix.entry_names.size(); j++ ){ - // look for relevant fields - ROS_INFO("compare %s and %s", mr->map().at("base").c_str(), ps1->allowed_collision_matrix.entry_names[j].c_str()); - if( std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_box) || - std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_panda_links) || - std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_table) || - mr->map().at("base") == ps1->allowed_collision_matrix.entry_names[j]){ - // Iterate over values - ROS_INFO("true"); + if( mr->map().at("base") == ps1->allowed_collision_matrix.entry_names[j]){ + /* In case an entry matches an robot spezific link*/ + ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j); for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){ - // filter for irrelevant links of other robots, this would be _link and base_ pattern, which are not relevant - ROS_INFO("start filtering"); - - if( std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links) || - std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box) || - std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table) || - mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){ - - std::string relevant_field; - - if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)) relevant_field = match[0].str(); - if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)) relevant_field = match[0].str(); - if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)) relevant_field = match[0].str(); - if(mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]) relevant_field = mr->map().at("base").c_str(); - - // => Update acm_ - // calculate the distance in acm for name in in acm message ('cause they may differ, you know) - // totally not stolen from SO - ROS_INFO("calculate distance"); - int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k])); - - // better filter with regex - // set acm value with respect to the position in acm - ROS_INFO("acm überschreiben mit distnace %i", distance); - ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps1->allowed_collision_matrix.entry_names[k].c_str()); - - for(auto a : acm_.at(mr->map().at("base"))) ROS_INFO("%i", a); - ROS_INFO("value %i", acm_.at(mr->map().at("base"))[distance]); - acm_.at(mr->map().at(relevant_field))[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; - ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps1->allowed_collision_matrix.entry_names[k].c_str()); + /* For that specific entry, loop over values*/ + int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k])); + + if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){ + ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){ + ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){ + ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){ + ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + } + } + + if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){ + /* In case an entry matches an robot spezific link*/ + ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j); + for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){ + /* For that specific entry, loop over values*/ + int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k])); + + if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){ + ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){ + ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){ + ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){ + ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + } + } + + if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_box)){ + /* In case an entry matches an robot spezific link*/ + ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j); + for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){ + /* For that specific entry, loop over values*/ + int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k])); + + if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){ + ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){ + ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){ + ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){ + ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + } + } + + if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_table)){ + /* In case an entry matches an robot spezific link*/ + ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j], j); + for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){ + /* For that specific entry, loop over values*/ + int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k])); + + if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){ + ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){ + ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){ + ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){ + ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str()); + acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; } } } @@ -485,59 +408,132 @@ void Moveit_mediator::setup_task(){ if (t2){ Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[1]); th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2)); + std::regex rx_panda("panda_arm([0-9]+)"); std::smatch match; std::regex_match(mr->name(), match, rx_panda); // build panda link regex std::stringstream ss; - ss << "panda_" << match[1] << "_*"; - std::regex rx_panda_links(ss.str().c_str()); - std::regex rx_box("bottle*"); - std::regex rx_table("table*"); + ss << "panda_" << match[1] << "_.*"; + std::regex rx_panda_links(ss.str()); + std::regex rx_box("bottle.*"); + std::regex rx_table("table.*"); - ROS_INFO("start merging"); // Iterate task collsion matrix for(int j = 0; j < ps2->allowed_collision_matrix.entry_names.size(); j++ ){ - // look for relevant fields - ROS_INFO("compare %s and %s", mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[j].c_str()); - if( std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box) || - std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_panda_links) || - std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_table) || - mr->map().at("base") == ps2->allowed_collision_matrix.entry_names[j]){ - // Iterate over values - ROS_INFO("true"); + if( mr->map().at("base") == ps2->allowed_collision_matrix.entry_names[j]){ + /* In case an entry matches an robot spezific link*/ + ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j); + for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){ + /* For that specific entry, loop over values*/ + int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k])); + + if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){ + ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){ + ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){ + ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){ + ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + } + } + + if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){ + /* In case an entry matches an robot spezific link*/ + ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j); + for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){ + /* For that specific entry, loop over values*/ + int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k])); + + if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){ + ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){ + ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){ + ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){ + ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + } + } + + if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box)){ + /* In case an entry matches an robot spezific link*/ + ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j); for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){ - // filter for irrelevant links of other robots, this would be _link and base_ pattern, which are not relevant - ROS_INFO("start filtering"); - - if( std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links) || - std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box) || - std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table) || - mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){ - - std::string relevant_field; - - if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)) relevant_field = match[0].str(); - if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)) relevant_field = match[0].str(); - if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)) relevant_field = match[0].str(); - if(mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]) relevant_field = mr->map().at("base").c_str(); - - // => Update acm_ - // calculate the distance in acm for name in in acm message ('cause they may differ, you know) - // totally not stolen from SO - ROS_INFO("calculate distance"); - int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k])); - - // better filter with regex - // set acm value with respect to the position in acm - ROS_INFO("acm überschreiben mit distnace %i", distance); - ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[k].c_str()); - - for(auto a : acm_.at(mr->map().at("base"))) ROS_INFO("%i", a); - ROS_INFO("value %i", acm_.at(mr->map().at("base"))[distance]); - acm_.at(mr->map().at(relevant_field))[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; - ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[k].c_str()); + /* For that specific entry, loop over values*/ + int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k])); + + if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){ + ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){ + ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){ + ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){ + ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + } + } + + if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_table)){ + /* In case an entry matches an robot spezific link*/ + ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j], j); + for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){ + /* For that specific entry, loop over values*/ + int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k])); + + if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){ + ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){ + ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){ + ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; + } + + if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){ + ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str()); + acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; } } } @@ -545,11 +541,6 @@ void Moveit_mediator::setup_task(){ merge_ps(ps_m, ps2, mr); } - /* - if(!acm_m.empty()){ - merge_acm(acm_m); - } - */ for(int i = 0; i < th.size(); i++){ th[i].join(); @@ -567,13 +558,17 @@ void Moveit_mediator::setup_task(){ void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){ moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt; + acmt.entry_values.resize(acm_.size()); + int i = 0; for (auto& a : acm_){ acmt.entry_names.push_back(a.first); acmt.entry_values[i].enabled = a.second; i++; } + ps_m.allowed_collision_matrix = acmt; + ROS_INFO("broken after merge"); } @@ -618,24 +613,6 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::Pla } } - - /* - for( auto aco : in->robot_state.attached_collision_objects) { - out.robot_state.attached_collision_objects.push_back(aco); - } - - for( auto aco : in->robot_state.joint_state) { - out.robot_state.attached_collision_objects.push_back(aco); - } - sensor_msgs::JointState:: - - for (auto str : res.allowed_collision_matrix.entry_names){ - auto entry = std::find(origin->allowed_collision_matrix.entry_names.begin(), origin->allowed_collision_matrix.entry_names.end(), str); - if (entry != origin->allowed_collision_matrix.entry_names.end()){ - origin->allowed_collision_matrix.entry_values[entry - origin->allowed_collision_matrix.entry_names.begin()] - } - } - */ } void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){ diff --git a/src/mtc2taskspace.cpp b/src/mtc2taskspace.cpp index e1b78f574d5fff8c1ecafe3b0f77c75f8e21390d..e73a81b69cc752585ca88d1c0bfecf5ee0bee468 100644 --- a/src/mtc2taskspace.cpp +++ b/src/mtc2taskspace.cpp @@ -1,13 +1,16 @@ #include "reader/abstract_param_reader.h" #include "reader/task_space_reader.h" #include "reader/robot_reader.h" +#include "reader/job_reader.h" + #include <regex> int main(int argc, char **argv){ ros::init(argc, argv, "mtc2taskspace"); std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle); - std::unique_ptr<Abstract_param_reader> ts_reader(new Task_space_reader(n)); - ts_reader->read(); + //std::unique_ptr<Abstract_param_reader> ts_reader(new Task_space_reader(n)); + std::unique_ptr<Abstract_param_reader> ts_reader(new Job_reader(n)); + return 0; } \ No newline at end of file diff --git a/src/reader/job_reader.cpp b/src/reader/job_reader.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fd7955d1937a19e57277c6cf81beedb054c43946 --- /dev/null +++ b/src/reader/job_reader.cpp @@ -0,0 +1,38 @@ +#include "reader/job_reader.h" + +void Job_reader::read(){ + ROS_INFO("--- JOB_READER ---"); + XmlRpc::XmlRpcValue resources; + nh_->getParam("/task/groups", resources); + + for (int i = 0; i < resources.size(); i++){ + std::string robot = resources[i]["name"]; + //nh_->getParam("/tasks/groups/" + robot, jobs); + std::vector<job_data> v_jd; + for (int j = 0; j < resources[i]["jobs"].size(); j++){ + ROS_INFO("--- %s --- JOB_%i ---", robot.c_str(), j); + job_data jd; + for(int k = 0; k < resources[i]["jobs"][j].size(); k++){ + XmlRpc::XmlRpcValue job_data = resources[i]["jobs"][j][k]; + tf2::Vector3 pos; + tf2::Quaternion rot; + + + (job_data["pos"].hasMember("x")) ? pos.setX(float_of(job_data["pos"]["x"])) :pos.setX(0); + (job_data["pos"].hasMember("y")) ? pos.setY(float_of(job_data["pos"]["y"])) :pos.setY(0); + (job_data["pos"].hasMember("z")) ? pos.setZ(float_of(job_data["pos"]["z"])) :pos.setZ(0); + (job_data["orientation"].hasMember("x")) ? rot.setX(float_of(job_data["orientation"]["x"])) :rot.setX(0); + (job_data["orientation"].hasMember("y")) ? rot.setY(float_of(job_data["orientation"]["y"])) :rot.setY(0); + (job_data["orientation"].hasMember("z")) ? rot.setZ(float_of(job_data["orientation"]["z"])) :rot.setZ(0); + (job_data["orientation"].hasMember("w")) ? rot.setW(float_of(job_data["orientation"]["w"])) :rot.setW(0); + + + ROS_INFO("=> Grab: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ()); + ROS_INFO("=> Grab: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW()); + jd.jobs_.push_back(tf2::Transform(rot, pos)); + } + v_jd.push_back(jd); + } + job_data_.insert(std::pair<std::string, std::vector<job_data>>(robot, v_jd)); + } +} \ No newline at end of file