panda_decorator.cpp
Go to the documentation of this file.
2 #include <regex>
3 
4 Panda_decorator::Panda_decorator(std::unique_ptr<Abstract_robot> next)
5  : Abstract_robot_decorator(std::move(next))
6  {
8  ROS_INFO("penis1");
9  // spezifie_robot_groups();
10 
11 
12  }
13 
15  // root
16  next_->robot_root_bounds().clear();
17  next_->robot_root_bounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, 0.095f, 0))); // ++
18  next_->robot_root_bounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, 0.095f, 0))); // +-
19  next_->robot_root_bounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, -0.095f, 0)));
20  next_->robot_root_bounds().push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, -0.095f, 0)));
21 }
22 
24  std::stringstream hand_n, ik_frame_n, name_n, base_n;
25 
26  std::regex panda_id("panda_arm([0-9]+)"), left_finger("left_finger"), right_finger("right_finger"), hand_link("hand_link");
27  std::smatch match;
28  std::regex_match(next_->name(), match, panda_id);
29 
30  hand_n << "hand_" << match[1];
31  ik_frame_n << "panda_" << match[1] << "_link8";
32  base_n << "base_" << match[1];
33 
34  for (auto& link : mgi_hand_->getLinkNames()){
35  if (std::regex_match(link, match, left_finger)) map_.insert(std::pair<std::string, std::string>("left_finger", link));
36  if (std::regex_match(link, match, right_finger)) map_.insert(std::pair<std::string, std::string>("right_finger", link));
37  if (std::regex_match(link, match, hand_link)) map_.insert(std::pair<std::string, std::string>("hand_link", link));
38  }
39 
40  mgi_hand_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(hand_n.str());
41 
42  map_.insert(std::make_pair<std::string, std::string>("eef_name", hand_n.str()));
43  map_.insert(std::make_pair<std::string, std::string>("hand_frame", ik_frame_n.str()));
44  map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str()));
45  map_.insert(std::make_pair<std::string, std::string>("base", base_n.str()));
46 }
Panda_decorator::Panda_decorator
Panda_decorator(std::unique_ptr< Abstract_robot > next)
Definition: panda_decorator.cpp:4
Panda_decorator::spezifie_robot_groups
void spezifie_robot_groups() override
Definition: panda_decorator.cpp:23
Abstract_robot_decorator::map_
std::map< std::string, std::string > map_
Definition: abstract_robot_decorator.h:24
Abstract_robot_decorator::mgi_hand_
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi_hand_
Definition: abstract_robot_decorator.h:22
Panda_decorator::spezifie_root_bounds
void spezifie_root_bounds() override
Definition: panda_decorator.cpp:14
Abstract_robot_decorator::next_
std::unique_ptr< Abstract_robot > next_
Definition: abstract_robot_decorator.h:20
Abstract_robot_decorator
Definition: abstract_robot_decorator.h:18
panda_decorator.h


multi_cell_builder
Author(s): MA
autogenerated on Thu Jan 12 2023 23:45:43