| 163 | } |
| 164 | |
| 165 | void LocalizationNode::LaserScanCallback(const sensor_msgs::LaserScan::ConstPtr &laser_scan_msg_ptr){ |
| 166 | |
| 167 | last_laser_msg_timestamp_ = laser_scan_msg_ptr->header.stamp; |
| 168 | |
| 169 | Vec3d pose_in_odom; |
| 170 | if(!GetPoseFromTf(odom_frame_, base_frame_, last_laser_msg_timestamp_, pose_in_odom)) |
| 171 | { |
| 172 | LOG_ERROR << "Couldn't determine robot's pose"; |
| 173 | return; |
| 174 | } |
| 175 | |
| 176 | double angle_min = 0 , angle_increment = 0; |
| 177 | sensor_msgs::LaserScan laser_scan_msg = *laser_scan_msg_ptr; |
| 178 | TransformLaserscanToBaseFrame(angle_min, angle_increment, laser_scan_msg); |
| 179 | |
| 180 | amcl_ptr_->Update(pose_in_odom, |
| 181 | laser_scan_msg, |
| 182 | angle_min, |
| 183 | angle_increment, |
| 184 | particlecloud_msg_, |
| 185 | hyp_pose_); |
| 186 | |
| 187 | LOG_ERROR_IF(!PublishTf()) << "Publish Tf Error!"; |
| 188 | |
| 189 | if(publish_visualize_){ |
| 190 | PublishVisualize(); |
| 191 | } |
| 192 | |
| 193 | } |
| 194 | |
| 195 | void LocalizationNode::PublishVisualize(){ |
| 196 | |