19 #include "OgreResourceGroupManager.h" 22 #include "cartographer_ros_msgs/SubmapList.h" 23 #include "cartographer_ros_msgs/SubmapQuery.h" 24 #include "geometry_msgs/TransformStamped.h" 37 constexpr
int kMaxOnGoingRequestsPerTrajectory = 6;
38 constexpr
char kMaterialsDirectory[] =
"/ogre_media/materials";
39 constexpr
char kGlsl120Directory[] =
"/glsl120";
40 constexpr
char kScriptsDirectory[] =
"/scripts";
41 constexpr
char kDefaultMapFrame[] =
"map";
42 constexpr
char kDefaultTrackingFrame[] =
"base_link";
43 constexpr
char kDefaultSubmapQueryServiceName[] =
"/submap_query";
49 "Submap query service", kDefaultSubmapQueryServiceName,
50 "Submap query service to connect to.",
this, SLOT(
Reset()));
52 "Map frame", kDefaultMapFrame,
"Map frame, used for fading out submaps.",
55 "Tracking frame", kDefaultTrackingFrame,
56 "Tracking frame, used for fading out submaps.",
this);
59 "Submaps", QVariant(),
"List of all submaps, organized by trajectories.",
63 "Whether all the submaps should be displayed or not.", submaps_category_,
66 Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
68 Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
69 package_path + kMaterialsDirectory + kGlsl120Directory,
"FileSystem",
71 Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
72 package_path + kMaterialsDirectory + kScriptsDirectory,
"FileSystem",
74 Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
95 trajectories_.clear();
100 const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
104 if (msg->trajectory.size() < trajectories_.size()) {
105 trajectories_.clear();
107 for (
size_t trajectory_id = 0; trajectory_id < msg->trajectory.size();
109 if (trajectory_id >= trajectories_.size()) {
113 ::cartographer::common::make_unique<::rviz::Property>(
114 QString(
"Trajectory %1").arg(trajectory_id), QVariant(),
115 QString(
"List of all submaps in Trajectory %1.")
118 std::vector<std::unique_ptr<DrawableSubmap>>()));
120 auto& trajectory_category = trajectories_[trajectory_id].first;
121 auto& trajectory = trajectories_[trajectory_id].second;
122 const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries =
123 msg->trajectory[trajectory_id].submap;
126 if (submap_entries.size() < trajectory.size()) {
129 for (
size_t submap_index = 0; submap_index < submap_entries.size();
131 if (submap_index >= trajectory.size()) {
132 trajectory.push_back(
133 ::cartographer::common::make_unique<DrawableSubmap>(
137 trajectory[submap_index]->Update(msg->header,
138 submap_entries[submap_index],
148 const ::geometry_msgs::TransformStamped transform_stamped =
152 for (
auto& trajectory : trajectories_) {
153 for (
auto& submap : trajectory.second) {
154 submap->SetAlpha(transform_stamped.transform.translation.z);
158 ROS_WARN(
"Could not compute submap fading: %s", ex.what());
162 for (
const auto& trajectory : trajectories_) {
163 int num_ongoing_requests = 0;
164 for (
const auto& submap : trajectory.second) {
165 if (submap->QueryInProgress()) {
166 ++num_ongoing_requests;
169 for (
int submap_index = static_cast<int>(trajectory.second.size()) - 1;
171 num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
173 if (trajectory.second[submap_index]->MaybeFetchTexture(&
client_)) {
174 ++num_ongoing_requests;
183 for (
auto& trajectory : trajectories_) {
184 for (
auto& submap : trajectory.second) {
185 submap->set_visibility(visibility);
::rviz::Property * submaps_category_
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
~SubmapsDisplay() override
::cartographer::common::Mutex mutex_
::rviz::StringProperty * tracking_frame_property_
virtual void onInitialize()
::tf2_ros::Buffer tf_buffer_
DisplayContext * context_
ros::NodeHandle update_nh_
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
std::string getStdString()
void processMessage(const ::cartographer_ros_msgs::SubmapList::ConstPtr &msg) override
virtual bool getBool() const
virtual FrameManager * getFrameManager() const =0
std::pair< std::unique_ptr<::rviz::Property >, std::vector< std::unique_ptr< DrawableSubmap >>> Trajectory
::rviz::StringProperty * submap_query_service_property_
::rviz::StringProperty * map_frame_property_
::rviz::BoolProperty * visibility_all_enabled_
virtual Ogre::SceneManager * getSceneManager() const =0
void onInitialize() override
Mutex::Locker MutexLocker
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void update(float wall_dt, float ros_dt) override
ros::ServiceClient client_