diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index bf168fc51fdcad9ba0f69ab1a01217510adfb5e6..df4c3405384153530cb533ae2d4fe8e16102bfc8 100644 --- a/.catkin_tools/profiles/default/devel_collisions.txt +++ b/.catkin_tools/profiles/default/devel_collisions.txt @@ -1,4 +1,4 @@ /home/matteo/ws_panda/devel/./cmake.lock 42 -/home/matteo/reachability/devel/./cmake.lock 24364 +/home/matteo/reachability/devel/./cmake.lock 24515 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 diff --git a/.catkin_tools/profiles/default/packages/mtc/package.xml b/.catkin_tools/profiles/default/packages/mtc/package.xml index 79cef2f50eeaaf2fe9620c2581950b1cfe090300..adf55f1a8d449da0cd12059fdcb7d3be596edc8a 100644 --- a/.catkin_tools/profiles/default/packages/mtc/package.xml +++ b/.catkin_tools/profiles/default/packages/mtc/package.xml @@ -39,7 +39,6 @@ <build_depend>pcl_ros</build_depend> - <build_depend>yaml_to_mtc</build_depend> <run_depend>filesystem</run_depend> diff --git a/src/mtc/CMakeLists.txt b/src/mtc/CMakeLists.txt index f052aacc56636e26791cd5399f53536172f276c3..ef2e7ca1813757c2a4e1397b3cdc533b8f4f03a4 100644 --- a/src/mtc/CMakeLists.txt +++ b/src/mtc/CMakeLists.txt @@ -37,7 +37,6 @@ find_package(catkin REQUIRED COMPONENTS tf_conversions trajectory_msgs pcl_ros - yaml_to_mtc ) # Catkin diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h index ae5dac0aba3d76bac6b2d049b2daffc84cededb2..d7bf3707f4d88b52cf9bf3a637f12da78ba1cae1 100644 --- a/src/mtc/include/impl/moveit_mediator.h +++ b/src/mtc/include/impl/moveit_mediator.h @@ -72,7 +72,7 @@ class Moveit_mediator : public Abstract_mediator{ void load_robot_description(); void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); void parallel_exec(Abstract_robot& penis, moveit_msgs::CollisionObject& source, std::vector<tf2::Transform>& target); - void create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); + moveit::task_constructor::Task create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;}; diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch index a808d5e4adfc4c5fde0828bbd04ceee86cf38359..e88799b9301dfe7644f674f3d7c29cc54c764ae9 100644 --- a/src/mtc/launch/cell_routine.launch +++ b/src/mtc/launch/cell_routine.launch @@ -8,7 +8,7 @@ <rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" /> <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/> - <rosparam command="load" file="$(find mtc)/descriptions/dummy2.yaml"/> + <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/> <include file="$(find ceti_double)/launch/demo.launch"> diff --git a/src/mtc/package.xml b/src/mtc/package.xml index 79cef2f50eeaaf2fe9620c2581950b1cfe090300..adf55f1a8d449da0cd12059fdcb7d3be596edc8a 100644 --- a/src/mtc/package.xml +++ b/src/mtc/package.xml @@ -39,7 +39,6 @@ <build_depend>pcl_ros</build_depend> - <build_depend>yaml_to_mtc</build_depend> <run_depend>filesystem</run_depend> diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp index 5dfc397ce55ce5ef8922305982c350366c6af718..b9788bd81632f9f5c5f8f80c101b2095b7b8553e 100644 --- a/src/mtc/src/impl/moveit_mediator.cpp +++ b/src/mtc/src/impl/moveit_mediator.cpp @@ -124,9 +124,16 @@ void Moveit_mediator::setup_task(){ for( int i = 0; i < objects_.size();i++){ for(int j =0; j < objects_[i].size();j++){ - create_Task(robots_[i], psi_->getObjects().at("bottle"), objects_[i][j]); + moveit::task_constructor::Task task = create_Task(robots_[i], psi_->getObjects().at("bottle"), objects_[i][j]); + if (task.plan(1)){ + moveit_msgs::MoveItErrorCodes execute_result; + execute_result = task.execute(*task.solutions().front()); + //task_.introspection().publishSolution(*task_.solutions().front()); + } + } } + } else { moveit_msgs::CollisionObject bottle1; bottle1.id = "bottle1"; @@ -182,11 +189,11 @@ void Moveit_mediator::setup_task(){ create_Task(robots_[1], psi_->getObjects().at("bottle2"), objects_[1][j]); } */ - std::thread one(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[0]), std::ref(psi_->getObjects().at("bottle1")), std::ref(objects_[0])); - std::thread two(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[1]), std::ref(psi_->getObjects().at("bottle2")), std::ref(objects_[1])); + //std::thread one(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[0]), std::ref(psi_->getObjects().at("bottle1")), std::ref(objects_[0])); + //std::thread two(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[1]), std::ref(psi_->getObjects().at("bottle2")), std::ref(objects_[1])); - one.join(); - two.join(); + //one.join(); + //two.join(); } } @@ -197,7 +204,7 @@ void Moveit_mediator::parallel_exec(Abstract_robot& ar, moveit_msgs::CollisionOb } } -void Moveit_mediator::create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){ +moveit::task_constructor::Task Moveit_mediator::create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){ tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z)); std::string support_surface1 = "nichts"; std::string support_surface2 = "nichts"; @@ -480,14 +487,7 @@ void Moveit_mediator::create_Task(Abstract_robot* r, moveit_msgs::CollisionObjec task_.add(std::move(stage)); } - //task_map_.at(mr->name()).push_back(std::move(task_)); - - if (task_.plan(1)){ - moveit_msgs::MoveItErrorCodes execute_result; - execute_result = task_.execute(*task_.solutions().front()); - //task_.introspection().publishSolution(*task_.solutions().front()); - } - + return task_; }