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

...container

parent ae0e0eb7
Branches
No related tags found
No related merge requests found
{ '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
<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>
{ '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
...@@ -4,6 +4,7 @@ Execution::Execution(const std::string& name, const BT::NodeConfiguration& confi ...@@ -4,6 +4,7 @@ Execution::Execution(const std::string& name, const BT::NodeConfiguration& confi
: BT::StatefulActionNode(name, config) {} : BT::StatefulActionNode(name, config) {}
BT::NodeStatus Execution::onStart(){ BT::NodeStatus Execution::onStart(){
ROS_INFO("PING");
if (it_ != ets_.solution.sub_trajectory.end()){ if (it_ != ets_.solution.sub_trajectory.end()){
std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff); std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
executions_->insert_or_assign(mr_reference_->name(), pair); executions_->insert_or_assign(mr_reference_->name(), pair);
......
...@@ -96,9 +96,9 @@ void GraspMediator::generateGraspMap() { ...@@ -96,9 +96,9 @@ void GraspMediator::generateGraspMap() {
voxelSpace.y_width = 0.3f; voxelSpace.y_width = 0.3f;
voxelSpace.z_heigth = 0.2f; voxelSpace.z_heigth = 0.2f;
voxel_manager_->setVerboseLevel(0); //voxel_manager_->setVerboseLevel(0);
voxel_manager_->setVoxelSize(0.02f); //voxel_manager_->setVoxelSize(0.02f);
voxel_manager_->setVoxelSpace(voxelSpace); //voxel_manager_->setVoxelSpace(voxelSpace);
ROS_INFO("Debug2.5!"); ROS_INFO("Debug2.5!");
XmlRpc::XmlRpcValue data; XmlRpc::XmlRpcValue data;
......
...@@ -134,33 +134,24 @@ void MoveitMediator::manipulateACM(AbstractRobotDecorator* mr, moveit_msgs::Plan ...@@ -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++ ){ 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]){ if( mr->map().at("base") == ps.allowed_collision_matrix.entry_names[j]){
//In case an entry matches an robot spezific link //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(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
// For that specific entry, loop over values // For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k])); 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)){ 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]; 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)){ 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]; 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)){ 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]; 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]){ 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]; 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 ...@@ -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])); 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)){ 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]; 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)){ 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]; 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)){ 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]; 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]){ 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]; 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 ...@@ -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)){ if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_box)){
//In case an entry matches an robot spezific link //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(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
//For that specific entry, loop over values //For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k])); 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)){ 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]; 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)){ 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]; 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)){ 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]; 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]){ 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]; 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 ...@@ -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)){ if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_table)){
//In case an entry matches an robot spezific link //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(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
//For that specific entry, loop over values //For that specific entry, loop over values
int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k])); 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)){ 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]; 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)){ 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]; 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)){ 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]; 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]){ 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]; 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(){ ...@@ -425,9 +382,14 @@ void MoveitMediator::taskPlanner(){
std::stringstream ss; std::stringstream ss;
ss << "<root main_tree_to_execute = \"MainTree\">\n"; ss << "<root main_tree_to_execute = \"MainTree\">\n";
ss << "<BehaviorTree ID=\"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_){ 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"; 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)){ for (auto& p_obj: task_but_different.at(s_r.first)){
ss << "<SequenceStar name=\"root_sequence\">\n"; ss << "<SequenceStar name=\"root_sequence\">\n";
...@@ -448,6 +410,11 @@ void MoveitMediator::taskPlanner(){ ...@@ -448,6 +410,11 @@ void MoveitMediator::taskPlanner(){
auto node_it = tree.nodes.begin(); auto node_it = tree.nodes.begin();
for (const auto& s_r : robots_){ 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)){ for (auto& p_obj: task_but_different.at(s_r.first)){
while (node_it->get()->type() == BT::NodeType::CONTROL) ++node_it; while (node_it->get()->type() == BT::NodeType::CONTROL) ++node_it;
if(node_it->get()->type() == BT::NodeType::ACTION) ROS_INFO("Action"); if(node_it->get()->type() == BT::NodeType::ACTION) ROS_INFO("Action");
...@@ -478,6 +445,7 @@ void MoveitMediator::taskPlanner(){ ...@@ -478,6 +445,7 @@ void MoveitMediator::taskPlanner(){
for (const auto& s_r : robots_){ for (const auto& s_r : robots_){
try{ try{
ROS_INFO("%s", s_r.first.c_str());
auto temp = executions_.at(s_r.first); auto temp = executions_.at(s_r.first);
th.push_back(std::thread(&MoveitMediator::parallelExec, this, std::ref(*s_r.second.get()), temp.first)); th.push_back(std::thread(&MoveitMediator::parallelExec, this, std::ref(*s_r.second.get()), temp.first));
//mr->mgi()->execute(temp.first); //mr->mgi()->execute(temp.first);
...@@ -572,7 +540,6 @@ void MoveitMediator::mergeACM(moveit_msgs::PlanningScene& ps_m){ ...@@ -572,7 +540,6 @@ void MoveitMediator::mergeACM(moveit_msgs::PlanningScene& ps_m){
// } // }
ps_m.allowed_collision_matrix = acmt; ps_m.allowed_collision_matrix = acmt;
ROS_INFO("broken after merge");
} }
...@@ -711,7 +678,7 @@ moveit::task_constructor::Task MoveitMediator::createTask(AbstractRobotDecorator ...@@ -711,7 +678,7 @@ moveit::task_constructor::Task MoveitMediator::createTask(AbstractRobotDecorator
stage->properties().set("marker_ns", "grasp_pose"); stage->properties().set("marker_ns", "grasp_pose");
stage->setPreGraspPose("open"); stage->setPreGraspPose("open");
stage->setObject(object); stage->setObject(object);
stage->setAngleDelta(M_PI / 12); stage->setAngleDelta(M_PI / 2);
stage->setMonitoredStage(current_state_ptr); // Hook into current state stage->setMonitoredStage(current_state_ptr); // Hook into current state
// Compute IK // Compute IK
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment