7 , pub_(std::make_unique<ros::Publisher>()){};
10 for(
int i = 0; i <
robots_.size();i++){
11 std::vector<pcl::PointXYZ> ground_plane;
12 for (
float x = origin.getX() - diameter * 1.5; x <= origin.getX() + diameter * 1.5; x += resolution){
13 for (
float y = origin.getY() - diameter * 1.5; y <= origin.getY() + diameter * 1.5; y += resolution){
14 ground_plane.push_back(pcl::PointXYZ(x,y, 0.4425f));
15 ROS_INFO(
"%f",
robots_[i]->tf().getOrigin().getZ());
18 grounds_.insert(std::pair<
const std::string, std::vector<pcl::PointXYZ>>(
robots_[i]->name(), ground_plane));
24 robots_.push_back(std::move(robot));
25 ROS_INFO(
"%s connected...",
robots_.front()->name().c_str());
29 for (
int i = 0; i <
robots_.size(); i++){
47 std::vector<int> count_v;
49 count_v.resize(r->observers().size()+1);
52 visualization_msgs::MarkerArray ma;
100 ROS_INFO(
"assigne result to first robot...");
141 visualization_msgs::MarkerArray ma;
144 for(
int j = 0; j <= 7; j++){
148 visualization_msgs::Marker m;
149 m.header.frame_id =
"map";
150 m.header.stamp = ros::Time();
151 m.ns = ceti1->name();
153 m.type = visualization_msgs::Marker::CUBE;
154 m.action = visualization_msgs::Marker::ADD;
155 m.pose.position.x = ceti1->tf().getOrigin().getX();
156 m.pose.position.y = ceti1->tf().getOrigin().getY();
157 m.pose.position.z = ceti1->tf().getOrigin().getZ();
158 m.pose.orientation.x = ceti1->tf().getRotation().getX();
159 m.pose.orientation.y = ceti1->tf().getRotation().getY();
160 m.pose.orientation.z = ceti1->tf().getRotation().getZ();
161 m.pose.orientation.w = ceti1->tf().getRotation().getW();
162 m.scale.x = ceti1->size().getX();
163 m.scale.y = ceti1->size().getY();
164 m.scale.z = ceti1->tf().getOrigin().getZ()*2;
169 ma.markers.push_back(m);
173 ros::Duration timer(0.10);
175 for (
long unsigned int i = 0; i < ground_per_robot.size(); i++){
176 ceti1->set_tf(ground_per_robot[i]);
179 for (
float p = 0; p < 2*M_PI; p += 0.0872665f){
180 ceti1->rotate(0.0872665f);
183 m.action = visualization_msgs::Marker::MODIFY;
184 m.pose.position.x = ceti1->tf().getOrigin().getX();
185 m.pose.position.y = ceti1->tf().getOrigin().getY();
186 m.pose.position.z = ceti1->tf().getOrigin().getZ();
187 m.pose.orientation.x = ceti1->tf().getRotation().getX();
188 m.pose.orientation.y = ceti1->tf().getRotation().getY();
189 m.pose.orientation.z = ceti1->tf().getRotation().getZ();
190 m.pose.orientation.w = ceti1->tf().getRotation().getW();
191 ma.markers.push_back(m);
193 for (
int j = 0; j < ceti1->observers().size(); j++){
194 auto wrd =
dynamic_cast<Rviz_panel*
>(ceti1->observers()[j].get());
195 ma.markers.push_back(wrd->marker());
211 m.action = visualization_msgs::Marker::DELETEALL;
212 ma.markers.push_back(m);
222 visualization_msgs::MarkerArray ma;
224 for (
int i = 0; i <= 7; i++){
228 visualization_msgs::Marker m;
229 m.header.frame_id =
"map";
230 m.header.stamp = ros::Time();
231 m.ns = ceti1->name();
233 m.type = visualization_msgs::Marker::CUBE;
234 m.action = visualization_msgs::Marker::ADD;
235 m.pose.position.x = ceti1->tf().getOrigin().getX();
236 m.pose.position.y = ceti1->tf().getOrigin().getY();
237 m.pose.position.z = ceti1->tf().getOrigin().getZ();
238 m.pose.orientation.x = ceti1->tf().getRotation().getX();
239 m.pose.orientation.y = ceti1->tf().getRotation().getY();
240 m.pose.orientation.z = ceti1->tf().getRotation().getZ();
241 m.pose.orientation.w = ceti1->tf().getRotation().getW();
242 m.scale.x = ceti1->size().getX();
243 m.scale.y = ceti1->size().getY();
244 m.scale.z = ceti1->tf().getOrigin().getZ()*2;
249 ma.markers.push_back(m);
256 for (
float p = 0; p < 2*M_PI; p += M_PI/2){
257 ceti1->rotate(M_PI/2);
260 m.action = visualization_msgs::Marker::MODIFY;
261 m.pose.position.x = ceti1->tf().getOrigin().getX();
262 m.pose.position.y = ceti1->tf().getOrigin().getY();
263 m.pose.position.z = ceti1->tf().getOrigin().getZ();
264 m.pose.orientation.x = ceti1->tf().getRotation().getX();
265 m.pose.orientation.y = ceti1->tf().getRotation().getY();
266 m.pose.orientation.z = ceti1->tf().getRotation().getZ();
267 m.pose.orientation.w = ceti1->tf().getRotation().getW();
268 ma.markers.push_back(m);
270 for(
int j = 0; j < ceti1->observers().size(); j++){
271 auto wrd =
dynamic_cast<Rviz_panel*
>(ceti1->observers()[j].get());
272 ma.markers.push_back(wrd->marker());
291 m.action = visualization_msgs::Marker::DELETEALL;
292 ma.markers.push_back(m);
300 std::ofstream o(ros::package::getPath(
"multi_cell_builder") +
"/results/" +
dirname_ +
"/" + std::to_string(
static_cast<int>(ros::Time::now().toNSec())) +
".yaml");
302 tf2::Matrix3x3 m(A->
tf().getRotation());
306 float size_x = A->
size().getX();
307 float size_y = A->
size().getY();
308 float size_z = A->
size().getZ();
310 float pos_x = A->
tf().getOrigin().getX();
311 float pos_y = A->
tf().getOrigin().getY();
312 float pos_z = A->
tf().getOrigin().getZ() *2 ;
313 float rot_x = A->
tf().getRotation().getX();
314 float rot_y = A->
tf().getRotation().getY();
315 float rot_z = A->
tf().getRotation().getZ();
316 float rot_w = A->
tf().getRotation().getW();
322 std::stringstream ss;
323 ss <<
"{ 'objects' : [ \n";
324 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";
325 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";
326 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";
327 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";
328 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";
329 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";
330 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";
331 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";
332 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";
334 m.setRotation(B->
tf().getRotation());
337 size_x = B->
size().getX();
338 size_y = B->
size().getY();
339 size_z = B->
size().getZ();
341 pos_x = B->
tf().getOrigin().getX();
342 pos_y = B->
tf().getOrigin().getY();
343 pos_z = B->
tf().getOrigin().getZ() *2 ;
344 rot_x = B->
tf().getRotation().getX();
345 rot_y = B->
tf().getRotation().getY();
346 rot_z = B->
tf().getRotation().getZ();
347 rot_w = B->
tf().getRotation().getW();
349 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";
350 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";
351 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";
352 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";
353 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";
354 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";
355 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";
356 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";
357 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";
360 for (
int i = 0; i < A->
observers().size(); i++){
361 float x = A->
observers()[i]->world_tf().getOrigin().getX();
362 float y = A->
observers()[i]->world_tf().getOrigin().getY();
363 float z = A->
observers()[i]->world_tf().getOrigin().getZ();
364 float qx = A->
observers()[i]->world_tf().getRotation().getX();
365 float qy = A->
observers()[i]->world_tf().getRotation().getY();
366 float qz = A->
observers()[i]->world_tf().getRotation().getZ();
367 float qw = A->
observers()[i]->world_tf().getRotation().getW();
369 float length = A->
observers()[i]->size().getX();
370 float width = A->
observers()[i]->size().getY();
371 float height = A->
observers()[i]->size().getZ();
373 ss <<
"{ 'id': '" << A->
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";
377 for (
int i = 0; i < B->
observers().size(); i++){
378 float x = B->
observers()[i]->world_tf().getOrigin().getX();
379 float y = B->
observers()[i]->world_tf().getOrigin().getY();
380 float z = B->
observers()[i]->world_tf().getOrigin().getZ();
381 float qx = B->
observers()[i]->world_tf().getRotation().getX();
382 float qy = B->
observers()[i]->world_tf().getRotation().getY();
383 float qz = B->
observers()[i]->world_tf().getRotation().getZ();
384 float qw = B->
observers()[i]->world_tf().getRotation().getW();
386 float length = B->
observers()[i]->size().getX();
387 float width = B->
observers()[i]->size().getY();
388 float height = B->
observers()[i]->size().getZ();
391 ss <<
"{ 'id': '" << B->
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";
393 tf2::Transform tf_arm = A->
tf() * A->
root_tf();
394 float arm_x = tf_arm.getOrigin().getX();
395 float arm_y = tf_arm.getOrigin().getY();
396 float arm_z = tf_arm.getOrigin().getZ();
397 float arm_qx = tf_arm.getRotation().getX();
398 float arm_qy = tf_arm.getRotation().getY();
399 float arm_qz = tf_arm.getRotation().getZ();
400 float arm_qw = tf_arm.getRotation().getW();
402 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";
405 arm_x = tf_arm.getOrigin().getX();
406 arm_y = tf_arm.getOrigin().getY();
407 arm_z = tf_arm.getOrigin().getZ();
408 arm_qx = tf_arm.getRotation().getX();
409 arm_qy = tf_arm.getRotation().getY();
410 arm_qz = tf_arm.getRotation().getZ();
411 arm_qw = tf_arm.getRotation().getW();
413 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";
426 for (std::size_t i = 0; i < result.size(); i++){
428 ceti->register_observers(std::move(
wings_[robot][i]));
432 visualization_msgs::MarkerArray ma;
433 for (
int i = 0; i < ceti->observers().size(); i++){
434 auto wrd =
dynamic_cast<Rviz_panel*
>(ceti->observers()[i].get());
435 wrd->
marker().action = visualization_msgs::Marker::ADD;
436 ma.markers.push_back(wrd->marker());
442 visualization_msgs::MarkerArray ma;
443 visualization_msgs::Marker marker;
444 marker.header.frame_id =
"map";
445 marker.header.stamp = ros::Time();
446 marker.ns = ar->
name();
448 marker.type = visualization_msgs::Marker::CUBE;
449 marker.action = visualization_msgs::Marker::MODIFY;
450 marker.pose.position.x = ar->
tf().getOrigin().getX();
451 marker.pose.position.y = ar->
tf().getOrigin().getY();
452 marker.pose.position.z = ar->
tf().getOrigin().getZ();
453 marker.pose.orientation.x = ar->
tf().getRotation().getX();
454 marker.pose.orientation.y = ar->
tf().getRotation().getY();
455 marker.pose.orientation.z = ar->
tf().getRotation().getZ();
456 marker.pose.orientation.w = ar->
tf().getRotation().getW();
457 marker.scale.x = ar->
size().getX();
458 marker.scale.y = ar->
size().getY();
459 marker.scale.z = ar->
size().getZ();
460 marker.color.r = 1.0;
461 marker.color.g = 1.0;
462 marker.color.b = 1.0;
463 marker.color.a = 1.0;
464 for (
int i = 0; i < ar->
observers().size(); i++){
466 ma.markers.push_back(wrd->marker());