Skip to content
Snippets Groups Projects
Commit 97007c06 authored by KingMaZito's avatar KingMaZito
Browse files

update acm, not perfect, but ok

parent 7aa3f9fa
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 410 additions and 272 deletions
.vscode/c_cpp_properties.json 0 → 100644
+ 39
− 0
View file @ 97007c06
{
"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
.vscode/settings.json 0 → 100644
+ 6
− 0
View file @ 97007c06
{
"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
CMakeLists.txt
+ 6
− 0
View file @ 97007c06
...@@ -103,6 +103,8 @@ src/reader/abstract_param_reader.cpp ...@@ -103,6 +103,8 @@ 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
) )
add_executable(grasp_cell_routine src/grasp_cell_routine.cpp add_executable(grasp_cell_routine src/grasp_cell_routine.cpp
...@@ -122,6 +124,8 @@ src/reader/abstract_param_reader.cpp ...@@ -122,6 +124,8 @@ 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
) )
...@@ -129,6 +133,8 @@ add_executable(mtc2taskspace src/mtc2taskspace.cpp ...@@ -129,6 +133,8 @@ add_executable(mtc2taskspace src/mtc2taskspace.cpp
src/reader/abstract_param_reader.cpp src/reader/abstract_param_reader.cpp
src/reader/task_space_reader.cpp src/reader/task_space_reader.cpp
src/reader/robot_reader.cpp src/reader/robot_reader.cpp
src/reader/job_reader.cpp
) )
......
include/impl/moveit_grasp_mediator.h
+ 5
− 1
View file @ 97007c06
...@@ -15,6 +15,8 @@ ...@@ -15,6 +15,8 @@
#include "reader/abstract_param_reader.h" #include "reader/abstract_param_reader.h"
#include "reader/robot_reader.h" #include "reader/robot_reader.h"
#include "reader/cuboid_reader.h" #include "reader/cuboid_reader.h"
#include "reader/job_reader.h"
...@@ -28,9 +30,11 @@ class Moveit_grasp_mediator : public Moveit_mediator{ ...@@ -28,9 +30,11 @@ class Moveit_grasp_mediator : public Moveit_mediator{
// Readers // 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> wing_reader_;
std::unique_ptr<Abstract_param_reader> cuboid_reader_; std::unique_ptr<Abstract_param_reader> cuboid_reader_;
std::unique_ptr<Job_reader> job_reader_;
public: public:
Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh); Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);
......
include/reader/abstract_param_reader.h
+ 0
− 1
View file @ 97007c06
...@@ -17,7 +17,6 @@ struct object_data { ...@@ -17,7 +17,6 @@ struct object_data {
}; };
struct job_data { struct job_data {
std::string name_;
std::vector<tf2::Transform> jobs_; std::vector<tf2::Transform> jobs_;
}; };
......
include/reader/job_reader.h
+ 4
− 6
View file @ 97007c06
...@@ -10,17 +10,15 @@ ...@@ -10,17 +10,15 @@
class Job_reader : public Abstract_param_reader{ class Job_reader : public Abstract_param_reader{
protected: protected:
std::vector<job_data> job_data_; std::map<std::string, std::vector<job_data>> job_data_;
std::string name_;
public: 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) : Abstract_param_reader(d)
, name_(name)
{read();} {read();}
inline void set_job_data(std::vector<job_data>& robot_data) {job_data_ = robot_data;} inline void set_job_data(std::map<std::string, std::vector<job_data>>& robot_data) {job_data_ = robot_data;}
inline std::vector<job_data> robot_data() {return job_data_;} inline std::map<std::string, std::vector<job_data>>& robot_data() {return job_data_;}
void read() override; void read() override;
}; };
......
jobs/dummy.yaml 0 → 100644
+ 56
− 0
View file @ 97007c06
{ '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 } }
]]}
]
}
}
launch/cell_routine.launch
+ 4
− 0
View file @ 97007c06
<launch> <launch>
<arg name="result" default="dummy/-562289182.yaml" /> <arg name="result" default="dummy/-562289182.yaml" />
<arg name="jobs" default="jobs/dummy.yaml" />
<!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> --> <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
<!-- this is to change--> <!-- this is to change-->
...@@ -7,6 +9,8 @@ ...@@ -7,6 +9,8 @@
<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 ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam> <rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
......
launch/mtc2taskspace.launch 0 → 100644
+ 8
− 0
View file @ 97007c06
<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>
src/impl/moveit_grasp_mediator.cpp
+ 2
− 2
View file @ 97007c06
...@@ -527,7 +527,7 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf ...@@ -527,7 +527,7 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf
, robot_reader_(std::make_unique<Robot_reader>(nh)) , robot_reader_(std::make_unique<Robot_reader>(nh))
, wing_reader_(std::make_unique<Wing_reader>(nh)) , wing_reader_(std::make_unique<Wing_reader>(nh))
, cuboid_reader_(std::make_unique<Cuboid_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>( visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_); robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_);
visual_tools_->loadMarkerPub(); visual_tools_->loadMarkerPub();
...@@ -551,7 +551,7 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf ...@@ -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"); 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_); 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_)); 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(); auto wd = static_cast<Wing_reader*>(wing_reader_.get())->wing_data();
......
src/impl/moveit_mediator.cpp
+ 237
− 260
View file @ 97007c06
...@@ -244,6 +244,7 @@ void Moveit_mediator::setup_task(){ ...@@ -244,6 +244,7 @@ void Moveit_mediator::setup_task(){
for(auto& a: acm_) a.second.resize(acm_.size(), 0); for(auto& a: acm_) a.second.resize(acm_.size(), 0);
while (!tasks[0].empty() && !tasks[1].empty()){ 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++){ 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; moveit_msgs::RobotTrajectory* t1 = nullptr;
...@@ -253,228 +254,150 @@ void Moveit_mediator::setup_task(){ ...@@ -253,228 +254,150 @@ void Moveit_mediator::setup_task(){
moveit_msgs::PlanningScene ps_m; moveit_msgs::PlanningScene ps_m;
std::vector<moveit_msgs::PlanningScene::_allowed_collision_matrix_type> acm_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()){ if (i < tasks[0].front().solution.sub_trajectory.size()){
t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory; t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
ps1 = &tasks[0].front().solution.sub_trajectory[i].scene_diff; 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()){ if (i < tasks[1].front().solution.sub_trajectory.size()){
t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory; t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory;
ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff; ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff;
}
std::vector<std::thread> th;
ROS_INFO("[Robot 2] acm"); if (t1){
ROS_INFO("default entry names"); Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]);
for (auto den : ps2->allowed_collision_matrix.default_entry_names){ th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t1, *ps1));
ROS_INFO("%s", den.c_str());
// First find ID from panda to start with
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());
std::regex rx_box("bottle.*");
std::regex rx_table("table.*");
// Iterate task collsion matrix
for(int j = 0; j < ps1->allowed_collision_matrix.entry_names.size(); j++ ){
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++){
/* 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];
} }
ROS_INFO("default entry values");
for (auto den : ps2->allowed_collision_matrix.default_entry_values){ if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
ROS_INFO("%i", den); 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];
} }
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());
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];
} }
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"); if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
for (auto lp : ps2->link_padding){ /* In case an entry matches an robot spezific link*/
ROS_INFO("link padding name: %s, padding %f", lp.link_name.c_str(), lp.padding); 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];
} }
ROS_INFO("[Robot 2] link scale");
for (auto ls : ps2->link_scale){ if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
ROS_INFO("link scale name: %s, padding %f", ls.link_name.c_str(), ls.scale); 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];
} }
ROS_INFO("[Robot 2] name");
ROS_INFO("name: %s", ps2->name.c_str()); if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
ROS_INFO("[Robot 2] object color"); ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
for (auto oc : ps2->object_colors){ acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
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()); if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
ROS_INFO("[Robot 2] rs"); ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
for (auto aco : ps2->robot_state.attached_collision_objects){ acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
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);
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]));
for (auto p : ps2->world.collision_objects) ROS_INFO("world co id %s", p.id.c_str()); if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
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("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
ROS_INFO("world om header time, id %s, seq %i", ps2->world.octomap.header.frame_id.c_str(), ps2->world.octomap.header.seq); acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
ROS_INFO("world om Data");
} }
std::vector<std::thread> th; 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());
if (t1){ acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]); }
th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t1, *ps1));
// First find ID from panda to start with if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
std::regex rx_panda("panda_arm([0-9]+)"); ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
std::smatch match; acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
std::regex_match(mr->name(), match, rx_panda); }
// build panda link regex if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
std::stringstream ss; ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
ss << "panda_" << match[1] << "_*"; acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
std::regex rx_panda_links(ss.str().c_str()); }
std::regex rx_box("bottle*"); }
std::regex rx_table("table*"); }
ROS_INFO("start merging"); if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_table)){
// Iterate task collsion matrix /* In case an entry matches an robot spezific link*/
for(int j = 0; j < ps1->allowed_collision_matrix.entry_names.size(); j++ ){ ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j], 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");
for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){ 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 /* For that specific entry, loop over values*/
ROS_INFO("start filtering"); 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) ||
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_panda_links)){
if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)) relevant_field = match[0].str(); ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)) relevant_field = match[0].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]) relevant_field = mr->map().at("base").c_str(); }
// => Update acm_ if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
// calculate the distance in acm for name in in acm message ('cause they may differ, you know) ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
// totally not stolen from SO acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
ROS_INFO("calculate distance"); }
int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
// better filter with regex if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
// set acm value with respect to the position in acm ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
ROS_INFO("acm überschreiben mit distnace %i", distance); acm_.at(ps1->allowed_collision_matrix.entry_names[k])[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(auto a : acm_.at(mr->map().at("base"))) ROS_INFO("%i", a); if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
ROS_INFO("value %i", acm_.at(mr->map().at("base"))[distance]); ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
acm_.at(mr->map().at(relevant_field))[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k]; acm_.at(ps1->allowed_collision_matrix.entry_names[k])[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());
} }
} }
} }
...@@ -485,71 +408,139 @@ void Moveit_mediator::setup_task(){ ...@@ -485,71 +408,139 @@ void Moveit_mediator::setup_task(){
if (t2){ if (t2){
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[1]); Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[1]);
th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2)); th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2));
std::regex rx_panda("panda_arm([0-9]+)"); std::regex rx_panda("panda_arm([0-9]+)");
std::smatch match; std::smatch match;
std::regex_match(mr->name(), match, rx_panda); std::regex_match(mr->name(), match, rx_panda);
// build panda link regex // build panda link regex
std::stringstream ss; std::stringstream ss;
ss << "panda_" << match[1] << "_*"; ss << "panda_" << match[1] << "_.*";
std::regex rx_panda_links(ss.str().c_str()); std::regex rx_panda_links(ss.str());
std::regex rx_box("bottle*"); std::regex rx_box("bottle.*");
std::regex rx_table("table*"); std::regex rx_table("table.*");
ROS_INFO("start merging");
// Iterate task collsion matrix // Iterate task collsion matrix
for(int j = 0; j < ps2->allowed_collision_matrix.entry_names.size(); j++ ){ for(int j = 0; j < ps2->allowed_collision_matrix.entry_names.size(); j++ ){
// look for relevant fields if( mr->map().at("base") == ps2->allowed_collision_matrix.entry_names[j]){
ROS_INFO("compare %s and %s", mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[j].c_str()); /* In case an entry matches an robot spezific link*/
if( std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box) || ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
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");
for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){ 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 /* For that specific entry, loop over values*/
ROS_INFO("start filtering"); 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) || 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) || ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table) || acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
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_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_panda_links)) relevant_field = match[0].str(); if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)) relevant_field = match[0].str(); ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)) relevant_field = match[0].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]) relevant_field = mr->map().at("base").c_str(); }
// => Update acm_ if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
// calculate the distance in acm for name in in acm message ('cause they may differ, you know) ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
// totally not stolen from SO acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
ROS_INFO("calculate distance"); }
}
}
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])); int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
// better filter with regex if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
// set acm value with respect to the position in acm ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
ROS_INFO("acm überschreiben mit distnace %i", distance); acm_.at(ps2->allowed_collision_matrix.entry_names[k])[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(auto a : acm_.at(mr->map().at("base"))) ROS_INFO("%i", a); if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
ROS_INFO("value %i", acm_.at(mr->map().at("base"))[distance]); ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
acm_.at(mr->map().at(relevant_field))[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k]; acm_.at(ps2->allowed_collision_matrix.entry_names[k])[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());
} }
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];
} }
} }
merge_ps(ps_m, ps2, mr);
} }
/* if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box)){
if(!acm_m.empty()){ /* In case an entry matches an robot spezific link*/
merge_acm(acm_m); 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_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];
}
}
}
}
merge_ps(ps_m, ps2, mr);
}
for(int i = 0; i < th.size(); i++){ for(int i = 0; i < th.size(); i++){
th[i].join(); th[i].join();
...@@ -567,13 +558,17 @@ void Moveit_mediator::setup_task(){ ...@@ -567,13 +558,17 @@ void Moveit_mediator::setup_task(){
void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){ void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt; moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
acmt.entry_values.resize(acm_.size());
int i = 0; int i = 0;
for (auto& a : acm_){ for (auto& a : acm_){
acmt.entry_names.push_back(a.first); acmt.entry_names.push_back(a.first);
acmt.entry_values[i].enabled = a.second; acmt.entry_values[i].enabled = a.second;
i++; i++;
} }
ps_m.allowed_collision_matrix = acmt; 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 ...@@ -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){ void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){
......
src/mtc2taskspace.cpp
+ 5
− 2
View file @ 97007c06
#include "reader/abstract_param_reader.h" #include "reader/abstract_param_reader.h"
#include "reader/task_space_reader.h" #include "reader/task_space_reader.h"
#include "reader/robot_reader.h" #include "reader/robot_reader.h"
#include "reader/job_reader.h"
#include <regex> #include <regex>
int main(int argc, char **argv){ int main(int argc, char **argv){
ros::init(argc, argv, "mtc2taskspace"); ros::init(argc, argv, "mtc2taskspace");
std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle); std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
std::unique_ptr<Abstract_param_reader> ts_reader(new Task_space_reader(n)); //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 Job_reader(n));
return 0; return 0;
} }
\ No newline at end of file
src/reader/job_reader.cpp 0 → 100644
+ 38
− 0
View file @ 97007c06
#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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment