Set the pose message that is published next.
| 375 | |
| 376 | // Set the pose message that is published next. |
| 377 | void Publisher::setPose(const okvis::kinematics::Transformation& T_WS) { |
| 378 | okvis::kinematics::Transformation T; |
| 379 | if (parameters_.publishing.trackedBodyFrame == FrameName::S) { |
| 380 | poseMsg_.child_frame_id = "sensor"; |
| 381 | T = parameters_.publishing.T_Wc_W * T_WS; |
| 382 | } else if (parameters_.publishing.trackedBodyFrame == FrameName::B) { |
| 383 | poseMsg_.child_frame_id = "body"; |
| 384 | T = parameters_.publishing.T_Wc_W * T_WS * parameters_.imu.T_BS.inverse(); |
| 385 | } else { |
| 386 | LOG(ERROR) << "Pose frame does not exist for publishing. Choose 'S' or 'B'."; |
| 387 | poseMsg_.child_frame_id = "body"; |
| 388 | T = parameters_.publishing.T_Wc_W * T_WS * parameters_.imu.T_BS.inverse(); |
| 389 | } |
| 390 | |
| 391 | poseMsg_.header.frame_id = "world"; |
| 392 | poseMsg_.header.stamp = _t; |
| 393 | if ((node_->now().seconds() - _t.seconds()) > 10.0) poseMsg_.header.stamp = node_->now(); |
| 394 | |
| 395 | // fill orientation |
| 396 | Eigen::Quaterniond q = T.q(); |
| 397 | poseMsg_.transform.rotation.x = q.x(); |
| 398 | poseMsg_.transform.rotation.y = q.y(); |
| 399 | poseMsg_.transform.rotation.z = q.z(); |
| 400 | poseMsg_.transform.rotation.w = q.w(); |
| 401 | |
| 402 | // fill position |
| 403 | Eigen::Vector3d r = T.r(); |
| 404 | poseMsg_.transform.translation.x = r[0]; |
| 405 | poseMsg_.transform.translation.y = r[1]; |
| 406 | poseMsg_.transform.translation.z = r[2]; |
| 407 | |
| 408 | // also do the mesh |
| 409 | /*if (parameters_.publishing.trackedBodyFrame == FrameName::S) { |
| 410 | meshMsg_.child_frame_id = "sensor"; |
| 411 | } else if (parameters_.publishing.trackedBodyFrame == FrameName::B) { |
| 412 | meshMsg_.child_frame_id = "body"; |
| 413 | } else { |
| 414 | meshMsg_.child_frame_id = "body"; |
| 415 | }*/ |
| 416 | meshMsg_.header.frame_id = "world"; |
| 417 | meshMsg_.header.stamp = _t; |
| 418 | meshMsg_.type = visualization_msgs::msg::Marker::MESH_RESOURCE; |
| 419 | if ((node_->now().seconds() - _t.seconds()) > 10.0) meshMsg_.header.stamp = node_->now(); |
| 420 | |
| 421 | // fill orientation |
| 422 | meshMsg_.pose.orientation.x = q.x(); |
| 423 | meshMsg_.pose.orientation.y = q.y(); |
| 424 | meshMsg_.pose.orientation.z = q.z(); |
| 425 | meshMsg_.pose.orientation.w = q.w(); |
| 426 | |
| 427 | // fill position |
| 428 | meshMsg_.pose.position.x = r[0]; |
| 429 | meshMsg_.pose.position.y = r[1]; |
| 430 | meshMsg_.pose.position.z = r[2]; |
| 431 | |
| 432 | // scale -- needed |
| 433 | meshMsg_.scale.x = 1.0; |
| 434 | meshMsg_.scale.y = 1.0; |