4 #include <tf2/LinearMath/Scalar.h>
9 visualization_msgs::MarkerArray ma;
10 for (
int i = 0; i <
wings_.size(); i++){
12 for (
int j = 0; j <
wings_[i].size(); j++){
15 visualization_msgs::Marker marker;
16 marker.header.frame_id =
"map";
17 marker.header.stamp = ros::Time();
18 marker.ns = w->
name();
20 marker.type = visualization_msgs::Marker::CUBE;
21 marker.action = visualization_msgs::Marker::ADD;
22 marker.pose.position.x = w->
world_tf().getOrigin().getX();
23 marker.pose.position.y = w->
world_tf().getOrigin().getY();
24 marker.pose.position.z = w->
world_tf().getOrigin().getZ();
25 marker.pose.orientation.x = w->
world_tf().getRotation().getX();
26 marker.pose.orientation.y = w->
world_tf().getRotation().getY();
27 marker.pose.orientation.z = w->
world_tf().getRotation().getZ();
28 marker.pose.orientation.w = w->
world_tf().getRotation().getW();
29 marker.scale.x = w->
size().getX();
30 marker.scale.y = w->
size().getY();
31 marker.scale.z = w->
size().getZ();
36 ma.markers.push_back(marker);
44 for (
int i =0; i < wbp.size(); i++){
45 std::vector<Abstract_robot_element*> v;
46 for (
int j =0; j < wbp[i].first.size(); j++){
57 std::vector<int> count_v;
62 visualization_msgs::MarkerArray ma;
63 for(
long unsigned int j = 0; j <
objects_[robot].size(); j++){
64 visualization_msgs::Marker marker;
65 marker.header.frame_id =
"map";
66 marker.header.stamp = ros::Time();
67 marker.ns =
"objects ";
69 marker.type = visualization_msgs::Marker::CUBE;
70 marker.action = visualization_msgs::Marker::ADD;
71 marker.pose.position.x =
objects_[robot][j].getOrigin().getX();
72 marker.pose.position.y =
objects_[robot][j].getOrigin().getY();
73 marker.pose.position.z =
objects_[robot][j].getOrigin().getZ();
74 marker.pose.orientation.x =
objects_[robot][j].getRotation().getX();
75 marker.pose.orientation.y =
objects_[robot][j].getRotation().getY();
76 marker.pose.orientation.z =
objects_[robot][j].getRotation().getZ();
77 marker.pose.orientation.w =
objects_[robot][j].getRotation().getW();
81 if(
robots_[robot]->check_single_object_collision(
objects_[robot][j], str)){
93 ma.markers.push_back(marker);
98 for (
int& i : count_v){
99 if(i == 0) {succ =
false;
break;}
106 ROS_INFO(
"assigne result to first robot...");
108 std::vector<std::vector<tf2::Transform>> filter_per_robot;
110 pcl::octree::OctreePointCloudSearch< pcl::PointXYZ >first_cloud(0.4f);
112 first_cloud.addPointsFromInputCloud();
113 std::vector<tf2::Transform> ground_per_robot;
114 for(pcl::PointXYZ& p : grCenter){
115 double min_x, min_y, min_z, max_x, max_y, max_z;
116 first_cloud.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
117 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);
118 if (isInBox && first_cloud.isVoxelOccupiedAtPoint(p)) {
119 std::vector< int > pointIdxVec;
120 if (first_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)));
124 for(
int i = 0; i <
objects_.size(); i++){
126 for (
long unsigned int j =
objects_[i].size()-1; j > 0; j--){
145 visualization_msgs::MarkerArray ma;
148 for(
int j = 0; j <= 7; j++){
152 visualization_msgs::Marker m;
153 m.header.frame_id =
"map";
154 m.header.stamp = ros::Time();
155 m.ns = ceti1->
name();
157 m.type = visualization_msgs::Marker::CUBE;
158 m.action = visualization_msgs::Marker::ADD;
159 m.pose.position.x = ceti1->
tf().getOrigin().getX();
160 m.pose.position.y = ceti1->
tf().getOrigin().getY();
161 m.pose.position.z = ceti1->
tf().getOrigin().getZ();
162 m.pose.orientation.x = ceti1->
tf().getRotation().getX();
163 m.pose.orientation.y = ceti1->
tf().getRotation().getY();
164 m.pose.orientation.z = ceti1->
tf().getRotation().getZ();
165 m.pose.orientation.w = ceti1->
tf().getRotation().getW();
166 m.scale.x = ceti1->
size().getX();
167 m.scale.y = ceti1->
size().getY();
168 m.scale.z = ceti1->
tf().getOrigin().getZ()*2;
173 ma.markers.push_back(m);
177 ros::Duration timer(0.10);
179 for (
long unsigned int i = 0; i < ground_per_robot.size(); i++){
180 ceti1->
set_tf(ground_per_robot[i]);
183 for (
float p = 0; p < 2*M_PI; p += 0.0872665f){
184 ceti1->
rotate(0.0872665f);
187 m.action = visualization_msgs::Marker::MODIFY;
188 m.pose.position.x = ceti1->
tf().getOrigin().getX();
189 m.pose.position.y = ceti1->
tf().getOrigin().getY();
190 m.pose.position.z = ceti1->
tf().getOrigin().getZ();
191 m.pose.orientation.x = ceti1->
tf().getRotation().getX();
192 m.pose.orientation.y = ceti1->
tf().getRotation().getY();
193 m.pose.orientation.z = ceti1->
tf().getRotation().getZ();
194 m.pose.orientation.w = ceti1->
tf().getRotation().getW();
195 ma.markers.push_back(m);
215 m.action = visualization_msgs::Marker::DELETEALL;
216 ma.markers.push_back(m);
226 visualization_msgs::MarkerArray ma;
228 for (
int i = 0; i <= 7; i++){
232 visualization_msgs::Marker m;
233 m.header.frame_id =
"map";
234 m.header.stamp = ros::Time();
235 m.ns = ceti1->
name();
237 m.type = visualization_msgs::Marker::CUBE;
238 m.action = visualization_msgs::Marker::ADD;
239 m.pose.position.x = ceti1->
tf().getOrigin().getX();
240 m.pose.position.y = ceti1->
tf().getOrigin().getY();
241 m.pose.position.z = ceti1->
tf().getOrigin().getZ();
242 m.pose.orientation.x = ceti1->
tf().getRotation().getX();
243 m.pose.orientation.y = ceti1->
tf().getRotation().getY();
244 m.pose.orientation.z = ceti1->
tf().getRotation().getZ();
245 m.pose.orientation.w = ceti1->
tf().getRotation().getW();
246 m.scale.x = ceti1->
size().getX();
247 m.scale.y = ceti1->
size().getY();
248 m.scale.z = ceti1->
tf().getOrigin().getZ()*2;
253 ma.markers.push_back(m);
258 ceti1->
set_tf(fields->world_tf());
260 for (
float p = 0; p < 2*M_PI; p += M_PI/2){
264 m.action = visualization_msgs::Marker::MODIFY;
265 m.pose.position.x = ceti1->
tf().getOrigin().getX();
266 m.pose.position.y = ceti1->
tf().getOrigin().getY();
267 m.pose.position.z = ceti1->
tf().getOrigin().getZ();
268 m.pose.orientation.x = ceti1->
tf().getRotation().getX();
269 m.pose.orientation.y = ceti1->
tf().getRotation().getY();
270 m.pose.orientation.z = ceti1->
tf().getRotation().getZ();
271 m.pose.orientation.w = ceti1->
tf().getRotation().getW();
272 ma.markers.push_back(m);
295 m.action = visualization_msgs::Marker::DELETEALL;
296 ma.markers.push_back(m);
304 std::ofstream o(ros::package::getPath(
"multi_cell_builder") +
"/results/" +
dirname_ +
"/" + std::to_string(
static_cast<int>(ros::Time::now().toNSec())) +
".yaml");
306 tf2::Matrix3x3 m(A->
tf().getRotation());
310 float size_x = A->
size().getX();
311 float size_y = A->
size().getY();
312 float size_z = A->
size().getZ();
314 float pos_x = A->
tf().getOrigin().getX();
315 float pos_y = A->
tf().getOrigin().getY();
316 float pos_z = A->
tf().getOrigin().getZ() *2 ;
317 float rot_x = A->
tf().getRotation().getX();
318 float rot_y = A->
tf().getRotation().getY();
319 float rot_z = A->
tf().getRotation().getZ();
320 float rot_w = A->
tf().getRotation().getW();
326 std::stringstream ss;
327 ss <<
"{ 'objects' : [ \n";
328 ss <<
"{ 'id' : 'table" << A->
name().back() <<
"_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";
329 ss <<
"{ 'id' : 'table" << A->
name().back() <<
"_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";
330 ss <<
"{ 'id' : 'table" << A->
name().back() <<
"_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";
331 ss <<
"{ 'id' : 'table" << A->
name().back() <<
"_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";
332 ss <<
"{ 'id' : 'table" << A->
name().back() <<
"_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";
333 ss <<
"{ 'id' : 'table" << A->
name().back() <<
"_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";
334 ss <<
"{ 'id' : 'table" << A->
name().back() <<
"_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";
335 ss <<
"{ 'id' : 'table" << A->
name().back() <<
"_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";
336 ss <<
"{ 'id' : 'table" << A->
name().back() <<
"_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";
338 m.setRotation(B->
tf().getRotation());
341 size_x = B->
size().getX();
342 size_y = B->
size().getY();
343 size_z = B->
size().getZ();
345 pos_x = B->
tf().getOrigin().getX();
346 pos_y = B->
tf().getOrigin().getY();
347 pos_z = B->
tf().getOrigin().getZ() *2 ;
348 rot_x = B->
tf().getRotation().getX();
349 rot_y = B->
tf().getRotation().getY();
350 rot_z = B->
tf().getRotation().getZ();
351 rot_w = B->
tf().getRotation().getW();
353 ss <<
"{ 'id' : 'table" << B->
name().back() <<
"_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";
354 ss <<
"{ 'id' : 'table" << B->
name().back() <<
"_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";
355 ss <<
"{ 'id' : 'table" << B->
name().back() <<
"_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";
356 ss <<
"{ 'id' : 'table" << B->
name().back() <<
"_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";
357 ss <<
"{ 'id' : 'table" << B->
name().back() <<
"_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";
358 ss <<
"{ 'id' : 'table" << B->
name().back() <<
"_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";
359 ss <<
"{ 'id' : 'table" << B->
name().back() <<
"_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";
360 ss <<
"{ 'id' : 'table" << B->
name().back() <<
"_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";
361 ss <<
"{ 'id' : 'table" << B->
name().back() <<
"_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";
375 float length = w->
size().getX();
376 float width = w->
size().getY();
377 float height = w->
size().getZ();
379 ss <<
"{ 'id': '" << w->
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";
394 float length = w->
size().getX();
395 float width = w->
size().getY();
396 float height = w->
size().getZ();
398 ss <<
"{ 'id': '" << w->
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";
400 tf2::Transform tf_arm = A->
tf() * A->
root_tf();
401 float arm_x = tf_arm.getOrigin().getX();
402 float arm_y = tf_arm.getOrigin().getY();
403 float arm_z = tf_arm.getOrigin().getZ();
404 float arm_qx = tf_arm.getRotation().getX();
405 float arm_qy = tf_arm.getRotation().getY();
406 float arm_qz = tf_arm.getRotation().getZ();
407 float arm_qw = tf_arm.getRotation().getW();
409 ss <<
"{ 'id': 'arm" << A->
name().back() <<
"','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";
412 arm_x = tf_arm.getOrigin().getX();
413 arm_y = tf_arm.getOrigin().getY();
414 arm_z = tf_arm.getOrigin().getZ();
415 arm_qx = tf_arm.getRotation().getX();
416 arm_qy = tf_arm.getRotation().getY();
417 arm_qz = tf_arm.getRotation().getZ();
418 arm_qw = tf_arm.getRotation().getW();
420 ss <<
"{ 'id': 'arm" << B->
name().back() <<
"','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";
430 std::bitset<3> result =
robots_[robot]->observer_mask() & wing;
433 for (std::size_t i = 0; i < result.size(); i++){
439 visualization_msgs::MarkerArray ma;
442 wad->
markers()->action = visualization_msgs::Marker::ADD;
443 ma.markers.push_back(*wad->
markers());
449 visualization_msgs::MarkerArray ma;
450 visualization_msgs::Marker marker;
451 marker.header.frame_id =
"map";
452 marker.header.stamp = ros::Time();
453 marker.ns = ar->
name();
455 marker.type = visualization_msgs::Marker::CUBE;
456 marker.action = visualization_msgs::Marker::MODIFY;
457 marker.pose.position.x = ar->
tf().getOrigin().getX();
458 marker.pose.position.y = ar->
tf().getOrigin().getY();
459 marker.pose.position.z = ar->
tf().getOrigin().getZ();
460 marker.pose.orientation.x = ar->
tf().getRotation().getX();
461 marker.pose.orientation.y = ar->
tf().getRotation().getY();
462 marker.pose.orientation.z = ar->
tf().getRotation().getZ();
463 marker.pose.orientation.w = ar->
tf().getRotation().getW();
464 marker.scale.x = ar->
size().getX();
465 marker.scale.y = ar->
size().getY();
466 marker.scale.z = ar->
size().getZ();
467 marker.color.r = 1.0;
468 marker.color.g = 1.0;
469 marker.color.b = 1.0;
470 marker.color.a = 1.0;