diff --git a/cartographer_ros/cartographer_ros/submap.cc b/cartographer_ros/cartographer_ros/submap.cc index f62e9a3..c05ecfb 100644 --- a/cartographer_ros/cartographer_ros/submap.cc +++ b/cartographer_ros/cartographer_ros/submap.cc @@ -20,6 +20,7 @@ #include "cartographer/common/port.h" #include "cartographer/transform/transform.h" #include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros_msgs/StatusCode.h" #include "cartographer_ros_msgs/SubmapQuery.h" namespace cartographer_ros { @@ -30,7 +31,8 @@ std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures( ::cartographer_ros_msgs::SubmapQuery srv; srv.request.trajectory_id = submap_id.trajectory_id; srv.request.submap_index = submap_id.submap_index; - if (!client->call(srv)) { + if (!client->call(srv) || + srv.response.status.code != ::cartographer_ros_msgs::StatusCode::OK) { return nullptr; } CHECK(!srv.response.textures.empty());