diff --git a/include/impl/moveit_grasp_mediator.h b/include/impl/moveit_grasp_mediator.h
index b3f3856cd48507220e3f9b1bc314ff8666ed586f..09f727b3a24f522bea77f6a0d2beab8de8b1be46 100644
--- a/include/impl/moveit_grasp_mediator.h
+++ b/include/impl/moveit_grasp_mediator.h
@@ -41,7 +41,7 @@ class Moveit_grasp_mediator : public Moveit_mediator{
void manipulate_grasp_data(Moveit_robot* robot);
void scene_setup();
- void manipulate_mapspace();
+ void manipulate_mapspace(tf2::Transform& tf, tf2::Vector3& size);
bool generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose, double& x_depth, double& y_width, double& z_height);
bool getIKSolution(const moveit::core::JointModelGroup* arm_jmg, const Eigen::Isometry3d& target_pose, robot_state::RobotState& solution, const std::string& link_name);
bool planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates,
diff --git a/include/impl/moveit_mediator.h b/include/impl/moveit_mediator.h
index e551a84d2050defb744ab4faca4ab3be8a96ff17..df4adfb259953ac42012975a9403b46b60ece674 100644
--- a/include/impl/moveit_mediator.h
+++ b/include/impl/moveit_mediator.h
@@ -45,6 +45,7 @@
#include <moveit/planning_scene_monitor/current_state_monitor.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <stdint.h>
class Moveit_mediator : public Abstract_mediator{
@@ -66,6 +67,11 @@ class Moveit_mediator : public Abstract_mediator{
std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;
+ std::map<std::string, std::vector<uint8_t>> acm_;
+
+
+
+
public:
Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);
@@ -82,6 +88,8 @@ class Moveit_mediator : public Abstract_mediator{
void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
+ void merge_acm(moveit_msgs::PlanningScene& in);
+
void publish_tables();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps);
diff --git a/include/impl/moveit_robot.h b/include/impl/moveit_robot.h
index ca49cc751d7330c98fb8fec078bc3b819713195d..4b08048ab55abe743cfcd92d1fb7b7c7c550b161 100644
--- a/include/impl/moveit_robot.h
+++ b/include/impl/moveit_robot.h
@@ -23,9 +23,10 @@ class Moveit_robot : public Robot{
Moveit_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size)
, mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>(name)){
- std::stringstream hand_n, ik_frame_n, name_n;
+ std::stringstream hand_n, ik_frame_n, name_n, base_n;
hand_n << "hand_" << name.back();
ik_frame_n << "panda_" << name.back() << "_link8";
+ base_n << "base_" << name.back();
mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str());
@@ -35,6 +36,8 @@ class Moveit_robot : public Robot{
map_.insert(std::make_pair<std::string, std::string>("eef_name", hand_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand_frame", ik_frame_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str()));
+ map_.insert(std::make_pair<std::string, std::string>("base", base_n.str()));
+
}
inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;}
diff --git a/include/impl/wing.h b/include/impl/wing.h
index 6fc94d09ef38523433c6790bf5933dfd33324570..784945d454539beb9fd8450f942a03c571867e75 100644
--- a/include/impl/wing.h
+++ b/include/impl/wing.h
@@ -24,7 +24,7 @@ class Wing : public Abstract_robot_element{
inline void set_name(std::string str) {name_ = str;}
inline void set_set(tf2::Vector3& vec) {size_ = vec;}
- inline tf2::Vector3 size(){ return size_;}
+ inline tf2::Vector3& size(){ return size_;}
inline std::vector<tf2::Transform>& bounds() {return bounds_;}
void update(tf2::Transform& tf) override {this->calc_world_tf(tf);}
};
diff --git a/include/reader/abstract_param_reader.h b/include/reader/abstract_param_reader.h
index a3f7ee4b6b246ea52676b354f20c5d1190c38862..5cbf34d499d8c35e096217029c2df4ed614de4bf 100644
--- a/include/reader/abstract_param_reader.h
+++ b/include/reader/abstract_param_reader.h
@@ -16,6 +16,11 @@ struct object_data {
tf2::Vector3 size_;
};
+struct job_data {
+ std::string name_;
+ std::vector<tf2::Transform> jobs_;
+};
+
class Abstract_param_reader{
protected:
std::shared_ptr<ros::NodeHandle> nh_;
diff --git a/include/reader/job_reader.h b/include/reader/job_reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e75feef365c0f769c3928832a10417959d9178c
--- /dev/null
+++ b/include/reader/job_reader.h
@@ -0,0 +1,27 @@
+#ifndef JOB_READER_
+#define JOB_READER_
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include <xmlrpcpp/XmlRpc.h>
+
+#include "reader/abstract_param_reader.h"
+
+
+class Job_reader : public Abstract_param_reader{
+ protected:
+ std::vector<job_data> job_data_;
+ std::string name_;
+
+ public:
+ Job_reader(std::shared_ptr<ros::NodeHandle> const& d, std::string const& name)
+ : Abstract_param_reader(d)
+ , name_(name)
+ {read();}
+
+ inline void set_job_data(std::vector<job_data>& robot_data) {job_data_ = robot_data;}
+ inline std::vector<job_data> robot_data() {return job_data_;}
+
+ void read() override;
+};
+#endif
diff --git a/src/impl/moveit_grasp_mediator.cpp b/src/impl/moveit_grasp_mediator.cpp
index a933e6b2f0f34dcae5fab341bdfa1c58cf9cc9a2..cc7129286701ceb2a19b66d2bbfb423a26aa7aae 100644
--- a/src/impl/moveit_grasp_mediator.cpp
+++ b/src/impl/moveit_grasp_mediator.cpp
@@ -2,6 +2,10 @@
#include <regex>
#include <gb_grasp/MapConfigLoader.h>
#include <moveit/robot_state/conversions.h>
+<<<<<<< HEAD
+=======
+#include "impl/wing_moveit_decorator.h"
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
@@ -10,25 +14,35 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
auto mr = dynamic_cast<Moveit_robot*>(robot);
manipulate_grasp_data(mr);
+<<<<<<< HEAD
manipulate_mapspace();
+=======
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
auto map_gen = std::make_shared<MapGenerator>(*nh_, visual_tools_, planning_scene_monitor_, mr->map().at("eef_name"), mr->name().c_str(), robot_model_->getJointModelGroup(mr->name().c_str()), mgi().get()->getCurrentState(), voxel_manager_);
double min_quality, max_candidates;
bool top_only;
- rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"min_quality",min_quality);
+ rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"min_quality",min_quality);
rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"max_candidate_count",max_candidates);
rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"top_grasps_only",top_only);
+<<<<<<< HEAD
+=======
+ ROS_INFO("min_quality: %f", min_quality);
+ ROS_INFO("max_candidate_count: %f", max_candidates);
+ ROS_INFO("top_grasps_only: %f", top_only);
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
- map_gen->SetQuality(min_quality);
- map_gen->SetMaxGrasp(max_candidates);
+ map_gen->SetQuality(min_quality);
+ map_gen->SetMaxGrasp(max_candidates);
map_gen->SetZGraspOnly(top_only);
mr->set_grasp_map_generator(map_gen);
}
+<<<<<<< HEAD
void Moveit_grasp_mediator::manipulate_mapspace(){
std::vector<float> pos_voxel_space;
std::vector<float> pos_mapspace;
@@ -38,6 +52,20 @@ void Moveit_grasp_mediator::manipulate_mapspace(){
pos_voxel_space[2]= 0.91f;
nh_->setParam("/voxel_space/pos", pos_voxel_space);
nh_->setParam("/mapspace/pos", pos_mapspace);
+=======
+void Moveit_grasp_mediator::manipulate_mapspace(tf2::Transform& tf, tf2::Vector3& size){
+ double r,p,y;
+ std::vector<double> rot_mapspace(3,0);
+
+ tf2::Matrix3x3 m(tf.getRotation());
+ m.getRPY(r,p, y);
+ nh_->setParam("/mapspace/rot", std::vector<double>({r,p,y}));
+ nh_->setParam("/mapspace/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), 0.91f}));
+ nh_->setParam("/mapspace/dim", std::vector<double>({size.getX(), size.getY(), size.getZ()}));
+ //nh_->setParam("/voxel_space/dim", std::vector<float>({0.3f,0.3f,0.2f}));
+ //nh_->setParam("/voxel_space/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), tf.getOrigin().getZ()}));
+ //nh_->setParam("/voxel_space/rot", std::vector<double>({r,p,y}));
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
}
void Moveit_grasp_mediator::manipulate_grasp_data(Moveit_robot* robot) {
@@ -103,7 +131,11 @@ bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geome
double& y_width, double& z_height)
{
// Generate random cuboid
+<<<<<<< HEAD
double xmin = 0.5;
+=======
+ double xmin = 0.1;
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
double xmax = 0.7;
double ymin = -0.25;
double ymax = 0.25;
@@ -134,6 +166,7 @@ bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geome
void Moveit_grasp_mediator::mediate(){
visual_tools_->deleteAllMarkers();
+<<<<<<< HEAD
/*
MapConfigLoader map_config(*nh_, grasp_ns_);
auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
@@ -162,6 +195,87 @@ void Moveit_grasp_mediator::mediate(){
}
voxel_manager_->Voxelize(true);
*/
+=======
+ ROS_INFO("ns is %s", grasp_ns_.c_str());
+ MapConfigLoader map_config(*nh_, grasp_ns_.c_str());
+ auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
+ map_config.SetObjects(cbd);
+ float object_offset = 0.02f;
+
+ tf2::Vector3 middle_pt(0,0,0);
+ std::vector<double> x;
+ std::vector<double> y;
+ for(auto* ar: robots_){
+ Moveit_robot* r = dynamic_cast<Moveit_robot*>(ar);
+ tf2::Vector3 pos = r->tf().getOrigin();
+ pos.setZ((pos.getZ()*2) +object_offset);
+ tf2::Transform man_tf(r->tf().getRotation(), pos);
+ manipulate_mapspace(man_tf, r->size());
+ map_config.Create(grasp_maps_);
+ middle_pt+= pos;
+ ROS_INFO("X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());
+
+ tf2::Transform A = r->tf() * r->bounds()[0];
+ tf2::Transform B = r->tf() * r->bounds()[1];
+ tf2::Transform C = r->tf() * r->bounds()[2];
+ tf2::Transform D = r->tf() * r->bounds()[3];
+ x.push_back(A.getOrigin().getX());
+ x.push_back(B.getOrigin().getX());
+ x.push_back(C.getOrigin().getX());
+ x.push_back(D.getOrigin().getX());
+ y.push_back(A.getOrigin().getY());
+ y.push_back(B.getOrigin().getY());
+ y.push_back(C.getOrigin().getY());
+ y.push_back(D.getOrigin().getY());
+
+ r->grasp_map_generator()->SampleMap(grasp_maps_.front());
+ for (auto* are: r->observers()){
+ auto wmd = dynamic_cast<Wing_moveit_decorator*>(are);
+ auto w = dynamic_cast<Wing*>(wmd->wing());
+ pos.setX(w->world_tf().getOrigin().getX());
+ pos.setY(w->world_tf().getOrigin().getY());
+ man_tf.setRotation(w->world_tf().getRotation());
+ man_tf.setOrigin(pos);
+ manipulate_mapspace(man_tf, w->size());
+ map_config.Create(grasp_maps_);
+ middle_pt+= pos;
+ ROS_INFO("X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());
+
+ A = w->world_tf() * w->bounds()[0];
+ B = w->world_tf() * w->bounds()[1];
+ C = w->world_tf() * w->bounds()[2];
+ D = w->world_tf() * w->bounds()[3];
+ x.push_back(A.getOrigin().getX());
+ x.push_back(B.getOrigin().getX());
+ x.push_back(C.getOrigin().getX());
+ x.push_back(D.getOrigin().getX());
+ y.push_back(A.getOrigin().getY());
+ y.push_back(B.getOrigin().getY());
+ y.push_back(C.getOrigin().getY());
+ y.push_back(D.getOrigin().getY());
+ r->grasp_map_generator()->SampleMap(grasp_maps_.front());
+ }
+ }
+ ROS_INFO_STREAM("number of maps: "<< grasp_maps_.size());
+ middle_pt/= grasp_maps_.size();
+ ROS_INFO("X: %f, Y: %f, Z: %f", middle_pt.getX(), middle_pt.getY(), middle_pt.getZ());
+ auto x_min = *std::min_element(x.begin(),x.end());
+ auto x_max = *std::max_element(x.begin(),x.end());
+ auto y_min = *std::min_element(y.begin(),y.end());
+ auto y_max = *std::max_element(y.begin(),y.end());
+ ROS_INFO("width: %f", x_max-x_min);
+ ROS_INFO("height: %f", y_max-y_min);
+
+ visual_tools_->setAlpha(0.1);
+ voxel_manager_->Voxelize();
+
+ auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
+ for(auto&o:cod){
+ voxel_manager_->GetCuboidCollisions(o,voxel_environment_);
+ }
+ voxel_manager_->Voxelize(true);
+
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
// -----------------------------------
// Generate random object to grasp
@@ -376,7 +490,11 @@ bool Moveit_grasp_mediator::planPreApproach(const robot_state::RobotState& goal_
double tolerance_above = 0.01;
double tolerance_below = 0.01;
// req.planner_id = "RRTConnectkConfigDefault";
+<<<<<<< HEAD
req.group_name = "panda_arm1";
+=======
+ req.group_name = "panda_1_arm1";
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
req.num_planning_attempts = 5;
req.allowed_planning_time = 1.5;
moveit_msgs::Constraints goal =
@@ -443,21 +561,31 @@ bool Moveit_grasp_mediator::getIKSolution(const moveit::core::JointModelGroup* a
void Moveit_grasp_mediator::scene_setup(){
for(Abstract_robot* ar: robots_) ar->notify();
visual_tools_->setAlpha(1.0);
- auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
+ auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
+<<<<<<< HEAD
ROS_INFO("%i die cuboids", cbd.size());
ROS_INFO("%i cuboids", cod.size());
+=======
+ ROS_INFO("%i die cuboids", cbd.size());
+ ROS_INFO("%i cuboids", cod.size());
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
for(auto&o:cod){
- o.publishCOwait(visual_tools_,planning_scene_monitor_,rviz_visual_tools::GREY);
- psi_->addCollisionObjects({o.getCoMsg()});
+ o.publishCOwait(visual_tools_,planning_scene_monitor_,rviz_visual_tools::GREY);
+ psi_->addCollisionObjects({o.getCoMsg()});
}
for(auto&o:cbd){
//o.publish(visual_tools_,rviz_visual_tools::GREEN);
+<<<<<<< HEAD
psi_->addCollisionObjects({o.getCoMsg()});
}
+=======
+ psi_->addCollisionObjects({o.getCoMsg()});
+ }
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
visual_tools_->publishRobotState(mgi_->getCurrentState());
visual_tools_->trigger();
@@ -487,6 +615,11 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf
visual_tools_->trigger();
nh_->getParam("/grasps_ns", grasp_ns_);
+<<<<<<< HEAD
+=======
+ grasp_ns_ = "grasps_ns";
+ ROS_INFO("ns is %s", grasp_ns_.c_str());
+>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
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_);
diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index 98fa329f2795583483bf9d34169cbbd70d775c4b..43d9901150650899154103e7b57c1655a962c6b0 100644
--- a/src/impl/moveit_mediator.cpp
+++ b/src/impl/moveit_mediator.cpp
@@ -3,6 +3,7 @@
#include <mutex>
#include <algorithm>
#include <gb_grasp/MapGenerator.h>
+#include <regex>
thread_local moveit::task_constructor::Task task__;
thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_;
@@ -13,17 +14,21 @@ std::mutex task_writing;
void Moveit_mediator::connect_robots(Abstract_robot* robot) {
robots_.push_back(robot);
ROS_INFO("%s connected...", robot->name().c_str());
+
+ Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robot);
+ acm_.insert(std::pair<std::string, std::vector<uint8_t>>(mr->map().at("base").c_str(), std::vector<uint8_t>()));
+ for(auto name: mr->mgi()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
+ for(auto name: mr->mgi_hand()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
}
void Moveit_mediator::publish_tables(){
ros::WallDuration sleep_time(1.0);
for(long unsigned int i = 0; i < robots_.size();i++){
Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]);
- std::stringstream ss;
- ss << "base_" << ceti->name().back();
+ /*
moveit_msgs::CollisionObject table;
- table.id = ss.str();
+ table.id = ceti->map().at("base").c_str();
table.header.frame_id = "world";
table.header.stamp = ros::Time::now();
table.primitives.resize(1);
@@ -41,11 +46,13 @@ void Moveit_mediator::publish_tables(){
table.primitive_poses[0].orientation.z = ceti->tf().getRotation().getZ();
table.primitive_poses[0].orientation.w = ceti->tf().getRotation().getW();
table.operation = table.ADD;
+ */
//psi_->applyCollisionObject(table);
for (Abstract_robot_element* are : ceti->observers()) {
Wing_moveit_decorator* wmd = dynamic_cast<Wing_moveit_decorator*>(are);
psi_->applyCollisionObject(*wmd->markers());
+ acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->markers()->id.c_str(), std::vector<uint8_t>()));
sleep_time.sleep();
}
}
@@ -139,6 +146,9 @@ void Moveit_mediator::setup_task(){
}
}
} else {
+ acm_.insert(std::pair<std::string, std::vector<uint8_t>>("bottle0", std::vector<uint8_t>()));
+ acm_.insert(std::pair<std::string, std::vector<uint8_t>>("bottle1", std::vector<uint8_t>()));
+
moveit_msgs::CollisionObject bottle1;
bottle1.id = "bottle0";
bottle1.header.frame_id = "world";
@@ -232,6 +242,7 @@ void Moveit_mediator::setup_task(){
}
*/
+ for(auto& a: acm_) a.second.resize(acm_.size(), 0);
while (!tasks[0].empty() && !tasks[1].empty()){
for (int i =0; i < std::max(tasks[0].front().solution.sub_trajectory.size(), tasks[1].front().solution.sub_trajectory.size()); i++){
@@ -240,6 +251,9 @@ void Moveit_mediator::setup_task(){
moveit_msgs::PlanningScene* ps1 = nullptr;
moveit_msgs::PlanningScene* ps2 = nullptr;
moveit_msgs::PlanningScene ps_m;
+ std::vector<moveit_msgs::PlanningScene::_allowed_collision_matrix_type> acm_m;
+
+ for(auto entry: acm_) ROS_INFO("%s", entry.first.c_str());
if (i < tasks[0].front().solution.sub_trajectory.size()){
t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
@@ -325,7 +339,6 @@ void Moveit_mediator::setup_task(){
ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff;
-
ROS_INFO("[Robot 2] acm");
ROS_INFO("default entry names");
for (auto den : ps2->allowed_collision_matrix.default_entry_names){
@@ -407,20 +420,141 @@ void Moveit_mediator::setup_task(){
if (t1){
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]);
th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t1, *ps1));
- merge_ps(ps_m, ps1, mr);
-
+ // 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().c_str());
+ std::regex rx_box("bottle*");
+ std::regex rx_table("table*");
+
+ ROS_INFO("start merging");
+ // Iterate task collsion matrix
+ for(int j = 0; j < ps1->allowed_collision_matrix.entry_names.size(); j++ ){
+ // look for relevant fields
+ ROS_INFO("compare %s and %s", mr->map().at("base").c_str(), ps1->allowed_collision_matrix.entry_names[j].c_str());
+ if( std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_box) ||
+ std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_panda_links) ||
+ std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_table) ||
+ mr->map().at("base") == ps1->allowed_collision_matrix.entry_names[j]){
+ // Iterate over values
+ ROS_INFO("true");
+ for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ // filter for irrelevant links of other robots, this would be _link and base_ pattern, which are not relevant
+ ROS_INFO("start filtering");
+
+ if( std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links) ||
+ std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box) ||
+ std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table) ||
+ mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
+
+ std::string relevant_field;
+
+ if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)) relevant_field = match[0].str();
+ if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)) relevant_field = match[0].str();
+ if(std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)) relevant_field = match[0].str();
+ if(mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]) relevant_field = mr->map().at("base").c_str();
+
+ // => Update acm_
+ // calculate the distance in acm for name in in acm message ('cause they may differ, you know)
+ // totally not stolen from SO
+ ROS_INFO("calculate distance");
+ int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
+
+ // better filter with regex
+ // set acm value with respect to the position in acm
+ ROS_INFO("acm überschreiben mit distnace %i", distance);
+ ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps1->allowed_collision_matrix.entry_names[k].c_str());
+
+ for(auto a : acm_.at(mr->map().at("base"))) ROS_INFO("%i", a);
+ ROS_INFO("value %i", acm_.at(mr->map().at("base"))[distance]);
+ acm_.at(mr->map().at(relevant_field))[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps1->allowed_collision_matrix.entry_names[k].c_str());
+ }
+ }
+ }
+ }
+ merge_ps(ps_m, ps1, mr);
}
if (t2){
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[1]);
th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2));
+ std::regex rx_panda("panda_arm([0-9]+)");
+ std::smatch match;
+ std::regex_match(mr->name(), match, rx_panda);
+
+ // build panda link regex
+ std::stringstream ss;
+ ss << "panda_" << match[1] << "_*";
+ std::regex rx_panda_links(ss.str().c_str());
+ std::regex rx_box("bottle*");
+ std::regex rx_table("table*");
+
+ ROS_INFO("start merging");
+ // Iterate task collsion matrix
+ for(int j = 0; j < ps2->allowed_collision_matrix.entry_names.size(); j++ ){
+ // look for relevant fields
+ ROS_INFO("compare %s and %s", mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[j].c_str());
+ if( std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box) ||
+ std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_panda_links) ||
+ std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_table) ||
+ mr->map().at("base") == ps2->allowed_collision_matrix.entry_names[j]){
+ // Iterate over values
+ ROS_INFO("true");
+ for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
+ // filter for irrelevant links of other robots, this would be _link and base_ pattern, which are not relevant
+ ROS_INFO("start filtering");
+
+ if( std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links) ||
+ std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box) ||
+ std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table) ||
+ mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
+
+ std::string relevant_field;
+
+ if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)) relevant_field = match[0].str();
+ if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)) relevant_field = match[0].str();
+ if(std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)) relevant_field = match[0].str();
+ if(mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]) relevant_field = mr->map().at("base").c_str();
+
+ // => Update acm_
+ // calculate the distance in acm for name in in acm message ('cause they may differ, you know)
+ // totally not stolen from SO
+ ROS_INFO("calculate distance");
+ int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
+
+ // better filter with regex
+ // set acm value with respect to the position in acm
+ ROS_INFO("acm überschreiben mit distnace %i", distance);
+ ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[k].c_str());
+
+ for(auto a : acm_.at(mr->map().at("base"))) ROS_INFO("%i", a);
+ ROS_INFO("value %i", acm_.at(mr->map().at("base"))[distance]);
+ acm_.at(mr->map().at(relevant_field))[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
+ ROS_INFO("Updating field %s in acm with %s",mr->map().at("base").c_str(), ps2->allowed_collision_matrix.entry_names[k].c_str());
+ }
+ }
+ }
+ }
merge_ps(ps_m, ps2, mr);
}
+ /*
+ if(!acm_m.empty()){
+ merge_acm(acm_m);
+ }
+ */
+
for(int i = 0; i < th.size(); i++){
th[i].join();
}
+ merge_acm(ps_m);
planning_scene_diff_publisher_->publish(ps_m);
}
@@ -431,6 +565,18 @@ void Moveit_mediator::setup_task(){
}
+void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
+ moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
+ int i = 0;
+ for (auto& a : acm_){
+ acmt.entry_names.push_back(a.first);
+ acmt.entry_values[i].enabled = a.second;
+ i++;
+ }
+ ps_m.allowed_collision_matrix = acmt;
+
+}
+
void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr){
// get full mr link list
std::vector<std::string> links;