5 , pub_(std::make_unique<ros::Publisher>(nh->advertise< visualization_msgs::MarkerArray >(
"visualization_marker_array", 1))){
9 if(std::filesystem::exists(ros::package::getPath(
"multi_cell_builder") +
"/results/" +
filename_)) std::filesystem::remove_all(ros::package::getPath(
"multi_cell_builder") +
"/results/" +
filename_);
10 std::filesystem::create_directory(ros::package::getPath(
"multi_cell_builder") +
"/results/" +
filename_);
15 std::vector<pcl::PointXYZ> ground_plane;
16 for (
float x = origin.getX() - diameter * 1.5; x <= origin.getX() + diameter * 1.5; x += resolution){
17 for (
float y = origin.getY() - diameter * 1.5; y <= origin.getY() + diameter * 1.5; y += resolution){
18 ground_plane.push_back(pcl::PointXYZ(x,y, s_r.second->tf().getOrigin().getZ() - 0.0025f));
21 grounds_.insert(std::pair<
const std::string, std::vector<pcl::PointXYZ>>(s_r.first, ground_plane));
26 ROS_INFO(
"connecting %s...", robot->name().c_str());
27 robots_.insert_or_assign(robot->name(), std::move(robot));
33 for (
const auto& s_r :
robots_){
35 auto ceti_bot =
dynamic_cast<CetiRobot*
>(s_r.second->next());
37 }
catch(
const std::out_of_range& oor) {
38 ROS_INFO(
"No mask data for %s", s_r.first.c_str());
42 for (
const auto& s_r :
robots_){
43 auto ceti_bot =
dynamic_cast<CetiRobot*
>(s_r.second->next());
44 std::vector<std::unique_ptr<AbstractRobotElementDecorator>> panels(3);
46 for (std::size_t j = 0; j < ceti_bot->observerMask().size(); j++){
47 if (ceti_bot->observerMask()[j]){
49 tf2::Transform relative_tf = ceti_bot->tf().inverse() * wd.at(ceti_bot->name()).first[j].pose_;
51 std::unique_ptr<AbstractRobotElement> panel = std::make_unique<RvizPanel>(wd.at(s_r.first).first[j].name_, relative_tf, wd.at(s_r.first).first[j].size_);
52 std::unique_ptr<AbstractRobotElementDecorator> log = std::make_unique<LogDecorator>(std::move(panel));
53 panels[j] = std::move(log);
54 }
catch (std::out_of_range &oor){
55 ROS_INFO(
"OOR in set_panel");
59 ceti_bot->setObservers(panels);
68 ns <<
"Targets " << robot;
74 }
catch (std::out_of_range& oor){}
77 std::map<std::string, int> workload_map;
81 visualization_msgs::MarkerArray ma;
82 for(
int i = 0; i < ts.size(); i++){
83 visualization_msgs::Marker marker;
84 marker.header.frame_id =
"map";
85 marker.header.stamp = ros::Time();
88 marker.type = visualization_msgs::Marker::CUBE;
89 marker.action = visualization_msgs::Marker::ADD;
90 marker.pose.position.x = ts[i].pose_.getOrigin().getX();
91 marker.pose.position.y = ts[i].pose_.getOrigin().getY();
92 marker.pose.position.z = ts[i].pose_.getOrigin().getZ();
93 marker.pose.orientation.x = ts[i].pose_.getRotation().getX();
94 marker.pose.orientation.y = ts[i].pose_.getRotation().getY();
95 marker.pose.orientation.z = ts[i].pose_.getRotation().getZ();
96 marker.pose.orientation.w = ts[i].pose_.getRotation().getW();
97 marker.scale.x = ts[i].size_.getX();
98 marker.scale.y = ts[i].size_.getY();
99 marker.scale.z = 0.1f;
104 workload_map.at(str)++;
105 }
catch (std::out_of_range& oor) {
106 ROS_INFO(
"Support surface not in map");
107 workload_map.insert(std::pair<std::string,int>(str,1));
110 marker.color.g = 1.0;
117 marker.color.a = 1.0;
121 ma.markers.push_back(marker);
125 workload = (workload_map.size() <= panel_mask.count()+1) && succ;
135 std::vector<std::vector<std::string>> wcs;
159 wcs.push_back(std::vector<std::string>{
"panda_arm1",
"panda_arm2"});
164 std::vector<std::map<const std::string, std::vector<tf2::Transform>>> workcells_with_bases;
167 for(
auto& workcell: wcs){
168 std::map<const std::string, std::vector<tf2::Transform>> ground_projection;
169 for (
auto& robot: workcell){
170 std::vector<tf2::Transform> ground_per_robot;
171 pcl::octree::OctreePointCloudSearch< pcl::PointXYZ >cloud(0.4f);
174 cloud.addPointsFromInputCloud();
176 for(pcl::PointXYZ& p :
grounds_.at(robot)){
177 double min_x, min_y, min_z, max_x, max_y, max_z;
178 cloud.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
179 bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);
180 if (isInBox && cloud.isVoxelOccupiedAtPoint(p)) {
181 std::vector< int > pointIdxVec;
182 if (cloud.voxelSearch(p, pointIdxVec)) ground_per_robot.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(p.x, p.y, p.z)));
185 }
catch (std::out_of_range& oor) {
186 ROS_INFO(
"No result for robot");
189 ground_projection.insert(std::pair<
const std::string, std::vector<tf2::Transform>>(robot,ground_per_robot));
191 workcells_with_bases.push_back(ground_projection);
194 ROS_INFO(
"assigne result to first robot...");
195 for(
auto& wc: workcells_with_bases){
207 std::vector<std::vector<protobuf_entry>> all_solutions_per_workcell;
209 visualization_msgs::MarkerArray ma;
210 for (
auto& s_pos : workcell){
214 }
catch(std::out_of_range& oor){}
216 visualization_msgs::Marker m;
217 m.header.frame_id =
"map";
218 m.header.stamp = ros::Time();
219 m.ns = ceti1->
name();
221 m.type = visualization_msgs::Marker::CUBE;
222 m.action = visualization_msgs::Marker::ADD;
223 m.pose.position.x = ceti1->
tf().getOrigin().getX();
224 m.pose.position.y = ceti1->
tf().getOrigin().getY();
225 m.pose.position.z = ceti1->
tf().getOrigin().getZ();
226 m.pose.orientation.x = ceti1->
tf().getRotation().getX();
227 m.pose.orientation.y = ceti1->
tf().getRotation().getY();
228 m.pose.orientation.z = ceti1->
tf().getRotation().getZ();
229 m.pose.orientation.w = ceti1->
tf().getRotation().getW();
231 m.scale.x = ceti1->
size().getX();
232 m.scale.y = ceti1->
size().getY();
233 m.scale.z = ceti1->
tf().getOrigin().getZ()*2;
239 for (std::size_t k = 0; k < ceti1->
observerMask().size(); k++){
242 ma.markers.push_back(wrd->marker());
247 ma.markers.push_back(m);
251 for(
int j = 5; j <= 7; j++){
253 for (
long unsigned int i = 0; i < s_pos.second.size(); i++){
254 ceti1->
setTf(s_pos.second[i]);
255 for (
float p = 0; p < 2*M_PI; p += 0.0872665f){
256 ceti1->
rotate(0.0872665f);
258 m.action = visualization_msgs::Marker::MODIFY;
259 m.pose.position.x = ceti1->
tf().getOrigin().getX();
260 m.pose.position.y = ceti1->
tf().getOrigin().getY();
261 m.pose.position.z = ceti1->
tf().getOrigin().getZ();
262 m.pose.orientation.x = ceti1->
tf().getRotation().getX();
263 m.pose.orientation.y = ceti1->
tf().getRotation().getY();
264 m.pose.orientation.z = ceti1->
tf().getRotation().getZ();
265 m.pose.orientation.w = ceti1->
tf().getRotation().getW();
266 ma.markers.push_back(m);
268 for (std::size_t k = 0; k < ceti1->
observerMask().size(); k++){
271 wrd->
marker().color.a = 1;
272 ma.markers.push_back(wrd->marker());
275 wrd->
marker().color.a = 0;
276 ma.markers.push_back(wrd->marker());
285 if (workload && target_collisions){
286 std::vector<protobuf_entry> wc_solution;
289 if (workcell.size() == wc_solution.size()){
290 all_solutions_per_workcell.push_back(wc_solution);
296 m.action = visualization_msgs::Marker::DELETEALL;
297 ma.markers.push_back(m);
300 protobuf_.push_back(all_solutions_per_workcell);
307 std::vector<tf2::Transform> access_map;
308 float padding = 0.0025f;
312 if (!(workcell.size() == wc_solution.size())){
313 for (
auto& r: workcell){
314 for (
auto& s: wc_solution){
315 if (!(r.first == s.name_)){
327 ROS_INFO(
"%i", ts.size());
328 for (
auto& r: wc_solution){
330 for (
auto it = ts.begin(); it != ts.end();){
331 if(tf2::tf2Distance2(it->pose_.getOrigin(), vec.pose_.getOrigin()) == 0) {
340 for (
auto& r : wc_solution){
345 for (
int i = 0; i <= 2; i++){
346 for (
int j = 0; j <= 2; j++){
347 if(i == 0 && j == 0) {
continue;}
348 if(i == 2 && j == 2) {
continue;}
350 access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,ceti->
size().getY()*j + padding,0)));
351 access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-ceti->
size().getY()*j + padding,0)));
353 access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(ceti->
size().getX()*i + padding,0,0)));
354 access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-ceti->
size().getX()*i + padding,0,0)));
356 access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(ceti->
size().getX()*i+padding,ceti->
size().getY()*j +padding,0)));
357 access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-ceti->
size().getX()*i+padding,ceti->
size().getY()*j +padding,0)));
358 access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(ceti->
size().getX()*i+padding,-ceti->
size().getY()*j +padding,0)));
359 access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-ceti->
size().getX()*i+padding,-ceti->
size().getY()*j+padding,0)));
365 std::bitset<3> actual_config(r.wing_config_);
366 for(std::size_t i = 0; i < ceti->
observers().size(); i++){
367 if(actual_config[i]){
368 access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.5f * ceti->
size().getX() + ceti->
observers()[i]->size().getX() + 2*padding + next_ceti->
size().getX()*0.5f,0,0)));
369 access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.5f * ceti->
size().getX() - ceti->
observers()[i]->size().getX() - 2*padding - next_ceti->
size().getX()*0.5f,0,0)));
370 access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.5f*ceti->
size().getY() + ceti->
observers()[i]->size().getY() + 2*padding + next_ceti->
size().getY()*0.5f ,0)));
371 access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.5f*ceti->
size().getY() - ceti->
observers()[i]->size().getY() - 2*padding - next_ceti->
size().getY()*0.5f,0)));
375 for(
auto& am: access_map){
381 visualization_msgs::MarkerArray ma;
382 visualization_msgs::Marker m;
383 m.header.frame_id =
"map";
384 m.header.stamp = ros::Time();
385 m.ns = next_ceti->
name();
387 m.type = visualization_msgs::Marker::CUBE;
388 m.action = visualization_msgs::Marker::ADD;
389 m.pose.position.x = next_ceti->
tf().getOrigin().getX();
390 m.pose.position.y = next_ceti->
tf().getOrigin().getY();
391 m.pose.position.z = next_ceti->
tf().getOrigin().getZ();
392 m.pose.orientation.x = next_ceti->
tf().getRotation().getX();
393 m.pose.orientation.y = next_ceti->
tf().getRotation().getY();
394 m.pose.orientation.z = next_ceti->
tf().getRotation().getZ();
395 m.pose.orientation.w = next_ceti->
tf().getRotation().getW();
397 m.scale.x = next_ceti->
size().getX();
398 m.scale.y = next_ceti->
size().getY();
399 m.scale.z = next_ceti->
tf().getOrigin().getZ()*2;
405 for (std::size_t k = 0; k < next_ceti->
observerMask().size(); k++){
408 ma.markers.push_back(wrd->marker());
412 ma.markers.push_back(m);
416 for(
int j = 0; j <= 7; j++){
418 for (
long unsigned int i = 0; i < access_map.size(); i++){
419 next_ceti->
setTf(access_map[i]);
420 for (
float p = 0; p < 2*M_PI; p += M_PI/2){
421 next_ceti->
rotate(M_PI/2);
423 m.action = visualization_msgs::Marker::MODIFY;
424 m.pose.position.x = next_ceti->
tf().getOrigin().getX();
425 m.pose.position.y = next_ceti->
tf().getOrigin().getY();
426 m.pose.position.z = next_ceti->
tf().getOrigin().getZ();
427 m.pose.orientation.x = next_ceti->
tf().getRotation().getX();
428 m.pose.orientation.y = next_ceti->
tf().getRotation().getY();
429 m.pose.orientation.z = next_ceti->
tf().getRotation().getZ();
430 m.pose.orientation.w = next_ceti->
tf().getRotation().getW();
431 ma.markers.push_back(m);
433 for (std::size_t k = 0; k < next_ceti->
observerMask().size(); k++){
436 wrd->
marker().color.a = 1;
437 ma.markers.push_back(wrd->marker());
440 wrd->
marker().color.a = 0;
441 ma.markers.push_back(wrd->marker());
449 if(workload && target_collisions) {
450 wc_solution.push_back({next_ceti->
name(), next_ceti->
tf(), j});
452 wc_solution.pop_back();
454 if ( workcell.size() == wc_solution.size()) {
463 m.action = visualization_msgs::Marker::DELETEALL;
464 ma.markers.push_back(m);
470 for(
auto& r1 : wc_solution){
471 std::vector<tf2::Transform> bounds;
472 for(
auto& r2 : wc_solution){
473 if (r1.name_ == r2.name_)
continue;
475 bounds.push_back(r2.tf_);
477 robot2->
setTf(r2.tf_);
480 bounds.push_back(r2.tf_ * robot2->rootTf() * robot2->robotRootBounds()[0]);
481 bounds.push_back(r2.tf_ * robot2->rootTf() * robot2->robotRootBounds()[1]);
482 bounds.push_back(r2.tf_ * robot2->rootTf() * robot2->robotRootBounds()[2]);
483 bounds.push_back(r2.tf_ * robot2->rootTf() * robot2->robotRootBounds()[3]);
485 bounds.push_back(r2.tf_ * robot2->bounds()[0]);
486 bounds.push_back(r2.tf_ * robot2->bounds()[1]);
487 bounds.push_back(r2.tf_ * robot2->bounds()[2]);
488 bounds.push_back(r2.tf_ * robot2->bounds()[3]);
490 std::bitset<3> panel_mask(r2.wing_config_);
491 for(std::size_t i = 0; i < robot2->observers().size(); i++){
492 if(robot2->observerMask()[i] & panel_mask[i]){
493 bounds.push_back(robot2->observers()[i]->worldTf());
494 bounds.push_back(robot2->observers()[i]->worldTf() * robot2->observers()[i]->bounds()[0]);
495 bounds.push_back(robot2->observers()[i]->worldTf() * robot2->observers()[i]->bounds()[1]);
496 bounds.push_back(robot2->observers()[i]->worldTf() * robot2->observers()[i]->bounds()[2]);
497 bounds.push_back(robot2->observers()[i]->worldTf() * robot2->observers()[i]->bounds()[3]);
503 robot1->
setTf(r1.tf_);
506 std::bitset<3> panel_mask(r1.wing_config_);
508 for(
int i = 0; i < bounds.size(); i++){
509 if (robot1->checkSingleObjectCollision(bounds[i], str, panel_mask)) {
557 std::string resultFile = std::to_string(
static_cast<int>(ros::Time::now().toNSec()));
558 std::filesystem::create_directory(ros::package::getPath(
"multi_cell_builder") +
"/results/" +
filename_ +
"/" + resultFile);
559 std::filesystem::create_directory(ros::package::getPath(
"multi_cell_builder") +
"/results/" +
filename_ +
"/" + resultFile +
"/configs");
560 std::filesystem::create_directory(ros::package::getPath(
"multi_cell_builder") +
"/results/" +
filename_ +
"/" + resultFile +
"/launch");
562 std::stringstream ss;
563 std::stringstream panel_ss;
564 std::stringstream root_ss;
565 std::stringstream box_ss;
569 box_ss <<
"{ 'id': '" << box.Name <<
"', 'type': 'BOX', 'pos': { 'x': "<<box.Pose.position.x<<
", 'y': "<<box.Pose.position.y<<
", 'z': "<< box.Pose.position.z<<
" },'size': { 'length': "<< box.x_depth<<
", 'width': "<<box.y_width<<
", 'height': "<< box.z_heigth<<
" },'orientation': { 'x':"<< box.Pose.orientation.x <<
", 'y':" << box.Pose.orientation.y<<
", 'z':" << box.Pose.orientation.z <<
", 'w':" << box.Pose.orientation.w<<
"},'color': { 'b': 1 } }, \n";
573 box_ss <<
"{ 'id': '" << box.Name <<
"', 'type': 'BIN', 'pos': { 'x': "<<box.Pose.position.x<<
", 'y': "<<box.Pose.position.y<<
", 'z': "<< box.Pose.position.z<<
" },'size': { 'length': "<< box.x_depth<<
", 'width': "<<box.y_width<<
", 'height': "<< box.z_heigth<<
" },'orientation': { 'x':"<< box.Pose.orientation.x <<
", 'y':" << box.Pose.orientation.y<<
", 'z':" << box.Pose.orientation.z <<
", 'w':" << box.Pose.orientation.w<<
"},'color': { 'b': 1 } }, \n";
576 ss <<
"{ 'objects' : [ \n";
577 for (
int c = 0; c < wc_solution.size(); c++){
579 std::regex rx(
"panda_arm([0-9]+)");
581 std::regex_match(wc_solution[c].name_, match, rx);
583 auto ceti =
dynamic_cast<CetiRobot*
>(
robots_.at(wc_solution[c].name_)->next());
584 ceti->
setTf(wc_solution[c].tf_);
588 tf2::Matrix3x3 m(wc_solution[c].tf_.getRotation());
591 float size_x = ceti->size().getX();
592 float size_y = ceti->size().getY();
593 float size_z = ceti->size().getZ();
595 float pos_x = ceti->tf().getOrigin().getX();
596 float pos_y = ceti->tf().getOrigin().getY();
597 float pos_z = ceti->tf().getOrigin().getZ() *2 ;
598 float rot_x = ceti->tf().getRotation().getX();
599 float rot_y = ceti->tf().getRotation().getY();
600 float rot_z = ceti->tf().getRotation().getZ();
601 float rot_w = ceti->tf().getRotation().getW();
604 ss <<
"{ 'id' : 'table" << match[1] <<
"_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
605 ss <<
"{ 'id' : 'table" << match[1] <<
"_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},\n";
606 ss <<
"{ 'id' : 'table" << match[1] <<
"_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
607 ss <<
"{ 'id' : 'table" << match[1] <<
"_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
608 ss <<
"{ 'id' : 'table" << match[1] <<
"_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
609 ss <<
"{ 'id' : 'table" << match[1] <<
"_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
610 ss <<
"{ 'id' : 'table" << match[1] <<
"_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
611 ss <<
"{ 'id' : 'table" << match[1] <<
"_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
612 ss <<
"{ 'id' : 'table" << match[1] <<
"_table_top', 'pos': { 'x': " << std::to_string(pos_x) <<
" , 'y': "<< std::to_string(pos_y) <<
" , 'z': "<< std::to_string(pos_z) <<
" },'size': { 'length': "<< std::to_string(size_x) <<
" ,'width': "<< std::to_string(size_y) <<
" ,'height': "<< std::to_string(size_z) <<
" },'orientation': { 'x': " << std::to_string(rot_x) <<
" , 'y': " << std::to_string(rot_y) <<
" , 'z': " << std::to_string(rot_z) <<
" , 'w': " << std::to_string(rot_w) <<
" },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': " << std::to_string(r) <<
" , 'p': " << std::to_string(p) <<
" , 'y': " << std::to_string(y) <<
" } },\n";
615 std::stringstream config;
616 config <<
"mapspace: {'dim': [";
617 config << size_x/2 <<
", ";
618 config << size_y/2 <<
", 0.04], ";
619 config <<
"'pos': [";
620 config << pos_x <<
", ";
621 config << pos_y <<
", ";
622 config << (pos_z + size_z/2.0f + 0.04f/2.0f) <<
"], ";
623 config <<
"'rot': [";
624 config << r <<
", " << p <<
", " << y <<
"], ";
625 config <<
"'verbose': false }\n";
626 config <<
"voxel_size: 0.02 \n";
627 config <<
"voxel_space: {'dim': [";
628 config << (size_x + 0.1f)/2 <<
", ";
629 config << (size_y + 0.1f)/2 <<
", 0.2], ";
630 config <<
"'pos': [";
631 config << pos_x <<
", ";
632 config << pos_y <<
", ";
633 config << (pos_z + size_z/2.0f + 0.2f/2.0f) <<
"], ";
634 config <<
"'rot': [";
635 config << r <<
", " << p <<
", " << y <<
"]}\n";
636 config <<
"voxel_verbose_level: 0\n";
637 config <<
"translation_rate: 0.03\n";
638 config <<
"rotation_rate: 45.0\n";
639 config <<
"rotation_max: 90.0\n";
640 config <<
"clearance: 0.01\n";
641 config <<
"min_quality: 0.1\n";
642 config <<
"max_candidate_count: 10\n";
643 config <<
"allow_dependencies: true\n";
644 config <<
"top_grasps_only: true\n";
646 std::ofstream o(ros::package::getPath(
"multi_cell_builder") +
"/results/"+
filename_ +
"/" + resultFile +
"/configs/" + resultFile +
"_"+ wc_solution[c].name_+
".yaml");
651 std::bitset<3> panel_mask(wc_solution[c].wing_config_);
652 for(std::size_t i = 0; i < ceti->observers().size(); i++){
653 if(ceti->observerMask()[i] & panel_mask[i]){
654 float x = ceti->observers()[i]->worldTf().getOrigin().getX();
655 float y = ceti->observers()[i]->worldTf().getOrigin().getY();
656 float z = ceti->observers()[i]->worldTf().getOrigin().getZ();
657 float qx = ceti->observers()[i]->worldTf().getRotation().getX();
658 float qy = ceti->observers()[i]->worldTf().getRotation().getY();
659 float qz = ceti->observers()[i]->worldTf().getRotation().getZ();
660 float qw = ceti->observers()[i]->worldTf().getRotation().getW();
662 float length = ceti->observers()[i]->size().getX();
663 float width = ceti->observers()[i]->size().getY();
664 float height = ceti->observers()[i]->size().getZ();
665 panel_ss <<
"{ 'id': '" << ceti->observers()[i]->name() <<
"' , 'pos': { 'x': "<< std::to_string(x) <<
" , 'y': "<< std::to_string(y) <<
" , 'z': "<< std::to_string(z - 0.25*height) <<
" } , 'size': { 'length': "<< std::to_string(length) <<
" , 'width': "<< std::to_string(width) <<
" , 'height': "<< std::to_string(height) <<
" } , 'orientation': { 'x': "<< std::to_string(qx) <<
" , 'y': "<< std::to_string(qy) <<
" , 'z': "<< std::to_string(qz) <<
" , 'w': "<< std::to_string(qw) <<
" } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n";
669 tf2::Transform tf_arm = wc_solution[c].tf_ * ceti->rootTf();
670 float arm_x = tf_arm.getOrigin().getX();
671 float arm_y = tf_arm.getOrigin().getY();
672 float arm_z = tf_arm.getOrigin().getZ();
673 float arm_qx = tf_arm.getRotation().getX();
674 float arm_qy = tf_arm.getRotation().getY();
675 float arm_qz = tf_arm.getRotation().getZ();
676 float arm_qw = tf_arm.getRotation().getW();
678 if (!(c == wc_solution.size())){
680 root_ss <<
"{ 'id': 'arm" << match[1] <<
"','type': 'ARM','pos': { 'x': " << std::to_string(arm_x) <<
", 'y': " << std::to_string(arm_y) <<
", 'z': 0.89 },'size': { },'orientation': { 'x': " << std::to_string(arm_qx) <<
", 'y': " << std::to_string(arm_qy) <<
", 'z': " << std::to_string(arm_qz) <<
", 'w': " << std::to_string(arm_qw) <<
" },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, \n";
682 root_ss <<
"{ 'id': 'arm" << match[1] <<
"','type': 'ARM','pos': { 'x': " << std::to_string(arm_x) <<
", 'y': " << std::to_string(arm_y) <<
", 'z': 0.89 },'size': { },'orientation': { 'x': " << std::to_string(arm_qx) <<
", 'y': " << std::to_string(arm_qy) <<
", 'z': " << std::to_string(arm_qz) <<
", 'w': " << std::to_string(arm_qw) <<
" },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }";
686 std::stringstream launch;
687 launch <<
"<launch>\n";
688 launch <<
"<arg name=\"referenceRobot\" default=\""<< wc_solution[c].name_ <<
"\" />\n";
689 launch <<
"<arg name=\"referenceXYZ\" default=\""<< arm_x <<
" "<< arm_y <<
" " << arm_z <<
"\"/>\n";
690 launch <<
"<arg name=\"referenceRPY\" default=\"" << r <<
" " << p <<
" " << y <<
"\"/>\n";
691 launch <<
"<arg name=\"result\" default=\"" <<
filename_ <<
"/" << resultFile <<
"/" << resultFile <<
".yaml\" />\n";
692 launch <<
"<rosparam command=\"load\" file=\"$(find multi_cell_builder)/results/$(arg result)\"/>\n";
693 launch <<
"<rosparam param=\"referenceRobot\" subst_value=\"True\"> $(arg referenceRobot)</rosparam>\n";
694 launch <<
"<rosparam param=\"resultPath\" subst_value=\"True\"> $(arg result)</rosparam>\n";
695 launch <<
"<arg name=\"pipeline\" default=\"ompl\"/>\n";
696 launch <<
"<arg name=\"db\" default=\"false\"/>\n";
697 launch <<
"<arg name=\"db_path\" default=\"$(find panda_moveit_config)/default_warehouse_mongo_db\"/>\n";
698 launch <<
"<arg name=\"debug\" default=\"false\" />\n";
699 launch <<
"<arg name=\"load_robot_description\" default=\"true\"/>\n";
700 launch <<
"<arg name=\"moveit_controller_manager\" default=\"fake\"/>\n";
701 launch <<
"<arg name=\"fake_execution_type\" default=\"interpolate\"/>\n";
702 launch <<
"<arg name=\"use_gui\" default=\"false\"/>\n";
703 launch <<
"<arg name=\"use_rviz\" default=\"true\"/>\n";
704 launch <<
"<node pkg=\"tf2_ros\" type=\"static_transform_publisher\" name=\"virtual_joint_broadcaster_0\" args=\"0 0 0 0 0 0 world panda_link0\"/>\n";
705 launch <<
"<group if=\"$(eval arg('moveit_controller_manager') == 'fake')\">\n";
706 launch <<
"<node name=\"joint_state_publisher\" pkg=\"joint_state_publisher\" type=\"joint_state_publisher\" unless=\"$(arg use_gui)\">\n";
707 launch <<
"<rosparam param=\"source_list\">[move_group/fake_controller_joint_states]</rosparam>\n";
708 launch <<
"</node>\n";
709 launch <<
"<node name=\"joint_state_publisher\" pkg=\"joint_state_publisher_gui\" type=\"joint_state_publisher_gui\" if=\"$(arg use_gui)\">\n";
710 launch <<
"<rosparam param=\"source_list\">[move_group/fake_controller_joint_states]</rosparam>\n";
711 launch <<
"</node>\n";
712 launch <<
"<node name=\"robot_state_publisher\" pkg=\"robot_state_publisher\" type=\"robot_state_publisher\" respawn=\"true\" output=\"screen\"/>\n";
713 launch <<
"</group>\n";
714 launch <<
"<include file=\"$(find panda_moveit_config)/launch/move_group.launch\">\n";
715 launch <<
"<arg name=\"allow_trajectory_execution\" value=\"true\"/>\n";
716 launch <<
"<arg name=\"moveit_controller_manager\" value=\"$(arg moveit_controller_manager)\"/>\n";
717 launch <<
"<arg name=\"fake_execution_type\" value=\"$(arg fake_execution_type)\"/>\n";
718 launch <<
"<arg name=\"info\" value=\"true\"/>\n";
719 launch <<
"<arg name=\"debug\" value=\"$(arg debug)\"/>\n";
720 launch <<
"<arg name=\"pipeline\" value=\"$(arg pipeline)\"/>\n";
721 launch <<
"<arg name=\"load_robot_description\" value=\"$(arg load_robot_description)\"/>\n";
722 launch <<
"<arg name=\"referenceXYZ\" value=\"$(arg referenceXYZ)\"/>\n";
723 launch <<
"<arg name=\"referenceRPY\" value=\"$(arg referenceRPY)\"/>\n";
724 launch <<
"</include>\n";
726 launch <<
"<include file=\"$(find panda_moveit_config)/launch/default_warehouse_db.launch\" if=\"$(arg db)\">\n";
727 launch <<
"<arg name=\"moveit_warehouse_database_path\" value=\"$(arg db_path)\"/>\n";
728 launch <<
"</include>\n";
730 launch <<
"<node name=\"$(anon rviz)\" pkg=\"rviz\" type=\"rviz\" respawn=\"false\" args=\"-d $(find moveit_grasps)/launch/rviz/grasps.rviz\" output=\"screen\">\n";
731 launch <<
"<rosparam command=\"load\" file=\"$(find panda_moveit_config)/config/kinematics.yaml\"/>\n";
732 launch <<
"</node>\n";
734 launch <<
"<node name=\"config_routine\" pkg=\"multi_cell_builder\" type=\"config_routine\" output=\"screen\">\n";
735 launch <<
"<param name=\"gripper\" value=\"two_finger\"/>\n";
736 launch <<
"<param name=\"ee_group_name\" value=\"hand\"/>\n";
737 launch <<
"<param name=\"planning_group_name\" value=\"panda_arm_hand\" />\n";
738 launch <<
"<rosparam command=\"load\" file=\"$(find gb_grasp)/config_robot/panda_grasp_data.yaml\"/>\n";
739 launch <<
"<rosparam command=\"load\" file=\"$(find gb_grasp)/config/moveit_grasps_config.yaml\"/>\n";
740 launch <<
"<rosparam command=\"load\" file=\"$(find multi_cell_builder)/results/" <<
filename_ <<
"/" << resultFile <<
"/configs/" << resultFile <<
"_" << wc_solution[c].name_ <<
".yaml\"/>\n";
741 launch <<
"</node>\n";
743 launch <<
"<arg name=\"planner\" default=\"ompl\" />\n";
744 launch <<
"<include ns=\"moveit_grasps_demo\" file=\"$(find panda_moveit_config)/launch/planning_pipeline.launch.xml\">\n";
745 launch <<
"<arg name=\"pipeline\" value=\"$(arg planner)\" />\n";
746 launch <<
"</include>\n";
747 launch <<
"</launch>\n";
749 std::ofstream l(ros::package::getPath(
"multi_cell_builder") +
"/results/"+
filename_ +
"/" + resultFile +
"/launch/" + wc_solution[c].name_+
"_" + resultFile +
".launch");
754 }
catch (std::out_of_range& oor) {
761 std::ofstream o(ros::package::getPath(
"multi_cell_builder") +
"/results/"+
filename_ +
"/" + resultFile +
"/" + resultFile +
".yaml");
762 ss << panel_ss.str();
771 visualization_msgs::MarkerArray ma;
772 visualization_msgs::Marker marker;
773 marker.header.frame_id =
"map";
774 marker.header.stamp = ros::Time();
775 marker.ns = ar->
name();
777 marker.type = visualization_msgs::Marker::CUBE;
778 marker.action = visualization_msgs::Marker::MODIFY;
779 marker.pose.position.x = ar->
tf().getOrigin().getX();
780 marker.pose.position.y = ar->
tf().getOrigin().getY();
781 marker.pose.position.z = ar->
tf().getOrigin().getZ();
782 marker.pose.orientation.x = ar->
tf().getRotation().getX();
783 marker.pose.orientation.y = ar->
tf().getRotation().getY();
784 marker.pose.orientation.z = ar->
tf().getRotation().getZ();
785 marker.pose.orientation.w = ar->
tf().getRotation().getW();
786 marker.scale.x = ar->
size().getX();
787 marker.scale.y = ar->
size().getY();
788 marker.scale.z = ar->
size().getZ();
789 marker.color.r = 1.0;
790 marker.color.g = 1.0;
791 marker.color.b = 1.0;
792 marker.color.a = 1.0;
793 for (
int i = 0; i < ar->
observers().size(); i++){
795 ma.markers.push_back(wrd->marker());