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

...

parent 3718a979
No related branches found
No related tags found
No related merge requests found
Pipeline #14962 failed
/home/matteo/ws_panda/devel/./cmake.lock 42 /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.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
...@@ -39,7 +39,6 @@ ...@@ -39,7 +39,6 @@
<build_depend>pcl_ros</build_depend> <build_depend>pcl_ros</build_depend>
<build_depend>yaml_to_mtc</build_depend>
<run_depend>filesystem</run_depend> <run_depend>filesystem</run_depend>
......
...@@ -37,7 +37,6 @@ find_package(catkin REQUIRED COMPONENTS ...@@ -37,7 +37,6 @@ find_package(catkin REQUIRED COMPONENTS
tf_conversions tf_conversions
trajectory_msgs trajectory_msgs
pcl_ros pcl_ros
yaml_to_mtc
) )
# Catkin # Catkin
......
...@@ -72,7 +72,7 @@ class Moveit_mediator : public Abstract_mediator{ ...@@ -72,7 +72,7 @@ class Moveit_mediator : public Abstract_mediator{
void load_robot_description(); void load_robot_description();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); 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 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_;}; inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;};
......
...@@ -8,7 +8,7 @@ ...@@ -8,7 +8,7 @@
<rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" /> <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)/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"> <include file="$(find ceti_double)/launch/demo.launch">
......
...@@ -39,7 +39,6 @@ ...@@ -39,7 +39,6 @@
<build_depend>pcl_ros</build_depend> <build_depend>pcl_ros</build_depend>
<build_depend>yaml_to_mtc</build_depend>
<run_depend>filesystem</run_depend> <run_depend>filesystem</run_depend>
......
...@@ -124,9 +124,16 @@ void Moveit_mediator::setup_task(){ ...@@ -124,9 +124,16 @@ void Moveit_mediator::setup_task(){
for( int i = 0; i < objects_.size();i++){ for( int i = 0; i < objects_.size();i++){
for(int j =0; j < objects_[i].size();j++){ 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 { } else {
moveit_msgs::CollisionObject bottle1; moveit_msgs::CollisionObject bottle1;
bottle1.id = "bottle1"; bottle1.id = "bottle1";
...@@ -182,11 +189,11 @@ void Moveit_mediator::setup_task(){ ...@@ -182,11 +189,11 @@ void Moveit_mediator::setup_task(){
create_Task(robots_[1], psi_->getObjects().at("bottle2"), objects_[1][j]); 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 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 two(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[1]), std::ref(psi_->getObjects().at("bottle2")), std::ref(objects_[1]));
one.join(); //one.join();
two.join(); //two.join();
} }
} }
...@@ -197,7 +204,7 @@ void Moveit_mediator::parallel_exec(Abstract_robot& ar, moveit_msgs::CollisionOb ...@@ -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)); 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_surface1 = "nichts";
std::string support_surface2 = "nichts"; std::string support_surface2 = "nichts";
...@@ -480,14 +487,7 @@ void Moveit_mediator::create_Task(Abstract_robot* r, moveit_msgs::CollisionObjec ...@@ -480,14 +487,7 @@ void Moveit_mediator::create_Task(Abstract_robot* r, moveit_msgs::CollisionObjec
task_.add(std::move(stage)); task_.add(std::move(stage));
} }
//task_map_.at(mr->name()).push_back(std::move(task_)); return task_;
if (task_.plan(1)){
moveit_msgs::MoveItErrorCodes execute_result;
execute_result = task_.execute(*task_.solutions().front());
//task_.introspection().publishSolution(*task_.solutions().front());
}
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment