| 193 | } |
| 194 | |
| 195 | void LocalizationNode::PublishVisualize(){ |
| 196 | |
| 197 | if(pose_pub_.getNumSubscribers() > 0){ |
| 198 | pose_msg_.header.stamp = ros::Time::now(); |
| 199 | pose_msg_.header.frame_id = global_frame_; |
| 200 | pose_msg_.pose.position.x = hyp_pose_.pose_mean[0]; |
| 201 | pose_msg_.pose.position.y = hyp_pose_.pose_mean[1]; |
| 202 | pose_msg_.pose.orientation = tf::createQuaternionMsgFromYaw(hyp_pose_.pose_mean[2]); |
| 203 | pose_pub_.publish(pose_msg_); |
| 204 | } |
| 205 | |
| 206 | if(particlecloud_pub_.getNumSubscribers() > 0){ |
| 207 | particlecloud_msg_.header.stamp = ros::Time::now(); |
| 208 | particlecloud_msg_.header.frame_id = global_frame_; |
| 209 | particlecloud_pub_.publish(particlecloud_msg_); |
| 210 | } |
| 211 | |
| 212 | if(!publish_first_distance_map_) { |
| 213 | distance_map_pub_.publish(amcl_ptr_->GetDistanceMapMsg()); |
| 214 | publish_first_distance_map_ = true; |
| 215 | } |
| 216 | } |
| 217 | |
| 218 | bool LocalizationNode::PublishTf() { |
| 219 | ros::Time transform_expiration = (last_laser_msg_timestamp_ + transform_tolerance_); |
nothing calls this directly
no outgoing calls
no test coverage detected