diff --git a/jobs/eval_dual.yaml b/jobs/eval_dual.yaml
index 3d2c9a1a25599f711ba057a92df371f302fe951c..a9f3874c83fe476805d7ff5f7556bf4c9b53c7de 100644
--- a/jobs/eval_dual.yaml
+++ b/jobs/eval_dual.yaml
@@ -14,4 +14,4 @@
}
]
}
-}
+}
\ No newline at end of file
diff --git a/jobs/eval_quad.yaml b/jobs/eval_quad.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2cbddbc07f2a38d825449cda84e0dc52bb6cbacd
--- /dev/null
+++ b/jobs/eval_quad.yaml
@@ -0,0 +1,33 @@
+{ 'tasks':
+ {'groups' : [
+ { 'name': 'panda_arm1', 'jobs':[
+ [
+ { 'pos': { 'x': 1.2 ,'y': 1,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 1.2 ,'y': 0.4,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+ ]
+ ]
+ },
+ { 'name': 'panda_arm2', 'jobs':[
+ [
+ { 'pos': { 'x': 0.2 ,'y': 1,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.2 ,'y': 0.4,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+ ]
+ ]
+ },
+ { 'name': 'panda_arm3', 'jobs':[
+ [
+ { 'pos': { 'x': 1.2 ,'y': 0.2,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 1.2 ,'y': -0.4,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+ ]
+ ]
+ },
+ { 'name': 'panda_arm4', 'jobs':[
+ [
+ { 'pos': { 'x': 0.2 ,'y': 0.2,'z': 0.935500 }, 'orientation': { 'w': 1 } },
+ { 'pos': { 'x': 0.2 ,'y': -0.4,'z': 0.935500 }, 'orientation': { 'w': 1 } }
+ ]
+ ]
+ }
+ ]
+ }
+}
\ No newline at end of file
diff --git a/launch/quad_cell_routine.launch b/launch/quad_cell_routine.launch
new file mode 100644
index 0000000000000000000000000000000000000000..18627ba73a2f4b2e4d82bde46473fc31ec9c2319
--- /dev/null
+++ b/launch/quad_cell_routine.launch
@@ -0,0 +1,36 @@
+<launch>
+ <arg name="result" default="evaluation/quadrupel_eval.yaml" />
+ <arg name="jobs" default="jobs/eval_quad.yaml" />
+
+ <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
+
+ <!-- this is to change-->
+ <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
+ <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)/$(arg jobs)"/>
+
+
+
+ <rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
+
+ <include file="$(find ceti_quadrupel)/launch/demo.launch">
+ <arg name="use_rviz" value="false"/>
+ <arg name="scene" value="$(arg result)" />
+ </include>
+
+ <include ns="cell_routine" file="$(find ceti_quadrupel)/launch/fake_moveit_controller_manager.launch.xml" />
+ <param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
+
+ <include file="$(find ceti_quadrupel)/launch/moveit_rviz.launch">
+ <arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
+ </include>
+
+ <node pkg="groot" type="Groot" name="Groot" output="screen" required="true">
+ </node>
+
+ <node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
+ </node>
+</launch>
+
+
diff --git a/results/evaluation/quadrupel_eval.yaml b/results/evaluation/quadrupel_eval.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..f5d2eb9d6d40f9f62af559e39353144b42e041c4
--- /dev/null
+++ b/results/evaluation/quadrupel_eval.yaml
@@ -0,0 +1,44 @@
+{ 'objects' : [
+{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table1_table_top', 'pos': { 'x': 1 , 'y': 1 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } },
+{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table2_table_top', 'pos': { 'x': 0 , 'y': 1 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000003 } },
+{ 'id' : 'table3_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table3_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table3_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table3_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table3_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table3_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table3_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table3_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table3_table_top', 'pos': { 'x': 1 , 'y': 0 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000003 } },
+{ 'id' : 'table4_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table4_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table4_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table4_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table4_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table4_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table4_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table4_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},
+{ 'id' : 'table4_table_top', 'pos': { 'x': 0 , 'y': 0 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000003 } },
+
+{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
+{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': 1.2, 'y': 1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': 1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': 1.2, 'y': 0.2, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+{ 'id': 'blue4', 'type': 'BOX', 'pos': { 'x': 0.2, 'y': 0.2, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+]}
\ No newline at end of file
diff --git a/src/bt/execution.cpp b/src/bt/execution.cpp
index a947b593174821ea67f9f14e072e895f7c0c80fa..c32033bf55cb039afc38d266d3acbad024711944 100644
--- a/src/bt/execution.cpp
+++ b/src/bt/execution.cpp
@@ -4,6 +4,7 @@ Execution::Execution(const std::string& name, const BT::NodeConfiguration& confi
: BT::StatefulActionNode(name, config) {}
BT::NodeStatus Execution::onStart(){
+ ROS_INFO("PING");
if (it_ != ets_.solution.sub_trajectory.end()){
std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
executions_->insert_or_assign(mr_reference_->name(), pair);
diff --git a/src/mediator/grasp_mediator.cpp b/src/mediator/grasp_mediator.cpp
index d9a34e8b453d8b37304c263d305bae19a352bbae..9bcd9e480fa6a093c90607bee17cf2c80e18e44d 100644
--- a/src/mediator/grasp_mediator.cpp
+++ b/src/mediator/grasp_mediator.cpp
@@ -96,9 +96,9 @@ void GraspMediator::generateGraspMap() {
voxelSpace.y_width = 0.3f;
voxelSpace.z_heigth = 0.2f;
- voxel_manager_->setVerboseLevel(0);
- voxel_manager_->setVoxelSize(0.02f);
- voxel_manager_->setVoxelSpace(voxelSpace);
+ //voxel_manager_->setVerboseLevel(0);
+ //voxel_manager_->setVoxelSize(0.02f);
+ //voxel_manager_->setVoxelSpace(voxelSpace);
ROS_INFO("Debug2.5!");
XmlRpc::XmlRpcValue data;
diff --git a/src/mediator/moveit_mediator.cpp b/src/mediator/moveit_mediator.cpp
index 4216a1487c1f81f70a1a9efec36939b92e628194..3d71f7a2e6bbcc1f3a3783702a14068b256a466d 100644
--- a/src/mediator/moveit_mediator.cpp
+++ b/src/mediator/moveit_mediator.cpp
@@ -134,33 +134,24 @@ void MoveitMediator::manipulateACM(AbstractRobotDecorator* mr, moveit_msgs::Plan
for(int j = 0; j < ps.allowed_collision_matrix.entry_names.size(); j++ ){
if( mr->map().at("base") == ps.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 < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
// For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
if (std::regex_match(ps.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());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
- ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
- ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
- ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
}
@@ -174,26 +165,18 @@ void MoveitMediator::manipulateACM(AbstractRobotDecorator* mr, moveit_msgs::Plan
int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
if (std::regex_match(ps.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());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
- ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
- ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
- ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
}
@@ -201,32 +184,23 @@ void MoveitMediator::manipulateACM(AbstractRobotDecorator* mr, moveit_msgs::Plan
if(std::regex_match(ps.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", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
//For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
if (std::regex_match(ps.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());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
- ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
- ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
- ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
}
@@ -234,45 +208,28 @@ void MoveitMediator::manipulateACM(AbstractRobotDecorator* mr, moveit_msgs::Plan
if(std::regex_match(ps.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", ps.allowed_collision_matrix.entry_names[j], j);
for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
//For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
if (std::regex_match(ps.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());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
- ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
- ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
- ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
- ROS_INFO("Changing at %s at position %i to %i", ps.allowed_collision_matrix.entry_names[k].c_str(), distance, ps.allowed_collision_matrix.entry_values[j].enabled[k]);
acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
}
}
}
}
-
- ROS_INFO("after run");
- for(auto& entry : acm_){
- ROS_INFO("%s", entry.first.c_str());
- for (auto& enable : entry.second) std::cout << +enable << " ";
- std::cout<< "\n";
- }
-
}
@@ -425,9 +382,14 @@ void MoveitMediator::taskPlanner(){
std::stringstream ss;
ss << "<root main_tree_to_execute = \"MainTree\">\n";
ss << "<BehaviorTree ID=\"MainTree\">\n";
- ss << "<Control ID=\"Parallel\" name=\"Agents\" success_threshold=\""<< robots_.size() << "\" failure_threshold=\""<< robots_.size() << "\">\n";
-
+ ss << "<Control ID=\"Parallel\" name=\"Agents\" success_threshold=\""<< 2 << "\" failure_threshold=\""<< 1 << "\">\n";
+
for (const auto& s_r : robots_){
+ try {
+ auto a = task_but_different.at(s_r.first);
+ } catch (std::out_of_range& oor){
+ continue;
+ }
ss << "<Control ID=\"Parallel_robot\" name=\""<< s_r.first << "\" success_threshold=\""<< task_but_different.at(s_r.first).size() << "\" failure_threshold=\""<< task_but_different.at(s_r.first).size() << "\">\n";
for (auto& p_obj: task_but_different.at(s_r.first)){
ss << "<SequenceStar name=\"root_sequence\">\n";
@@ -448,6 +410,11 @@ void MoveitMediator::taskPlanner(){
auto node_it = tree.nodes.begin();
for (const auto& s_r : robots_){
+ try {
+ auto a = task_but_different.at(s_r.first);
+ } catch (std::out_of_range& oor){
+ continue;
+ }
for (auto& p_obj: task_but_different.at(s_r.first)){
while (node_it->get()->type() == BT::NodeType::CONTROL) ++node_it;
if(node_it->get()->type() == BT::NodeType::ACTION) ROS_INFO("Action");
@@ -478,6 +445,7 @@ void MoveitMediator::taskPlanner(){
for (const auto& s_r : robots_){
try{
+ ROS_INFO("%s", s_r.first.c_str());
auto temp = executions_.at(s_r.first);
th.push_back(std::thread(&MoveitMediator::parallelExec, this, std::ref(*s_r.second.get()), temp.first));
//mr->mgi()->execute(temp.first);
@@ -572,7 +540,6 @@ void MoveitMediator::mergeACM(moveit_msgs::PlanningScene& ps_m){
// }
ps_m.allowed_collision_matrix = acmt;
- ROS_INFO("broken after merge");
}
@@ -711,7 +678,7 @@ moveit::task_constructor::Task MoveitMediator::createTask(AbstractRobotDecorator
stage->properties().set("marker_ns", "grasp_pose");
stage->setPreGraspPose("open");
stage->setObject(object);
- stage->setAngleDelta(M_PI / 12);
+ stage->setAngleDelta(M_PI / 2);
stage->setMonitoredStage(current_state_ptr); // Hook into current state
// Compute IK