mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-08 02:37:45 +08:00
First version rtabmap_launch working
This commit is contained in:
matlabbe
@@ -0,0 +1,71 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef INFO_DISPLAY_H
|
||||
#define INFO_DISPLAY_H
|
||||
|
||||
#include <rtabmap_msgs/Info.h>
|
||||
|
||||
#include <rviz/message_filter_display.h>
|
||||
#include <rtabmap/core/Transform.h>
|
||||
#include <ros/callback_queue.h>
|
||||
|
||||
namespace rtabmap_rviz_plugins
|
||||
{
|
||||
|
||||
class InfoDisplay: public rviz::MessageFilterDisplay<rtabmap_msgs::Info>
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
InfoDisplay();
|
||||
virtual ~InfoDisplay();
|
||||
|
||||
virtual void reset();
|
||||
virtual void update( float wall_dt, float ros_dt );
|
||||
|
||||
protected:
|
||||
/** @brief Do initialization. Overridden from MessageFilterDisplay. */
|
||||
virtual void onInitialize();
|
||||
|
||||
/** @brief Process a single message. Overridden from MessageFilterDisplay. */
|
||||
virtual void processMessage( const rtabmap_msgs::InfoConstPtr& 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_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_rviz_plugins
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,209 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef MAP_CLOUD_DISPLAY_H
|
||||
#define MAP_CLOUD_DISPLAY_H
|
||||
|
||||
#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829
|
||||
|
||||
#include <deque>
|
||||
#include <queue>
|
||||
#include <vector>
|
||||
|
||||
#include <rtabmap_msgs/MapData.h>
|
||||
#include <rtabmap/core/Transform.h>
|
||||
|
||||
#include <pluginlib/class_loader.hpp>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
|
||||
#include <ros/callback_queue.h>
|
||||
|
||||
#include <rviz/ogre_helpers/point_cloud.h>
|
||||
#include <rviz/message_filter_display.h>
|
||||
#include <rviz/default_plugin/point_cloud_transformer.h>
|
||||
|
||||
#endif
|
||||
|
||||
namespace rviz {
|
||||
class IntProperty;
|
||||
class BoolProperty;
|
||||
class EnumProperty;
|
||||
class FloatProperty;
|
||||
class PointCloudTransformer;
|
||||
typedef boost::shared_ptr<PointCloudTransformer> PointCloudTransformerPtr;
|
||||
typedef std::vector<std::string> V_string;
|
||||
}
|
||||
|
||||
using namespace rviz;
|
||||
|
||||
namespace rtabmap_rviz_plugins
|
||||
{
|
||||
|
||||
class PointCloudCommon;
|
||||
|
||||
/**
|
||||
* \class MapCloudDisplay
|
||||
* \brief Displays point clouds from rtabmap::MapData
|
||||
*
|
||||
* By default it will assume channel 0 of the cloud is an intensity value, and will color them by intensity.
|
||||
* 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_msgs::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();
|
||||
|
||||
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::StringProperty * download_namespace;
|
||||
rviz::BoolProperty* download_map_;
|
||||
rviz::BoolProperty* download_graph_;
|
||||
|
||||
public Q_SLOTS:
|
||||
void causeRetransform();
|
||||
|
||||
private Q_SLOTS:
|
||||
void updateStyle();
|
||||
void updateBillboardSize();
|
||||
void updateAlpha();
|
||||
void updateXyzTransformer();
|
||||
void updateColorTransformer();
|
||||
void setXyzTransformerOptions( EnumProperty* prop );
|
||||
void setColorTransformerOptions( EnumProperty* prop );
|
||||
void updateCloudParameters();
|
||||
void downloadNamespaceChanged();
|
||||
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_msgs::MapDataConstPtr& cloud );
|
||||
|
||||
private:
|
||||
void downloadMap(bool graphOnly);
|
||||
void processMapData(const rtabmap_msgs::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 );
|
||||
void retransform();
|
||||
|
||||
void loadTransformers();
|
||||
|
||||
void setPropertiesHidden( const QList<Property*>& props, bool hide );
|
||||
void fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask );
|
||||
|
||||
private:
|
||||
ros::AsyncSpinner spinner_;
|
||||
ros::CallbackQueue cbqueue_;
|
||||
ros::Publisher republishNodeDataPub_;
|
||||
|
||||
std::map<int, CloudInfoPtr> cloud_infos_;
|
||||
|
||||
std::map<int, CloudInfoPtr> new_cloud_infos_;
|
||||
boost::mutex new_clouds_mutex_;
|
||||
|
||||
std::set<int> nodeDataReceived_;
|
||||
bool fromScan_;
|
||||
|
||||
std::map<int, rtabmap::Transform> current_map_;
|
||||
boost::mutex current_map_mutex_;
|
||||
bool current_map_updated_;
|
||||
|
||||
int lastCloudAdded_;
|
||||
|
||||
struct TransformerInfo
|
||||
{
|
||||
rviz::PointCloudTransformerPtr transformer;
|
||||
QList<Property*> xyz_props;
|
||||
QList<Property*> color_props;
|
||||
|
||||
std::string readable_name;
|
||||
std::string lookup_name;
|
||||
};
|
||||
typedef std::map<std::string, TransformerInfo> M_TransformerInfo;
|
||||
|
||||
boost::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_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_rviz_plugins
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,91 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef MAP_GRAPH_DISPLAY_H
|
||||
#define MAP_GRAPH_DISPLAY_H
|
||||
|
||||
#include <rtabmap_msgs/MapGraph.h>
|
||||
|
||||
#include <rviz/message_filter_display.h>
|
||||
|
||||
namespace Ogre
|
||||
{
|
||||
class ManualObject;
|
||||
}
|
||||
|
||||
namespace rviz
|
||||
{
|
||||
class ColorProperty;
|
||||
class FloatProperty;
|
||||
}
|
||||
|
||||
using namespace rviz;
|
||||
|
||||
namespace rtabmap_rviz_plugins
|
||||
{
|
||||
|
||||
/**
|
||||
* \class MapGraphDisplay
|
||||
* \brief Displays the graph of rtabmap::MapGraph message
|
||||
*/
|
||||
class MapGraphDisplay: public MessageFilterDisplay<rtabmap_msgs::MapGraph>
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
MapGraphDisplay();
|
||||
virtual ~MapGraphDisplay();
|
||||
|
||||
/** @brief Overridden from Display. */
|
||||
virtual void reset();
|
||||
|
||||
protected:
|
||||
/** @brief Overridden from Display. */
|
||||
virtual void onInitialize();
|
||||
|
||||
/** @brief Overridden from MessageFilterDisplay. */
|
||||
void processMessage( const rtabmap_msgs::MapGraph::ConstPtr& 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_landmark_property_;
|
||||
ColorProperty* color_user_property_;
|
||||
ColorProperty* color_virtual_property_;
|
||||
FloatProperty* alpha_property_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_rviz_plugins
|
||||
|
||||
#endif /* MAP_GRAPH_DISPLAY_H */
|
||||
|
||||
@@ -0,0 +1,51 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef ORBITORIENTEDVIEWCONTROLLER_H_
|
||||
#define ORBITORIENTEDVIEWCONTROLLER_H_
|
||||
|
||||
#include "rviz/default_plugin/view_controllers/orbit_view_controller.h"
|
||||
|
||||
namespace rtabmap_rviz_plugins
|
||||
{
|
||||
|
||||
class OrbitOrientedViewController: public rviz::OrbitViewController
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
OrbitOrientedViewController() {}
|
||||
virtual ~OrbitOrientedViewController() {}
|
||||
|
||||
protected:
|
||||
virtual void updateCamera();
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif /* ORBITORIENTEDVIEWCONTROLLER_H_ */
|
||||
Reference in New Issue
Block a user