5 , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
"robot_description"))
6 , grasp_pipeline_demo_(std::make_unique<moveit_grasps_demo::GraspPipelineDemo>())
14 ROS_INFO(
"connecting %s...", robot->name().c_str());
15 robots_.insert_or_assign(robot->name(), std::move(robot));
21 for (
const auto& s_r :
robots_){
23 auto ceti_bot =
dynamic_cast<CetiRobot*
>(s_r.second->next());
25 }
catch(
const std::out_of_range& oor) {
26 ROS_INFO(
"No mask data for %s", s_r.first.c_str());
30 for (
const auto& s_r :
robots_){
31 auto ceti_bot =
dynamic_cast<CetiRobot*
>(s_r.second->next());
32 std::vector<std::unique_ptr<AbstractRobotElementDecorator>> panels(3);
34 for (std::size_t j = 0; j < ceti_bot->observerMask().size(); j++){
35 if (ceti_bot->observerMask()[j]){
37 tf2::Transform relative_tf = ceti_bot->tf().inverse() * wd.at(ceti_bot->name()).first[j].pose_;
39 std::unique_ptr<AbstractRobotElement> panel = std::make_unique<MoveitPanel>(wd.at(s_r.first).first[j].name_, relative_tf, wd.at(s_r.first).first[j].size_);
40 std::unique_ptr<AbstractRobotElementDecorator> log = std::make_unique<LogDecorator>(std::move(panel));
41 panels[j] = std::move(log);
42 }
catch (std::out_of_range &oor){
43 ROS_INFO(
"OOR in set_panel");
47 ceti_bot->setObservers(panels);
50 for (
const auto& s_r :
robots_){
58 ros::Duration(2).sleep();
59 std::vector<Cuboid> relevant_box;
60 std::vector<Cuboid> relevant_obstacle;
73 relevant_obstacle.push_back(rob);
77 for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){
78 if(ceti_bot->observerMask()[k]){
79 Cuboid wing(ceti_bot->observers()[k]->name());
80 wing.Pose.position.x = ceti_bot->observers()[k]->worldTf().getOrigin().getX();
81 wing.Pose.position.y = ceti_bot->observers()[k]->worldTf().getOrigin().getY();
82 wing.Pose.position.z = ceti_bot->observers()[k]->worldTf().getOrigin().getZ() - ceti_bot->observers()[k]->size().getZ()/2;
83 wing.Pose.orientation.x = ceti_bot->observers()[k]->worldTf().getRotation().getX();
84 wing.Pose.orientation.y = ceti_bot->observers()[k]->worldTf().getRotation().getY();
85 wing.Pose.orientation.z = ceti_bot->observers()[k]->worldTf().getRotation().getZ();
86 wing.Pose.orientation.w = ceti_bot->observers()[k]->worldTf().getRotation().getW();
87 wing.x_depth = ceti_bot->observers()[k]->size().getX();
88 wing.y_width = ceti_bot->observers()[k]->size().getY();
89 wing.z_heigth = ceti_bot->observers()[k]->size().getZ();
90 relevant_obstacle.push_back(wing);
94 for (
auto& ro :relevant_obstacle){
100 std::bitset<3> temp2 = ceti_bot->observerMask();
101 tf2::Transform pos(tf2::Quaternion(b.Pose.orientation.x, b.Pose.orientation.y, b.Pose.orientation.z, b.Pose.orientation.w), tf2::Vector3(b.Pose.position.x, b.Pose.position.y, b.Pose.position.z));
110 std::bitset<3> temp2 = ceti_bot->observerMask();
111 tf2::Transform pos(tf2::Quaternion(b.Pose.orientation.x, b.Pose.orientation.y, b.Pose.orientation.z, b.Pose.orientation.w), tf2::Vector3(b.Pose.position.x, b.Pose.position.y, b.Pose.position.z));
117 srand(ros::Time::now().toSec());
126 std::ifstream input_file(ros::package::getPath(
"multi_cell_builder") +
"/results/" +
resultPath_);
128 ROS_INFO(
"file not found");
132 std::ofstream output_file(
"input.txt.tmp");
134 ROS_INFO(
"temp file not open");
139 ROS_INFO(
"%s", c.first.c_str());