Skip to content
Snippets Groups Projects
Commit 103319bf authored by KingMaZito's avatar KingMaZito
Browse files

...

parent 43c5be22
Branches
No related tags found
No related merge requests found
...@@ -76,6 +76,8 @@ class Moveit_mediator : public Abstract_mediator{ ...@@ -76,6 +76,8 @@ class Moveit_mediator : public Abstract_mediator{
std::unique_ptr<Cuboid_reader> cuboid_reader_; std::unique_ptr<Cuboid_reader> cuboid_reader_;
std::map<std::string, std::vector<uint8_t>> acm_; std::map<std::string, std::vector<uint8_t>> acm_;
std::map<std::string, std::vector<uint8_t>> rs_;
std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> tasks_; std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> tasks_;
std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>> executions_; std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>> executions_;
......
...@@ -29,10 +29,17 @@ void Moveit_mediator::connect_robots(Abstract_robot* robot) { ...@@ -29,10 +29,17 @@ void Moveit_mediator::connect_robots(Abstract_robot* robot) {
robots_.push_back(robot); robots_.push_back(robot);
ROS_INFO("%s connected...", robot->name().c_str()); ROS_INFO("%s connected...", robot->name().c_str());
Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robot); 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>())); 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()->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>())); for(auto name: mr->mgi_hand()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
for (auto link : mr->mgi()->getLinkNames()) rs_.insert_or_assign(link, std::vector<std::uint8_t>());
rs_.insert_or_assign(mr->map()["left_finger"], std::vector<std::uint8_t>());
rs_.insert_or_assign(mr->map()["right_finger"], std::vector<std::uint8_t>());
rs_.insert_or_assign(mr->map()["hand_link"], std::vector<std::uint8_t>());
rs_.insert_or_assign(mr->map()["base"], std::vector<std::uint8_t>());
} }
void Moveit_mediator::publish_tables(){ void Moveit_mediator::publish_tables(){
...@@ -896,8 +903,8 @@ void Moveit_mediator::task_planner(){ ...@@ -896,8 +903,8 @@ void Moveit_mediator::task_planner(){
for (int i = 0; i < robots_.size(); i++){ for (int i = 0; i < robots_.size(); i++){
if (exec.first == robots_[i]->name()){ if (exec.first == robots_[i]->name()){
auto mr = dynamic_cast<Moveit_robot*>(robots_[i]); auto mr = dynamic_cast<Moveit_robot*>(robots_[i]);
th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), exec.second.first)); //th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), exec.second.first));
//mr->mgi()->execute(exec.second.first); mr->mgi()->execute(exec.second.first);
//manipulate_acm(mr, exec.second.second); //manipulate_acm(mr, exec.second.second);
//merge_ps(ps_m, exec.second.second, mr); //merge_ps(ps_m, exec.second.second, mr);
} }
...@@ -977,40 +984,37 @@ void Moveit_mediator::status_callback(const actionlib_msgs::GoalStatusArray::Con ...@@ -977,40 +984,37 @@ void Moveit_mediator::status_callback(const actionlib_msgs::GoalStatusArray::Con
void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene in, Moveit_robot* mr){ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene in, Moveit_robot* mr){
// get full mr link list // get full mr link list
std::vector<std::string> links;
for (auto link : mr->mgi()->getLinkNames())links.push_back(link);
links.push_back(mr->map()["left_finger"]);
links.push_back(mr->map()["right_finger"]);
links.push_back(mr->map()["hand_link"]);
links.push_back(mr->map()["base"]);
for (auto ao : in.robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao); for (auto ao : in.robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao);
if (in.robot_state.is_diff) out.robot_state.is_diff = in.robot_state.is_diff; out.robot_state.is_diff |= in.robot_state.is_diff;
if (in.is_diff) out.is_diff = in.is_diff; out.is_diff |= in.is_diff;
out.robot_state.joint_state.header = in.robot_state.joint_state.header; out.robot_state.joint_state.header = in.robot_state.joint_state.header;
out.robot_model_name = "panda"; out.robot_model_name = "panda";
std::vector<std::string> links;
for (auto link : mr->mgi()->getLinkNames()) links.push_back(link);
links.push_back(mr->map()["left_finger"]);
links.push_back(mr->map()["right_finger"]);
links.push_back(mr->map()["hand_link"]);
links.push_back(mr->map()["base"]);
for (auto link : links) { for (auto link : links) {
for(int i = 0; i < in.robot_state.joint_state.name.size(); i++){ for(int i = 0; i < in.robot_state.joint_state.name.size(); i++){
if(link.c_str() == in.robot_state.joint_state.name[i].c_str()){ if(link == in.robot_state.joint_state.name[i]){
out.robot_state.joint_state.effort.push_back(in.robot_state.joint_state.effort[i]); out.robot_state.joint_state.effort.push_back(in.robot_state.joint_state.effort[i]);
out.robot_state.joint_state.position.push_back(in.robot_state.joint_state.position[i]); out.robot_state.joint_state.position.push_back(in.robot_state.joint_state.position[i]);
out.robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]); out.robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]);
ROS_INFO("check");
} }
} }
for(int i = 0; i < in.link_padding.size(); i++){ for(int i = 0; i < in.link_padding.size(); i++){
if(link.c_str() == in.link_padding[i].link_name.c_str()){ if(link == in.link_padding[i].link_name){
out.link_padding.push_back(in.link_padding[i]); out.link_padding.push_back(in.link_padding[i]);
} }
} }
for(int i = 0; i < in.link_scale.size(); i++){ for(int i = 0; i < in.link_scale.size(); i++){
if(link.c_str() == in.link_scale[i].link_name.c_str()){ if(link == in.link_scale[i].link_name){
out.link_scale.push_back(in.link_scale[i]); out.link_scale.push_back(in.link_scale[i]);
} }
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment