i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1 from introlab/master

Browse Source 
Fork Sync
This commit is contained in:
PrescilliaA
2019-11-14 17:04:36 -05:00
committed by GitHub
parent cb57676af8 1765a7a7a0
commit 73e56c442a
11 changed files with 345 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.travis.yml
+1
View File
@@ -41,6 +41,7 @@ script:
- cd ../../catkin_ws
- source devel/setup.bash
- catkin_make
- catkin_make install
notifications:
email:
CMakeLists.txt
+124 -123
View File
@@ -4,12 +4,12 @@ project(rtabmap_ros)
# Policy CMP0043 introduced in cmake version 3.0 IGNORES the use of COMPILE_DEFINITIONS suffixed variables, e.g. COMPILE_DEFINITIONS_DEBUG
# Set to OLD behavior until minimum cmake version >= 2.8.10 (version that COMPILE_DEFINITIONS can be set by generator expressions instead)
if (POLICY CMP0043)
cmake_policy(SET CMP0043 OLD)
cmake_policy(SET CMP0043 OLD)
endif (POLICY CMP0043)
# Policy CMP0042 introduced in cmake version 3.0 enables the use of @rpath in an install name via MACOSX_RPATH by default
# Set to OLD behavior so that all versions use the same behavior, or until minimum cmake version >= 2.8.12 (version where @rpath is available)
if (POLICY CMP0042)
cmake_policy(SET CMP0042 OLD)
cmake_policy(SET CMP0042 OLD)
endif (POLICY CMP0042)
## Find catkin macros and libraries
@@ -58,13 +58,6 @@ IF(RTABMAP_GUI OR rviz_FOUND)
ENDIF()
ENDIF()
ENDIF(RTABMAP_GUI OR rviz_FOUND)
IF(rviz_FOUND)
## We also use Ogre for rviz plugins
include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake)
pkg_check_modules(OGRE OGRE)
include_directories( ${OGRE_INCLUDE_DIRS} )
link_directories( ${OGRE_LIBRARY_DIRS} )
ENDIF(rviz_FOUND)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
@@ -127,22 +120,19 @@ generate_dynamic_reconfigure_options(cfg/Camera.cfg)
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
SET(optional_dependencies "")
IF(costmap_2d_FOUND)
SET(optional_dependencies "")
IF(costmap_2d_FOUND)
SET(optional_dependencies ${optional_dependencies} costmap_2d)
IF(${costmap_2d_VERSION_MAJOR} GREATER 1 OR ${costmap_2d_VERSION_MINOR} GREATER 15)
ADD_DEFINITIONS("-DCOSTMAP_2D_POINTCLOUD2")
ENDIF(${costmap_2d_VERSION_MAJOR} GREATER 1 OR ${costmap_2d_VERSION_MINOR} GREATER 15)
ENDIF(costmap_2d_FOUND)
IF(octomap_msgs_FOUND)
SET(optional_dependencies ${optional_dependencies} octomap_msgs)
ENDIF(octomap_msgs_FOUND)
IF(rviz_FOUND)
SET(optional_dependencies ${optional_dependencies} rviz)
ENDIF(costmap_2d_FOUND)
IF(octomap_msgs_FOUND)
SET(optional_dependencies ${optional_dependencies} octomap_msgs)
ENDIF(octomap_msgs_FOUND)
IF(rviz_FOUND)
SET(optional_dependencies ${optional_dependencies} rviz)
ENDIF(rviz_FOUND)
IF(find_object_2d_FOUND)
SET(optional_dependencies ${optional_dependencies} find_object_2d)
ENDIF(find_object_2d_FOUND)
IF(find_object_2d_FOUND)
SET(optional_dependencies ${optional_dependencies} find_object_2d)
ENDIF(find_object_2d_FOUND)
catkin_package(
INCLUDE_DIRS include
@@ -223,21 +213,6 @@ ELSE()
ADD_DEFINITIONS("-DCV_BRIDGE_HYDRO")
ENDIF()
# If costmap_2d is found, add the plugin
IF(costmap_2d_FOUND)
MESSAGE(STATUS "WITH costmap_2d")
include_directories(${costmap_2d_INCLUDE_DIRS})
SET(Libraries
${costmap_2d_LIBRARIES}
${Libraries}
)
SET(rtabmap_plugins_lib_src
src/costmap_2d/static_layer.cpp
src/costmap_2d/voxel_layer.cpp
${rtabmap_plugins_lib_src}
)
ENDIF(costmap_2d_FOUND)
# If octomap is found, add definition
IF(octomap_msgs_FOUND)
MESSAGE(STATUS "WITH octomap_msgs")
@@ -264,51 +239,6 @@ SET(Libraries
ADD_DEFINITIONS("-DWITH_APRILTAG_ROS")
ENDIF(apriltag_ros_FOUND)
# If rviz is found, add plugins
IF(rviz_FOUND)
MESSAGE(STATUS "WITH rviz")
include_directories(
${rviz_INCLUDE_DIRS}
)
SET(Libraries
${Libraries}
${rviz_LIBRARIES}
${rviz_DEFAULT_PLUGIN_LIBRARIES}
)
## RVIZ plugin
IF(QT4_FOUND)
qt4_wrap_cpp(MOC_FILES
src/rviz/MapCloudDisplay.h
src/rviz/MapGraphDisplay.h
src/rviz/InfoDisplay.h
src/rviz/OrbitOrientedViewController.h
)
ELSE()
qt5_wrap_cpp(MOC_FILES
src/rviz/MapCloudDisplay.h
src/rviz/MapGraphDisplay.h
src/rviz/InfoDisplay.h
src/rviz/OrbitOrientedViewController.h
)
ENDIF()
# tf:message_filters, mixing boost and Qt signals
set_property(
SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp
PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
)
SET(rtabmap_plugins_lib_src
${rtabmap_plugins_lib_src}
src/rviz/MapCloudDisplay.cpp
src/rviz/MapGraphDisplay.cpp
src/rviz/InfoDisplay.cpp
src/rviz/OrbitOrientedViewController.cpp
${MOC_FILES}
)
ENDIF(rviz_FOUND)
############################
## Declare a cpp library
############################
@@ -331,9 +261,6 @@ target_link_libraries(rtabmap_ros
target_link_libraries(rtabmap_plugins
rtabmap_ros
)
IF(Qt5_FOUND)
QT5_USE_MODULES(rtabmap_plugins Widgets Core Gui)
ENDIF(Qt5_FOUND)
add_dependencies(rtabmap_ros ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_dependencies(rtabmap_plugins ${${PROJECT_NAME}_EXPORTED_TARGETS})
@@ -385,11 +312,11 @@ set_target_properties(rtabmap_wifi_signal_sub PROPERTIES OUTPUT_NAME "wifi_signa
# If find_object_2d is found, add save objects example
IF(find_object_2d_FOUND)
MESSAGE(STATUS "WITH find_object_2d")
include_directories(${find_object_2d_INCLUDE_DIRS})
add_executable(rtabmap_save_objects_example src/SaveObjectsExample.cpp)
target_link_libraries(rtabmap_save_objects_example ${Libraries} rtabmap_ros ${find_object_2d_LIBRARIES})
set_target_properties(rtabmap_save_objects_example PROPERTIES OUTPUT_NAME "save_objects_example")
MESSAGE(STATUS "WITH find_object_2d")
include_directories(${find_object_2d_INCLUDE_DIRS})
add_executable(rtabmap_save_objects_example src/SaveObjectsExample.cpp)
target_link_libraries(rtabmap_save_objects_example ${Libraries} rtabmap_ros ${find_object_2d_LIBRARIES})
set_target_properties(rtabmap_save_objects_example PROPERTIES OUTPUT_NAME "save_objects_example")
ENDIF(find_object_2d_FOUND)
add_executable(rtabmap_camera src/CameraNode.cpp)
@@ -423,6 +350,80 @@ add_executable(rtabmap_pointcloud_to_depthimage src/PointCloudToDepthImageNode.c
target_link_libraries(rtabmap_pointcloud_to_depthimage ${Libraries})
set_target_properties(rtabmap_pointcloud_to_depthimage PROPERTIES OUTPUT_NAME "pointcloud_to_depthimage")
# If rviz is found, add plugins
IF(rviz_FOUND)
## We also use Ogre for rviz plugins
include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake)
pkg_check_modules(OGRE OGRE)
include_directories( ${OGRE_INCLUDE_DIRS} )
link_directories( ${OGRE_LIBRARY_DIRS} )
MESSAGE(STATUS "WITH rviz")
include_directories(
${rviz_INCLUDE_DIRS}
)
SET(Libraries
${Libraries}
${rviz_LIBRARIES}
${rviz_DEFAULT_PLUGIN_LIBRARIES}
)
## RVIZ plugin
IF(QT4_FOUND)
qt4_wrap_cpp(MOC_FILES
src/rviz/MapCloudDisplay.h
src/rviz/MapGraphDisplay.h
src/rviz/InfoDisplay.h
src/rviz/OrbitOrientedViewController.h
)
ELSE()
qt5_wrap_cpp(MOC_FILES
src/rviz/MapCloudDisplay.h
src/rviz/MapGraphDisplay.h
src/rviz/InfoDisplay.h
src/rviz/OrbitOrientedViewController.h
)
ENDIF()
# tf:message_filters, mixing boost and Qt signals
set_property(
SOURCE src/rviz/MapCloudDisplay.cpp src/rviz/MapGraphDisplay.cpp src/rviz/InfoDisplay.cpp src/rviz/OrbitOrientedViewController.cpp
PROPERTY COMPILE_DEFINITIONS QT_NO_KEYWORDS
)
add_library(rtabmap_rviz_plugins
src/rviz/MapCloudDisplay.cpp
src/rviz/MapGraphDisplay.cpp
src/rviz/InfoDisplay.cpp
src/rviz/OrbitOrientedViewController.cpp
${MOC_FILES}
)
target_link_libraries(rtabmap_rviz_plugins
rtabmap_ros
)
IF(Qt5_FOUND)
QT5_USE_MODULES(rtabmap_rviz_plugins Widgets Core Gui)
ENDIF(Qt5_FOUND)
add_dependencies(rtabmap_rviz_plugins ${${PROJECT_NAME}_EXPORTED_TARGETS})
ENDIF(rviz_FOUND)
# If costmap_2d is found, add the plugins
IF(costmap_2d_FOUND)
MESSAGE(STATUS "WITH costmap_2d")
IF(${costmap_2d_VERSION_MAJOR} GREATER 1 OR ${costmap_2d_VERSION_MINOR} GREATER 15)
ADD_DEFINITIONS("-DCOSTMAP_2D_POINTCLOUD2")
ENDIF(${costmap_2d_VERSION_MAJOR} GREATER 1 OR ${costmap_2d_VERSION_MINOR} GREATER 15)
include_directories(${costmap_2d_INCLUDE_DIRS})
add_library(rtabmap_costmap_plugins
src/costmap_2d/static_layer.cpp
src/costmap_2d/voxel_layer.cpp
)
target_link_libraries(rtabmap_costmap_plugins
${costmap_2d_LIBRARIES}
)
ENDIF(costmap_2d_FOUND)
#############
## Install ##
#############
@@ -442,13 +443,11 @@ install(PROGRAMS
)
## Mark executables and/or libraries for installation
IF(RTABMAP_GUI)
install(TARGETS
rtabmap_sync
rtabmap_ros
rtabmap_plugins
rtabmap
rtabmapviz
rtabmap_rgbd_odometry
rtabmap_icp_odometry
rtabmap_rgbdicp_odometry
@@ -464,30 +463,15 @@ install(TARGETS
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
ELSE() # without rtabmapviz
install(TARGETS
rtabmap_sync
rtabmap_ros
rtabmap_plugins
rtabmap
rtabmap_rgbd_odometry
rtabmap_icp_odometry
rtabmap_rgbdicp_odometry
rtabmap_stereo_odometry
rtabmap_map_assembler
rtabmap_map_optimizer
rtabmap_data_player
rtabmap_odom_msg_to_tf
rtabmap_pointcloud_to_depthimage
rtabmap_camera
rtabmap_rgbd_sync
rtabmap_rgbd_relay
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
ENDIF()
)
IF(RTABMAP_GUI)
install(TARGETS
rtabmapviz
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
ENDIF(RTABMAP_GUI)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
@@ -515,15 +499,32 @@ install(DIRECTORY
## install plugins/nodelets xml
install(FILES
nodelet_plugins.xml
plugin_description.xml
costmap_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
IF(rviz_FOUND)
install(TARGETS
rtabmap_rviz_plugins
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(FILES
rviz_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
ENDIF(rviz_FOUND)
IF(costmap_2d_FOUND)
install(FILES
costmap_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(TARGETS
rtabmap_costmap_plugins
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(FILES
costmap_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
ENDIF(costmap_2d_FOUND)
#############
costmap_plugins.xml
+1 -1
View File
@@ -1,5 +1,5 @@
<class_libraries>
<library path="lib/librtabmap_plugins">
<library path="lib/librtabmap_costmap_plugins">
<class type="rtabmap_ros::StaticLayer" base_class_type="costmap_2d::Layer">
<description>Listens to OccupancyGrid messages and copies them in, like from map_server.</description>
</class>
docker/README.md
+8 -15
View File
@@ -1,20 +1,13 @@
### Docker
* Create rtabmap image (example with melodic) from this Dockerfile:
```dockerfile
FROM ros:melodic-perception
# install rtabmap packages
RUN apt-get update && apt-get install -y \
ros-melodic-rtabmap \
ros-melodic-rtabmap-ros \
&& rm -rf /var/lib/apt/lists/
* Available images on [introlab3it/rtabmap_ros](https://hub.docker.com/r/introlab3it/rtabmap_ros/):
```
* To build an image with latest rtabmap version from source, use Dockerfile inside one of the `latest` subdirectories.
indigo, indigo-latest
kinetic, kinetic-latest
melodic, melodic-latest
```
* The `-latest` images are automatically built from latest version of `rtabmap` and `rtabmap_ros` from source. The other images have the same version than the binaries released on ROS.
* Build it:
```bash
$ docker build --tag ros:rtabmap .
```
* The following example show how to launch a camera on host computer and run our pre-built rtabmap container. All examples from [RGB-D tutorial](http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping) and [stereo tutorial](http://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping) should work using rtabmap from the container instead. Launch camera on host computer (set ROS_IP as the IP used for docker):
```bash
@@ -24,11 +17,11 @@
$ export ROS_NAMESPACE=rtabmap && rosrun rtabmap_ros rtabmapviz _frame_id:=camera_link
```
* Launch rtabmap from inside the container, saving the database on host `~/.ros/rtabmap.db`:
* Launch `rtabmap` from inside the container (no gui), saving the database on host `~/.ros/rtabmap.db`:
```bash
$ docker run -it --rm \
--env ROS_MASTER_URI=http://172.17.0.1:11311 --env ROS_IP=172.17.0.2 \
-v ~/.ros:/root \
ros:rtabmap \
introlab3it/rtabmap_ros:kinetic-latest \
roslaunch rtabmap_ros rtabmap.launch rtabmapviz:=false database_path:=/root/rtabmap.db rtabmap_args:="--delete_db_on_start"
```
docker/kinetic/latest/Dockerfile
+2 -24
View File
@@ -1,35 +1,13 @@
FROM ros:kinetic-perception
# install rtabmap packages
RUN apt-get update && apt-get install -y \
ros-kinetic-rtabmap-ros \
&& apt-get remove -y \
ros-kinetic-rtabmap \
&& rm -rf /var/lib/apt/lists/
RUN rm /bin/sh && ln -s /bin/bash /bin/sh
WORKDIR /root/
FROM introlab3it/rtabmap:16.04
ARG CACHE_DATE=2016-01-01
RUN git clone https://github.com/introlab/rtabmap.git
# Build RTAB-Map project
RUN source /ros_entrypoint.sh && \
cd rtabmap/build && \
cmake .. && \
make -j$(nproc) && \
make install && \
cd ../.. && \
rm -rf rtabmap && \
ldconfig
RUN source /ros_entrypoint.sh && \
mkdir -p catkin_ws/src && \
cd catkin_ws/src && \
catkin_init_workspace && \
git clone https://github.com/introlab/rtabmap_ros.git && \
cd .. && \
catkin_make -DCMAKE_INSTALL_PREFIX=/opt/ros/kinetic install && \
catkin_make -j1 -DCMAKE_INSTALL_PREFIX=/opt/ros/kinetic install && \
cd && \
rm -rf catkin_ws
docker/melodic/latest/Dockerfile
+2 -24
View File
@@ -1,35 +1,13 @@
FROM ros:melodic-perception
# install rtabmap packages
RUN apt-get update && apt-get install -y \
ros-melodic-rtabmap-ros \
&& apt-get remove -y \
ros-melodic-rtabmap \
&& rm -rf /var/lib/apt/lists/
RUN rm /bin/sh && ln -s /bin/bash /bin/sh
WORKDIR /root/
FROM introlab3it/rtabmap:18.04
ARG CACHE_DATE=2016-01-01
RUN git clone https://github.com/introlab/rtabmap.git
# Build RTAB-Map project
RUN source /ros_entrypoint.sh && \
cd rtabmap/build && \
cmake .. && \
make -j$(nproc) && \
make install && \
cd ../.. && \
rm -rf rtabmap && \
ldconfig
RUN source /ros_entrypoint.sh && \
mkdir -p catkin_ws/src && \
cd catkin_ws/src && \
catkin_init_workspace && \
git clone https://github.com/introlab/rtabmap_ros.git && \
cd .. && \
catkin_make -DCMAKE_INSTALL_PREFIX=/opt/ros/melodic install && \
catkin_make -j1 -DCMAKE_INSTALL_PREFIX=/opt/ros/melodic install && \
cd && \
rm -rf catkin_ws
include/rtabmap_ros/CoreWrapper.h
+3
View File
@@ -143,6 +143,7 @@ private:
void tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetectionArray & tagDetections);
#endif
void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
void interOdomCallback(const nav_msgs::OdometryConstPtr & msg);
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
@@ -319,6 +320,8 @@ private:
std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_;
ros::Subscriber imuSub_;
std::map<double, rtabmap::Transform> imus_;
ros::Subscriber interOdomSub_;
std::list<nav_msgs::Odometry> interOdoms_;
bool stereoToDepth_;
bool odomSensorSync_;
package.xml
+1 -1
View File
@@ -82,7 +82,7 @@
<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
<rviz plugin="${prefix}/plugin_description.xml"/>
<rviz plugin="${prefix}/rviz_plugins.xml"/>
<costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
</export>
</package>
plugin_description.xml → rviz_plugins.xml
+1 -1
View File
@@ -1,4 +1,4 @@
<library path="lib/librtabmap_plugins">
<library path="lib/librtabmap_rviz_plugins">
<class name="rtabmap_ros/MapCloud"
type="rtabmap_ros::MapCloudDisplay"
base_class_type="rviz::Display">
scripts/assemble_local_grids.py
Executable
+119
View File
@@ -0,0 +1,119 @@
#!/usr/bin/env python
# Similar to map_assembler node, this minimal python example shows how
# to reconstruct the obstacle map by subscribing only to
# graph and latest data added to map (for constant network bandwidth usage).
import rospy
from sets import Set
import message_filters
from rtabmap_ros.msg import MapGraph
from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import Pose
from geometry_msgs.msg import TransformStamped
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
posesDict = {}
cloudsDict = {}
assembledCloud = PointCloud2()
pub = rospy.Publisher('assembled_local_grids', PointCloud2, queue_size=10)
def callback(graph, cloud):
global assembledCloud
global posesDict
global cloudsDict
global pub
begin = rospy.get_time()
nodeId = graph.posesId[-1]
pose = graph.poses[-1]
size = cloud.width
posesDict[nodeId] = pose
cloudsDict[nodeId] = cloud
# Update pose of our buffered clouds.
# Check also if the clouds have moved because of a loop closure. If so, we have to update the rendering.
maxDiff = 0
for i in range(0,len(graph.posesId)):
if graph.posesId[i] in posesDict:
currentPose = posesDict[graph.posesId[i]].position
newPose = graph.poses[i].position
diff = max([abs(currentPose.x-newPose.x), abs(currentPose.y-newPose.y), abs(currentPose.z-newPose.z)])
if maxDiff < diff:
maxDiff = diff
else:
rospy.loginfo("Old node %d not found in cache, creating an empty cloud.", graph.posesId[i])
posesDict[graph.posesId[i]] = graph.poses[i]
cloudsDict[graph.posesId[i]] = PointCloud2()
# If we don't move, some nodes would be removed from the graph, so remove them from our buffered clouds.
newGraph = Set(graph.posesId)
totalPoints = 0
for p in posesDict.keys():
if p not in newGraph:
posesDict.pop(p)
cloudsDict.pop(p)
else:
totalPoints = totalPoints + cloudsDict[p].width
if maxDiff > 0.1:
# if any node moved more than 10 cm, request an update of the assembled map so far
newAssembledCloud = PointCloud2()
rospy.loginfo("Map has been optimized! maxDiff=%.3fm, re-updating the whole map...", maxDiff)
for i in range(0,len(graph.posesId)):
posesDict[graph.posesId[i]] = graph.poses[i]
t = TransformStamped()
p = posesDict[graph.posesId[i]]
t.transform.translation = p.position
t.transform.rotation = p.orientation
transformedCloud = do_transform_cloud(cloudsDict[graph.posesId[i]], t)
if i==0:
newAssembledCloud = transformedCloud
else:
newAssembledCloud.data = newAssembledCloud.data + transformedCloud.data
newAssembledCloud.width = newAssembledCloud.width + transformedCloud.width
newAssembledCloud.row_step = newAssembledCloud.row_step + transformedCloud.row_step
assembledCloud = newAssembledCloud
else:
t = TransformStamped()
t.transform.translation = pose.position
t.transform.rotation = pose.orientation
transformedCloud = do_transform_cloud(cloud, t)
# just concatenate new cloud to current assembled map
if assembledCloud.width == 0:
assembledCloud = transformedCloud
else:
# Adding only the difference would be more efficient
assembledCloud.data = assembledCloud.data + transformedCloud.data
assembledCloud.width = assembledCloud.width + transformedCloud.width
assembledCloud.row_step = assembledCloud.row_step + transformedCloud.row_step
updateTime = rospy.get_time() - begin
rospy.loginfo("Received node %d (%d pts) at xyz=%.2f %.2f %.2f, q_xyzw=%.2f %.2f %.2f %.2f (Map: Nodes=%d Points=%d Assembled=%d Update=%.0fms)",
nodeId, size,
pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w,
len(cloudsDict), totalPoints, assembledCloud.width, updateTime*1000)
assembledCloud.header = graph.header
pub.publish(assembledCloud)
def main():
rospy.init_node('assemble_local_grids', anonymous=True)
graph_sub = message_filters.Subscriber('rtabmap/mapGraph', MapGraph)
cloud_sub = message_filters.Subscriber('rtabmap/local_grid_obstacle', PointCloud2)
ts = message_filters.TimeSynchronizer([graph_sub, cloud_sub], 2)
ts.registerCallback(callback)
rospy.spin()
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass
src/CoreWrapper.cpp
+83
View File
@@ -516,6 +516,11 @@ void CoreWrapper::onInit()
if(createIntermediateNodes_)
{
NODELET_INFO("Create intermediate nodes");
if(rate_ == 0.0f)
{
NODELET_INFO("Subscribe to inter odom messges");
interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this);
}
}
}
if(parameters_.find(Parameters::kGridGlobalMaxNodes()) != parameters_.end())
@@ -1632,6 +1637,75 @@ void CoreWrapper::process(
UTimer timer;
if(rtabmap_.isIDsGenerated() || data.id() > 0)
{
// Add intermediate nodes?
for(std::list<nav_msgs::Odometry>::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
{
if(iter->header.stamp < lastPoseStamp_)
{
Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->pose.pose);
if(!interOdom.isNull())
{
cv::Mat covariance;
double variance = iter->twist.covariance[0];
if(variance == BAD_COVARIANCE || variance <= 0.0f)
{
//use the one of the pose
covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->pose.covariance.data()).clone();
covariance /= 2.0;
}
else
{
covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->twist.covariance.data()).clone();
}
if(!uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
{
covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomDefaultLinVariance_ > 0.0f)
{
covariance.at<double>(0,0) = odomDefaultLinVariance_;
covariance.at<double>(1,1) = odomDefaultLinVariance_;
covariance.at<double>(2,2) = odomDefaultLinVariance_;
}
if(odomDefaultAngVariance_ > 0.0f)
{
covariance.at<double>(3,3) = odomDefaultAngVariance_;
covariance.at<double>(4,4) = odomDefaultAngVariance_;
covariance.at<double>(5,5) = odomDefaultAngVariance_;
}
}
cv::Mat rgb = cv::Mat::zeros(2,1,CV_8UC1);
cv::Mat depth = cv::Mat::zeros(2,1,CV_16UC1);
CameraModel model(
1,
1,
0.5,
1,
Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
0,
cv::Size(1,2));
SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->header.stamp));
Transform gt;
if(!groundTruthFrameId_.empty())
{
gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
}
interData.setGroundTruth(gt);
rtabmap_.process(interData, interOdom, covariance);
}
interOdoms_.erase(iter++);
}
else if(iter->header.stamp == lastPoseStamp_)
{
interOdoms_.erase(iter++);
break;
}
else
{
break;
}
}
//Add async stuff
Transform groundTruthPose;
if(!groundTruthFrameId_.empty())
@@ -2103,6 +2177,14 @@ void CoreWrapper::imuAsyncCallback(const sensor_msgs::ImuConstPtr & msg)
}
}
void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg)
{
if(!paused_)
{
interOdoms_.push_back(*msg);
}
}
void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
{
@@ -2377,6 +2459,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
userData_ = cv::Mat();
userDataMutex_.unlock();
imus_.clear();
interOdoms_.clear();
return true;
}