Skip to content
Snippets Groups Projects
Select Git revision
  • b313f6352f7807a44f7de755eec05e0440fc905b
  • main default protected
  • mg2bt
  • Part1
4 results

moveit_mediator.cpp

Blame
  • moveit_mediator.cpp 64.24 KiB
    #include "impl/moveit_mediator.h"
    #include <thread>
    #include <mutex> 
    #include <algorithm>
    #include <gb_grasp/MapGenerator.h>
    #include <regex>
    
    #include "behaviortree_cpp_v3/behavior_tree.h"
    #include "behaviortree_cpp_v3/tree_node.h"
    #include "behaviortree_cpp_v3/bt_factory.h"
    #include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
    #include "bt/planning_scene.h"
    #include "bt/trajetory.h"
    #include "bt/execution.h"
    #include "bt/position_condition.h"
    #include "bt/parallel_robot.h"
    #include <actionlib_msgs/GoalStatusArray.h>
    
    
    
    
    thread_local moveit::task_constructor::Task task__;
    thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_;
    
    std::mutex task_writing;
    //std::mutex publish;
    
    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]);
    
    		/*
    		moveit_msgs::CollisionObject table;
    		table.id = ceti->map().at("base").c_str();
    		table.header.frame_id = "world";
    		table.header.stamp = ros::Time::now();
    		table.primitives.resize(1);
    		table.primitives[0].type = table.primitives[0].BOX;
    		table.primitives[0].dimensions.resize(3);
    		table.primitives[0].dimensions[0] = ceti->size().getX();
    		table.primitives[0].dimensions[1] = ceti->size().getY();
    		table.primitives[0].dimensions[2] = ceti->tf().getOrigin().getZ() * 2;
    		table.primitive_poses.resize(1);
    		table.primitive_poses[0].position.x = ceti->tf().getOrigin().getX();
    		table.primitive_poses[0].position.y = ceti->tf().getOrigin().getY();
    		table.primitive_poses[0].position.z = ceti->tf().getOrigin().getZ();
    		table.primitive_poses[0].orientation.x = ceti->tf().getRotation().getX();
    		table.primitive_poses[0].orientation.y = ceti->tf().getRotation().getY();
    		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();