| 90 | } |
| 91 | |
| 92 | bool LocalizationNode::GetLaserPose() { |
| 93 | auto laser_scan_msg = ros::topic::waitForMessage<sensor_msgs::LaserScan>(laser_topic_); |
| 94 | |
| 95 | Vec3d laser_pose; |
| 96 | laser_pose.setZero(); |
| 97 | GetPoseFromTf(base_frame_, laser_scan_msg->header.frame_id, ros::Time(), laser_pose); |
| 98 | laser_pose[2] = 0; // No need for rotation, or will be error |
| 99 | DLOG_INFO << "Received laser's pose wrt robot: "<< |
| 100 | laser_pose[0] << ", " << |
| 101 | laser_pose[1] << ", " << |
| 102 | laser_pose[2]; |
| 103 | |
| 104 | amcl_ptr_->SetLaserSensorPose(laser_pose); |
| 105 | return true; |
| 106 | } |
| 107 | |
| 108 | void LocalizationNode::InitialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &init_pose_msg) { |
| 109 |
nothing calls this directly
no test coverage detected