mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-08 10:47:46 +08:00
created ros2 branch
This commit is contained in:
matlabbe
+24
-29
@@ -32,50 +32,45 @@ namespace rtabmap_ros
|
||||
{
|
||||
|
||||
InfoDisplay::InfoDisplay()
|
||||
: spinner_(1, &cbqueue_),
|
||||
globalCount_(0),
|
||||
: globalCount_(0),
|
||||
localCount_(0)
|
||||
{
|
||||
update_nh_.setCallbackQueue( &cbqueue_ );
|
||||
}
|
||||
|
||||
InfoDisplay::~InfoDisplay()
|
||||
{
|
||||
spinner_.stop();
|
||||
}
|
||||
|
||||
void InfoDisplay::onInitialize()
|
||||
{
|
||||
MFDClass::onInitialize();
|
||||
|
||||
this->setStatusStd(rviz::StatusProperty::Ok, "Info", "");
|
||||
this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", "");
|
||||
this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", "");
|
||||
this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", "0");
|
||||
this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", "0");
|
||||
|
||||
spinner_.start();
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Info", "");
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Position (XYZ)", "");
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Orientation (RPY)", "");
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Loop closures", "0");
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Proximity detections", "0");
|
||||
}
|
||||
|
||||
void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg )
|
||||
void InfoDisplay::processMessage( const rtabmap_ros::msg::Info::ConstSharedPtr msg )
|
||||
{
|
||||
{
|
||||
boost::mutex::scoped_lock lock(info_mutex_);
|
||||
if(msg->loopClosureId)
|
||||
std::unique_lock<std::mutex> lock(info_mutex_);
|
||||
if(msg->loop_closure_id)
|
||||
{
|
||||
info_ = QString("%1->%2").arg(msg->refId).arg(msg->loopClosureId);
|
||||
info_ = QString("%1->%2").arg(msg->ref_id).arg(msg->loop_closure_id);
|
||||
globalCount_ += 1;
|
||||
}
|
||||
else if(msg->proximityDetectionId)
|
||||
else if(msg->proximity_detection_id)
|
||||
{
|
||||
info_ = QString("%1->%2 [Proximity]").arg(msg->refId).arg(msg->proximityDetectionId);
|
||||
info_ = QString("%1->%2 [Proximity]").arg(msg->ref_id).arg(msg->proximity_detection_id);
|
||||
localCount_ += 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
info_ = "";
|
||||
}
|
||||
loopTransform_ = rtabmap_ros::transformFromGeometryMsg(msg->loopClosureTransform);
|
||||
loopTransform_ = rtabmap_ros::transformFromGeometryMsg(msg->loop_closure_transform);
|
||||
|
||||
rtabmap::Statistics stat;
|
||||
rtabmap_ros::infoFromROS(*msg, stat);
|
||||
@@ -89,26 +84,26 @@ void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg )
|
||||
void InfoDisplay::update( float wall_dt, float ros_dt )
|
||||
{
|
||||
{
|
||||
boost::mutex::scoped_lock lock(info_mutex_);
|
||||
this->setStatusStd(rviz::StatusProperty::Ok, "Info", tr("%1").arg(info_).toStdString());
|
||||
std::unique_lock<std::mutex> lock(info_mutex_);
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Info", tr("%1").arg(info_).toStdString());
|
||||
if(loopTransform_.isNull())
|
||||
{
|
||||
this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", "");
|
||||
this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", "");
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Position (XYZ)", "");
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Orientation (RPY)", "");
|
||||
}
|
||||
else
|
||||
{
|
||||
float x,y,z, roll,pitch,yaw;
|
||||
loopTransform_.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
|
||||
this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString());
|
||||
this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString());
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString());
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString());
|
||||
}
|
||||
this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString());
|
||||
this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString());
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString());
|
||||
this->setStatusStd(rviz_common::properties::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString());
|
||||
|
||||
for(std::map<std::string, float>::const_iterator iter=statistics_.begin(); iter!=statistics_.end(); ++iter)
|
||||
{
|
||||
this->setStatus(rviz::StatusProperty::Ok, iter->first.c_str(), tr("%1").arg(iter->second));
|
||||
this->setStatus(rviz_common::properties::StatusProperty::Ok, iter->first.c_str(), tr("%1").arg(iter->second));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -117,7 +112,7 @@ void InfoDisplay::reset()
|
||||
{
|
||||
MFDClass::reset();
|
||||
{
|
||||
boost::mutex::scoped_lock lock(info_mutex_);
|
||||
std::unique_lock<std::mutex> lock(info_mutex_);
|
||||
info_.clear();
|
||||
globalCount_ = 0;
|
||||
localCount_ = 0;
|
||||
@@ -128,4 +123,4 @@ void InfoDisplay::reset()
|
||||
} // namespace rtabmap_ros
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::InfoDisplay, rviz::Display )
|
||||
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::InfoDisplay, rviz_common::Display )
|
||||
|
||||
+13
-8
@@ -28,15 +28,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#ifndef INFO_DISPLAY_H
|
||||
#define INFO_DISPLAY_H
|
||||
|
||||
#include <rtabmap_ros/Info.h>
|
||||
#include <memory>
|
||||
#include <set>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
#include <rviz/message_filter_display.h>
|
||||
#include <rtabmap_ros/visibility.h>
|
||||
#include <rtabmap_ros/msg/info.hpp>
|
||||
|
||||
#include <rviz_common/display.hpp>
|
||||
#include "rviz_common/message_filter_display.hpp"
|
||||
#include <rtabmap/core/Transform.h>
|
||||
|
||||
namespace rtabmap_ros
|
||||
{
|
||||
|
||||
class InfoDisplay: public rviz::MessageFilterDisplay<rtabmap_ros::Info>
|
||||
class RTABMAP_ROS_PUBLIC InfoDisplay: public rviz_common::MessageFilterDisplay<rtabmap_ros::msg::Info>
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
@@ -51,18 +59,15 @@ protected:
|
||||
virtual void onInitialize();
|
||||
|
||||
/** @brief Process a single message. Overridden from MessageFilterDisplay. */
|
||||
virtual void processMessage( const rtabmap_ros::InfoConstPtr& cloud );
|
||||
virtual void processMessage( const rtabmap_ros::msg::Info::ConstSharedPtr cloud );
|
||||
|
||||
private:
|
||||
ros::AsyncSpinner spinner_;
|
||||
ros::CallbackQueue cbqueue_;
|
||||
|
||||
QString info_;
|
||||
int globalCount_;
|
||||
int localCount_;
|
||||
std::map<std::string, float> statistics_;
|
||||
rtabmap::Transform loopTransform_;
|
||||
boost::mutex info_mutex_;
|
||||
std::mutex info_mutex_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_ros
|
||||
|
||||
+359
-380
File diff suppressed because it is too large
Load Diff
+91
-79
@@ -30,38 +30,67 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829
|
||||
|
||||
#include <deque>
|
||||
#include <queue>
|
||||
#include <memory>
|
||||
#include <set>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
#include <rtabmap_ros/MapData.h>
|
||||
#include <rtabmap_ros/visibility.h>
|
||||
#include <rtabmap_ros/msg/map_data.hpp>
|
||||
#include <rtabmap/core/Transform.h>
|
||||
|
||||
#include <pluginlib/class_loader.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
|
||||
#include <rviz/ogre_helpers/point_cloud.h>
|
||||
#include <rviz/message_filter_display.h>
|
||||
#include <rviz/default_plugin/point_cloud_transformer.h>
|
||||
#include "rviz_common/message_filter_display.hpp"
|
||||
#include "rviz_default_plugins/displays/pointcloud/point_cloud_selection_handler.hpp"
|
||||
#include "rviz_default_plugins/displays/pointcloud/point_cloud_transformer.hpp"
|
||||
#include "rviz_default_plugins/displays/pointcloud/point_cloud_transformer_factory.hpp"
|
||||
|
||||
#endif
|
||||
|
||||
namespace rviz {
|
||||
class IntProperty;
|
||||
namespace rviz_common
|
||||
{
|
||||
|
||||
namespace properties
|
||||
{
|
||||
|
||||
class BoolProperty;
|
||||
class EnumProperty;
|
||||
class IntProperty;
|
||||
class FloatProperty;
|
||||
class PointCloudTransformer;
|
||||
typedef boost::shared_ptr<PointCloudTransformer> PointCloudTransformerPtr;
|
||||
typedef std::vector<std::string> V_string;
|
||||
}
|
||||
|
||||
using namespace rviz;
|
||||
} // namespace properties
|
||||
|
||||
} // namespace rviz_common
|
||||
|
||||
namespace rtabmap_ros
|
||||
{
|
||||
|
||||
class PointCloudCommon;
|
||||
struct RTABMAP_ROS_PUBLIC CloudInfo
|
||||
{
|
||||
CloudInfo();
|
||||
~CloudInfo();
|
||||
|
||||
// clear the point cloud, but keep selection handler around
|
||||
void clear();
|
||||
|
||||
rclcpp::Time receive_time_;
|
||||
|
||||
Ogre::SceneManager *manager_;
|
||||
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr message_;
|
||||
rtabmap::Transform pose_;
|
||||
int id_;
|
||||
|
||||
Ogre::SceneNode *scene_node_;
|
||||
std::shared_ptr<rviz_rendering::PointCloud> cloud_;
|
||||
std::shared_ptr<rviz_default_plugins::PointCloudSelectionHandler> selection_handler_;
|
||||
|
||||
std::vector<rviz_rendering::PointCloud::Point> transformed_points_;
|
||||
};
|
||||
typedef std::shared_ptr<CloudInfo> CloudInfoPtr;
|
||||
|
||||
/**
|
||||
* \class MapCloudDisplay
|
||||
@@ -71,54 +100,35 @@ class PointCloudCommon;
|
||||
* If you set the channel's name to "rgb", it will interpret the channel as an integer rgb value, with r, g and b
|
||||
* all being 8 bits.
|
||||
*/
|
||||
class MapCloudDisplay: public rviz::MessageFilterDisplay<rtabmap_ros::MapData>
|
||||
class RTABMAP_ROS_PUBLIC MapCloudDisplay: public rviz_common::MessageFilterDisplay<rtabmap_ros::msg::MapData>
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
struct CloudInfo
|
||||
{
|
||||
CloudInfo();
|
||||
~CloudInfo();
|
||||
|
||||
// clear the point cloud, but keep selection handler around
|
||||
void clear();
|
||||
|
||||
Ogre::SceneManager *manager_;
|
||||
|
||||
sensor_msgs::PointCloud2ConstPtr message_;
|
||||
rtabmap::Transform pose_;
|
||||
int id_;
|
||||
|
||||
Ogre::SceneNode *scene_node_;
|
||||
boost::shared_ptr<rviz::PointCloud> cloud_;
|
||||
|
||||
std::vector<rviz::PointCloud::Point> transformed_points_;
|
||||
};
|
||||
typedef boost::shared_ptr<CloudInfo> CloudInfoPtr;
|
||||
|
||||
MapCloudDisplay();
|
||||
virtual ~MapCloudDisplay();
|
||||
explicit MapCloudDisplay();
|
||||
virtual ~MapCloudDisplay() {}
|
||||
|
||||
virtual void reset();
|
||||
virtual void update( float wall_dt, float ros_dt );
|
||||
|
||||
rviz::FloatProperty* point_world_size_property_;
|
||||
rviz::FloatProperty* point_pixel_size_property_;
|
||||
rviz::FloatProperty* alpha_property_;
|
||||
rviz::EnumProperty* xyz_transformer_property_;
|
||||
rviz::EnumProperty* color_transformer_property_;
|
||||
rviz::EnumProperty* style_property_;
|
||||
rviz::BoolProperty* cloud_from_scan_;
|
||||
rviz::IntProperty* cloud_decimation_;
|
||||
rviz::FloatProperty* cloud_max_depth_;
|
||||
rviz::FloatProperty* cloud_min_depth_;
|
||||
rviz::FloatProperty* cloud_voxel_size_;
|
||||
rviz::FloatProperty* cloud_filter_floor_height_;
|
||||
rviz::FloatProperty* cloud_filter_ceiling_height_;
|
||||
rviz::FloatProperty* node_filtering_radius_;
|
||||
rviz::FloatProperty* node_filtering_angle_;
|
||||
rviz::BoolProperty* download_map_;
|
||||
rviz::BoolProperty* download_graph_;
|
||||
bool auto_size_;
|
||||
|
||||
rviz_common::properties::FloatProperty* point_world_size_property_;
|
||||
rviz_common::properties::FloatProperty* point_pixel_size_property_;
|
||||
rviz_common::properties::FloatProperty* alpha_property_;
|
||||
rviz_common::properties::EnumProperty* xyz_transformer_property_;
|
||||
rviz_common::properties::EnumProperty* color_transformer_property_;
|
||||
rviz_common::properties::EnumProperty* style_property_;
|
||||
rviz_common::properties::BoolProperty* cloud_from_scan_;
|
||||
rviz_common::properties::IntProperty* cloud_decimation_;
|
||||
rviz_common::properties::FloatProperty* cloud_max_depth_;
|
||||
rviz_common::properties::FloatProperty* cloud_min_depth_;
|
||||
rviz_common::properties::FloatProperty* cloud_voxel_size_;
|
||||
rviz_common::properties::FloatProperty* cloud_filter_floor_height_;
|
||||
rviz_common::properties::FloatProperty* cloud_filter_ceiling_height_;
|
||||
rviz_common::properties::FloatProperty* node_filtering_radius_;
|
||||
rviz_common::properties::FloatProperty* node_filtering_angle_;
|
||||
rviz_common::properties::BoolProperty* download_map_;
|
||||
rviz_common::properties::BoolProperty* download_graph_;
|
||||
|
||||
public Q_SLOTS:
|
||||
void causeRetransform();
|
||||
@@ -129,67 +139,69 @@ private Q_SLOTS:
|
||||
void updateAlpha();
|
||||
void updateXyzTransformer();
|
||||
void updateColorTransformer();
|
||||
void setXyzTransformerOptions( EnumProperty* prop );
|
||||
void setColorTransformerOptions( EnumProperty* prop );
|
||||
void setXyzTransformerOptions( rviz_common::properties::EnumProperty* prop );
|
||||
void setColorTransformerOptions( rviz_common::properties::EnumProperty* prop );
|
||||
void updateCloudParameters();
|
||||
void downloadMap();
|
||||
void downloadGraph();
|
||||
|
||||
protected:
|
||||
/** @brief Do initialization. Overridden from MessageFilterDisplay. */
|
||||
virtual void onInitialize();
|
||||
|
||||
/** @brief Process a single message. Overridden from MessageFilterDisplay. */
|
||||
virtual void processMessage( const rtabmap_ros::MapDataConstPtr& cloud );
|
||||
|
||||
virtual void processMessage( const rtabmap_ros::msg::MapData::ConstSharedPtr cloud );
|
||||
void onInitialize();
|
||||
private:
|
||||
void processMapData(const rtabmap_ros::MapData& map);
|
||||
void processMapData(const rtabmap_ros::msg::MapData& map);
|
||||
|
||||
/**
|
||||
* \brief Transforms the cloud into the correct frame, and sets up our renderable cloud
|
||||
*/
|
||||
bool transformCloud(const CloudInfoPtr& cloud, bool fully_update_transformers);
|
||||
|
||||
rviz::PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
|
||||
rviz::PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
|
||||
void updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud );
|
||||
std::shared_ptr<rviz_default_plugins::PointCloudTransformer> getXYZTransformer(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud);
|
||||
std::shared_ptr<rviz_default_plugins::PointCloudTransformer> getColorTransformer(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud);
|
||||
void updateTransformers( const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud );
|
||||
void retransform();
|
||||
|
||||
void loadTransformers();
|
||||
void loadTransformer(
|
||||
std::shared_ptr<rviz_default_plugins::PointCloudTransformer> trans,
|
||||
std::string name,
|
||||
const std::string & lookup_name);
|
||||
|
||||
void setPropertiesHidden( const QList<Property*>& props, bool hide );
|
||||
void fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask );
|
||||
void setPropertiesHidden( const QList<rviz_common::properties::Property*>& props, bool hide );
|
||||
void fillTransformerOptions( rviz_common::properties::EnumProperty* prop, uint32_t mask );
|
||||
|
||||
private:
|
||||
ros::AsyncSpinner spinner_;
|
||||
ros::CallbackQueue cbqueue_;
|
||||
|
||||
std::map<int, CloudInfoPtr> cloud_infos_;
|
||||
|
||||
std::map<int, CloudInfoPtr> new_cloud_infos_;
|
||||
boost::mutex new_clouds_mutex_;
|
||||
std::mutex new_clouds_mutex_;
|
||||
|
||||
std::map<int, rtabmap::Transform> current_map_;
|
||||
boost::mutex current_map_mutex_;
|
||||
std::mutex current_map_mutex_;
|
||||
|
||||
struct TransformerInfo
|
||||
{
|
||||
rviz::PointCloudTransformerPtr transformer;
|
||||
QList<Property*> xyz_props;
|
||||
QList<Property*> color_props;
|
||||
std::shared_ptr<rviz_default_plugins::PointCloudTransformer> transformer;
|
||||
QList<rviz_common::properties::Property*> xyz_props;
|
||||
QList<rviz_common::properties::Property*> color_props;
|
||||
|
||||
std::string readable_name;
|
||||
std::string lookup_name;
|
||||
};
|
||||
typedef std::map<std::string, TransformerInfo> M_TransformerInfo;
|
||||
|
||||
boost::recursive_mutex transformers_mutex_;
|
||||
std::recursive_mutex transformers_mutex_;
|
||||
M_TransformerInfo transformers_;
|
||||
bool new_xyz_transformer_;
|
||||
bool new_color_transformer_;
|
||||
bool needs_retransform_;
|
||||
|
||||
pluginlib::ClassLoader<rviz::PointCloudTransformer>* transformer_class_loader_;
|
||||
std::unique_ptr<rviz_default_plugins::PointCloudTransformerFactory> transformer_factory_;
|
||||
|
||||
rclcpp::Clock::SharedPtr clock_;
|
||||
|
||||
static const std::string message_status_name_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_ros
|
||||
|
||||
@@ -25,21 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
#include <OgreSceneNode.h>
|
||||
#include <OgreSceneManager.h>
|
||||
#include <OgreManualObject.h>
|
||||
#include <OgreBillboardSet.h>
|
||||
#include <OgreMatrix4.h>
|
||||
|
||||
#include <tf/transform_listener.h>
|
||||
|
||||
#include <rviz/display_context.h>
|
||||
#include <rviz/frame_manager.h>
|
||||
#include "rviz/properties/color_property.h"
|
||||
#include "rviz/properties/float_property.h"
|
||||
#include "rviz/properties/int_property.h"
|
||||
#include <rviz_common/display_context.hpp>
|
||||
#include "rviz_common/properties/color_property.hpp"
|
||||
#include "rviz_common/properties/float_property.hpp"
|
||||
#include "rviz_common/properties/int_property.hpp"
|
||||
#include "rviz_common/logging.hpp"
|
||||
|
||||
#include "MapGraphDisplay.h"
|
||||
|
||||
@@ -51,20 +41,20 @@ namespace rtabmap_ros
|
||||
|
||||
MapGraphDisplay::MapGraphDisplay()
|
||||
{
|
||||
color_neighbor_property_ = new rviz::ColorProperty( "Neighbor", Qt::blue,
|
||||
color_neighbor_property_ = new rviz_common::properties::ColorProperty( "Neighbor", Qt::blue,
|
||||
"Color to draw neighbor links.", this );
|
||||
color_neighbor_merged_property_ = new rviz::ColorProperty( "Merged neighbor", QColor(255,170,0),
|
||||
color_neighbor_merged_property_ = new rviz_common::properties::ColorProperty( "Merged neighbor", QColor(255,170,0),
|
||||
"Color to draw merged neighbor links.", this );
|
||||
color_global_property_ = new rviz::ColorProperty( "Global loop closure", Qt::red,
|
||||
color_global_property_ = new rviz_common::properties::ColorProperty( "Global loop closure", Qt::red,
|
||||
"Color to draw global loop closure links.", this );
|
||||
color_local_property_ = new rviz::ColorProperty( "Local loop closure", Qt::yellow,
|
||||
color_local_property_ = new rviz_common::properties::ColorProperty( "Local loop closure", Qt::yellow,
|
||||
"Color to draw local loop closure links.", this );
|
||||
color_user_property_ = new rviz::ColorProperty( "User", Qt::red,
|
||||
color_user_property_ = new rviz_common::properties::ColorProperty( "User", Qt::red,
|
||||
"Color to draw user links.", this );
|
||||
color_virtual_property_ = new rviz::ColorProperty( "Virtual", Qt::magenta,
|
||||
color_virtual_property_ = new rviz_common::properties::ColorProperty( "Virtual", Qt::magenta,
|
||||
"Color to draw virtual links.", this );
|
||||
|
||||
alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
|
||||
alpha_property_ = new rviz_common::properties::FloatProperty( "Alpha", 1.0,
|
||||
"Amount of transparency to apply to the path.", this );
|
||||
}
|
||||
|
||||
@@ -95,11 +85,11 @@ void MapGraphDisplay::destroyObjects()
|
||||
manual_objects_.clear();
|
||||
}
|
||||
|
||||
void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg )
|
||||
void MapGraphDisplay::processMessage( const rtabmap_ros::msg::MapGraph::ConstSharedPtr msg )
|
||||
{
|
||||
if(!(msg->poses.size() == msg->posesId.size()))
|
||||
if(!(msg->poses.size() == msg->poses_id.size()))
|
||||
{
|
||||
ROS_ERROR("rtabmap_ros::MapGraph: Error pose ids and poses must have all the same size.");
|
||||
RVIZ_COMMON_LOG_ERROR("rtabmap_ros::MapGraph: Error pose ids and poses must have all the same size.");
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -115,7 +105,8 @@ void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg
|
||||
Ogre::Quaternion orientation;
|
||||
if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
|
||||
{
|
||||
ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
|
||||
RVIZ_COMMON_LOG_ERROR( uFormat("Error transforming from frame '%s' to frame '%s'",
|
||||
msg->header.frame_id.c_str(), qPrintable( fixed_frame_ )));
|
||||
}
|
||||
|
||||
Ogre::Matrix4 transform( orientation );
|
||||
@@ -179,4 +170,4 @@ void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg
|
||||
} // namespace rtabmap_ros
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::MapGraphDisplay, rviz::Display )
|
||||
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::MapGraphDisplay, rviz_common::Display )
|
||||
|
||||
+20
-14
@@ -29,22 +29,28 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#ifndef MAP_GRAPH_DISPLAY_H
|
||||
#define MAP_GRAPH_DISPLAY_H
|
||||
|
||||
#include <rtabmap_ros/MapGraph.h>
|
||||
#include <rtabmap_ros/visibility.h>
|
||||
#include <rtabmap_ros/msg/map_graph.hpp>
|
||||
|
||||
#include <rviz/message_filter_display.h>
|
||||
#include <rviz_common/message_filter_display.hpp>
|
||||
|
||||
namespace Ogre
|
||||
{
|
||||
class ManualObject;
|
||||
}
|
||||
|
||||
namespace rviz
|
||||
namespace rviz_common
|
||||
{
|
||||
|
||||
namespace properties
|
||||
{
|
||||
|
||||
class ColorProperty;
|
||||
class FloatProperty;
|
||||
}
|
||||
|
||||
using namespace rviz;
|
||||
} // namespace properties
|
||||
|
||||
} // namespace rviz_common
|
||||
|
||||
namespace rtabmap_ros
|
||||
{
|
||||
@@ -53,7 +59,7 @@ namespace rtabmap_ros
|
||||
* \class MapGraphDisplay
|
||||
* \brief Displays the graph of rtabmap::MapGraph message
|
||||
*/
|
||||
class MapGraphDisplay: public MessageFilterDisplay<rtabmap_ros::MapGraph>
|
||||
class RTABMAP_ROS_PUBLIC MapGraphDisplay: public rviz_common::MessageFilterDisplay<rtabmap_ros::msg::MapGraph>
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
@@ -68,20 +74,20 @@ protected:
|
||||
virtual void onInitialize();
|
||||
|
||||
/** @brief Overridden from MessageFilterDisplay. */
|
||||
void processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg );
|
||||
void processMessage( const rtabmap_ros::msg::MapGraph::ConstSharedPtr msg );
|
||||
|
||||
private:
|
||||
void destroyObjects();
|
||||
|
||||
std::vector<Ogre::ManualObject*> manual_objects_;
|
||||
|
||||
ColorProperty* color_neighbor_property_;
|
||||
ColorProperty* color_neighbor_merged_property_;
|
||||
ColorProperty* color_global_property_;
|
||||
ColorProperty* color_local_property_;
|
||||
ColorProperty* color_user_property_;
|
||||
ColorProperty* color_virtual_property_;
|
||||
FloatProperty* alpha_property_;
|
||||
rviz_common::properties::ColorProperty* color_neighbor_property_;
|
||||
rviz_common::properties::ColorProperty* color_neighbor_merged_property_;
|
||||
rviz_common::properties::ColorProperty* color_global_property_;
|
||||
rviz_common::properties::ColorProperty* color_local_property_;
|
||||
rviz_common::properties::ColorProperty* color_user_property_;
|
||||
rviz_common::properties::ColorProperty* color_virtual_property_;
|
||||
rviz_common::properties::FloatProperty* alpha_property_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_ros
|
||||
|
||||
Reference in New Issue
Block a user