Select Git revision
moveit_mediator.cpp
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();