mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-03 16:27:46 +08:00
Merged master->ros2
This commit is contained in:
matlabbe
@@ -53,6 +53,7 @@ find_package(octomap_msgs)
|
||||
#find_package(apriltag_msgs)
|
||||
#find_package(find_object_2d)
|
||||
find_package(move_base_msgs)
|
||||
find_package(fiducial_msgs)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
find_package(RTABMap 0.20.15 REQUIRED)
|
||||
@@ -321,6 +322,19 @@ SET(Libraries
|
||||
ADD_DEFINITIONS("-DWITH_MOVE_BASE_MSGS")
|
||||
ENDIF(move_base_msgs_FOUND)
|
||||
|
||||
# If fiducial_msgs is found, add definition
|
||||
IF(fiducial_msgs_FOUND)
|
||||
MESSAGE(STATUS "WITH fiducial_msgs")
|
||||
include_directories(
|
||||
${fiducial_msgs_INCLUDE_DIRS}
|
||||
)
|
||||
SET(Libraries
|
||||
${fiducial_msgs_LIBRARIES}
|
||||
${Libraries}
|
||||
)
|
||||
ADD_DEFINITIONS("-DWITH_FIDUCIAL_MSGS")
|
||||
ENDIF(fiducial_msgs_FOUND)
|
||||
|
||||
############################
|
||||
## Declare a cpp library
|
||||
############################
|
||||
|
||||
@@ -88,6 +88,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#endif
|
||||
|
||||
//#define WITH_FIDUCIAL_MSGS
|
||||
#ifdef WITH_FIDUCIAL_MSGS
|
||||
#include <fiducial_msgs/FiducialTransformArray.h>
|
||||
#endif
|
||||
|
||||
namespace rtabmap {
|
||||
class StereoDense;
|
||||
}
|
||||
@@ -164,6 +169,9 @@ private:
|
||||
void gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedPtr gpsFixMsg);
|
||||
#ifdef WITH_APRILTAG_MSGS
|
||||
void tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr tagDetections);
|
||||
#endif
|
||||
#ifdef WITH_FIDUCIAL_MSGS
|
||||
void fiducialDetectionsAsyncCallback(const fiducial_msgs::msgs::FiducialTransformArray::SharedPtr fiducialDetections);
|
||||
#endif
|
||||
void imuAsyncCallback(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
void republishNodeDataCallback(const std_msgs::msg::Int32MultiArray::ConstSharedPtr msg);
|
||||
@@ -294,6 +302,7 @@ private:
|
||||
rclcpp::Publisher<rtabmap_ros::msg::Info>::SharedPtr infoPub_;
|
||||
rclcpp::Publisher<rtabmap_ros::msg::MapData>::SharedPtr mapDataPub_;
|
||||
rclcpp::Publisher<rtabmap_ros::msg::MapGraph>::SharedPtr mapGraphPub_;
|
||||
rclcpp::Publisher<rtabmap_ros::msg::MapGraph>::SharedPtr odomCachePub_;
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr landmarksPub_;
|
||||
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr labelsPub_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr mapPathPub_;
|
||||
@@ -375,9 +384,13 @@ private:
|
||||
rtabmap::GPS gps_;
|
||||
#ifdef WITH_APRILTAG_MSGS
|
||||
rclcpp::Subscription<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr tagDetectionsSub_;
|
||||
#endif
|
||||
#ifdef WITH_FIDUCIAL_MSGS
|
||||
rclcpp::Subscription<fiducial_msgs::msg::FiducialTransformArray>::SharedPtr fiducialTransfromsSub_;
|
||||
#endif
|
||||
std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > tags_; // id, <pose, size>
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub_;
|
||||
|
||||
std::map<double, rtabmap::Transform> imus_;
|
||||
std::string imuFrameId_;
|
||||
rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr republishNodeDataSub_;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
|
||||
<!-- -->
|
||||
<launch>
|
||||
|
||||
<!-- Bringup the Husky with SICK (2D LiDAR), realsense camera (RGB-D camera) and velodyne (3D LiDAR):
|
||||
|
||||
@@ -0,0 +1,76 @@
|
||||
<!-- -->
|
||||
<launch>
|
||||
<!-- For ref: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/tutorial_ros_navigation.html#isaac-sim-app-tutorial-ros-navigation
|
||||
|
||||
1) In Isaac sim, click on menu Isaac Examples - ROS - Navigation
|
||||
2) Terminal: $ roscore
|
||||
3) To teleop: $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py _speed:=0.15 _turn:=0.20
|
||||
4) Navigation: roslaunch rtabmap_ros demo_isaac_carter_navigation.launch
|
||||
5) Set max depth range to 10m in rtabmapviz for correct visualization (Preferences->3D Rendering under map and odom columns)
|
||||
6) Press Play in Isaac sim
|
||||
|
||||
Note: carter_2dnav package can be copied to your catkin_ws from:
|
||||
$ cp -r ~/.local/share/ov/pkg/isaac_sim-2021.2.0/ros_workspace/src/* ~/catkin_ws/src/.
|
||||
|
||||
For Lidar 3D Mode (lidar3d:=true), make sure in isaac sim that Carter robot is set with VLP16-like parameters (16 rings):
|
||||
Under World -> Carter_ROS -> chassis_link -> carter_lidar -> highLod = True
|
||||
-> verticalFov = 32
|
||||
-> verticalResolution = 2
|
||||
-> ROS_lidar -> pointCloudEnabled = True
|
||||
-->
|
||||
|
||||
<param name="use_sim_time" value="true" />
|
||||
<arg name="localization" default="false" />
|
||||
|
||||
<arg name="lidar3d" default="false" /> <!-- Best results if rtabmap is built with libpointmatcher -->
|
||||
<arg name="lidar3d_ray_tracing" default="true" />
|
||||
<arg name="lidar3d_grid3d" default="true" />
|
||||
|
||||
<!-- Load Robot Description -->
|
||||
<arg name="model" default="$(find carter_description)/urdf/carter.urdf"/>
|
||||
<param name="robot_description" textfile="$(arg model)" />
|
||||
|
||||
<!-- Lidar 3D based on demo_husky.launch -->
|
||||
<arg if="$(arg lidar3d)" name="cell_size" default="0.2" />
|
||||
<arg unless="$(arg lidar3d)" name="cell_size" default="0.05" />
|
||||
<arg if="$(arg lidar3d)" name="args3d" value="--Icp/Iterations 10 --Icp/PointToPlaneGroundNormalsUp 0.9 --Icp/PointToPlaneRadius 0 --Icp/MaxCorrespondenceDistance 1 --Grid/ClusterRadius 1 --Grid/RangeMax 10 --Grid/RayTracing $(arg lidar3d_ray_tracing) --Grid/CellSize $(arg cell_size) --Mem/LaserScanNormalK --Grid/3D $(arg lidar3d_grid3d)" />
|
||||
<arg unless="$(arg lidar3d)" name="args3d" value="--Grid/RangeMax 10" /> <!-- 2d scan, use default params -->
|
||||
|
||||
<!-- RTAB-Map -->
|
||||
<remap from="/rtabmap/grid_map" to="/map"/>
|
||||
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
|
||||
<arg if="$(arg localization)" name="args" value="--Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true $(arg args3d)"/>
|
||||
<arg unless="$(arg localization)" name="args" value="-d --Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true $(arg args3d)"/>
|
||||
<arg name="localization" value="$(arg localization)"/>
|
||||
<arg if="$(arg lidar3d)" name="subscribe_scan_cloud" value="true"/>
|
||||
<arg unless="$(arg lidar3d)" name="subscribe_scan" value="true"/>
|
||||
<arg name="visual_odometry" value="false"/>
|
||||
<arg name="odom_topic" value="/odom"/>
|
||||
<arg name="frame_id" value="base_link"/>
|
||||
<arg name="depth_topic" value="/depth_left"/>
|
||||
<arg name="rgb_topic" value="/rgb_left"/>
|
||||
<arg name="camera_info_topic" value="/camera_info_left"/>
|
||||
<arg name="scan_topic" value="/scan"/>
|
||||
<arg name="scan_cloud_topic" value="/point_cloud"/>
|
||||
<arg name="rgbd_sync" value="true"/>
|
||||
<arg name="approx_rgbd_sync" value="false"/>
|
||||
<arg name="use_sim_time" value="true"/>
|
||||
|
||||
<arg name="scan_cloud_assembling" value="$(arg lidar3d)"/>
|
||||
<arg name="scan_cloud_assembling_fixed_frame" value="odom"/>
|
||||
<arg name="scan_cloud_assembling_range_max" value="60"/>
|
||||
<arg name="scan_cloud_assembling_voxel_size" value="$(arg cell_size)"/>
|
||||
</include>
|
||||
|
||||
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
|
||||
<param name="TrajectoryPlannerROS/max_vel_theta" value="0.3"/>
|
||||
<param name="TrajectoryPlannerROS/min_vel_theta" value="-0.3"/>
|
||||
<rosparam file="$(find carter_2dnav)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
|
||||
<rosparam file="$(find carter_2dnav)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
|
||||
<rosparam file="$(find carter_2dnav)/params/local_costmap_params.yaml" command="load" />
|
||||
<rosparam file="$(find carter_2dnav)/params/global_costmap_params.yaml" command="load" />
|
||||
<rosparam file="$(find carter_2dnav)/params/base_local_planner_params.yaml" command="load" />
|
||||
</node>
|
||||
|
||||
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find carter_2dnav)/rviz/carter_2dnav.rviz" />
|
||||
</launch>
|
||||
@@ -296,6 +296,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
("user_data_async", LaunchConfiguration('user_data_async_topic')),
|
||||
("gps/fix", LaunchConfiguration('gps_topic')),
|
||||
("tag_detections", LaunchConfiguration('tag_topic')),
|
||||
("fiducial_transforms", LaunchConfiguration('fiducial_topic')),
|
||||
("odom", LaunchConfiguration('odom_topic')),
|
||||
("imu", LaunchConfiguration('imu_topic'))],
|
||||
arguments=[LaunchConfiguration("args")],
|
||||
@@ -465,6 +466,7 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('tag_topic', default_value='/tag_detections', description='AprilTag topic async subscription. This is used for SLAM graph optimization and loop closure detection. Landmark poses are also published accordingly to current optimized map.'),
|
||||
DeclareLaunchArgument('tag_linear_variance', default_value='0.0001', description=''),
|
||||
DeclareLaunchArgument('tag_angular_variance', default_value='9999.0', description='>=9999 means rotation is ignored in optimization, when rotation estimation of the tag is not reliable or not computed.'),
|
||||
DeclareLaunchArgument('fiducial_topic', default_value='/fiducial_transforms', description='aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covariance.'),
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
|
||||
|
||||
@@ -142,6 +142,7 @@
|
||||
<arg name="tag_topic" default="/tag_detections" /> <!-- apriltags async subscription -->
|
||||
<arg name="tag_linear_variance" default="0.0001" />
|
||||
<arg name="tag_angular_variance" default="9999" /> <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
|
||||
<arg name="fiducial_topic" default="/fiducial_transforms" /> <!-- aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covriance -->
|
||||
|
||||
<!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
|
||||
<arg if="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)_relay"/>
|
||||
@@ -382,6 +383,7 @@
|
||||
<remap from="user_data_async" to="$(arg user_data_async_topic)"/>
|
||||
<remap from="gps/fix" to="$(arg gps_topic)"/>
|
||||
<remap from="tag_detections" to="$(arg tag_topic)"/>
|
||||
<remap from="fiducial_transforms" to="$(arg fiducial_topic)"/>
|
||||
<remap from="odom" to="$(arg odom_topic)"/>
|
||||
<remap from="imu" to="$(arg imu_topic)"/>
|
||||
|
||||
|
||||
@@ -45,3 +45,6 @@ float32[] stats_values
|
||||
# std::vector<int> local_path
|
||||
int32[] local_path
|
||||
int32 current_goal_id
|
||||
|
||||
# std::vector<int> odomCache
|
||||
MapGraph odom_cache
|
||||
|
||||
+63
-1
@@ -245,6 +245,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
infoPub_ = this->create_publisher<rtabmap_ros::msg::Info>("info", 1);
|
||||
mapDataPub_ = this->create_publisher<rtabmap_ros::msg::MapData>("mapData", 1);
|
||||
mapGraphPub_ = this->create_publisher<rtabmap_ros::msg::MapGraph>("mapGraph", 1);
|
||||
odomCachePub_ = this->create_publisher<rtabmap_ros::msg::MapGraph>("mapOdomCache", 1);
|
||||
landmarksPub_ = this->create_publisher<geometry_msgs::msg::PoseArray>("landmarks", 1);
|
||||
labelsPub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("labels", 1);
|
||||
mapPathPub_ = this->create_publisher<nav_msgs::msg::Path>("mapPath", 1);
|
||||
@@ -792,6 +793,9 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
gpsFixAsyncSub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("gps/fix", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1));
|
||||
#ifdef WITH_APRILTAG_MSGS
|
||||
tagDetectionsSub_ = this->create_subscription<apriltag_ros::msg::AprilTagDetectionArray>("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1));
|
||||
#endif
|
||||
#ifdef WITH_FIDUCIAL_MSGS
|
||||
fiducialTransfromsSub_ = this->create_subscription<fiducial_msgs::msg::FiducialTransformArray>("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1));
|
||||
#endif
|
||||
imuSub_ = this->create_subscription<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1));
|
||||
republishNodeDataSub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>("republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1));
|
||||
@@ -964,7 +968,10 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti
|
||||
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg.pose.pose);
|
||||
if(!odom.isNull())
|
||||
{
|
||||
Transform odomTF = rtabmap_ros::getTransform(odomMsg.header.frame_id, frameId_, stamp, *tfBuffer_, waitForTransform_);
|
||||
Transform odomTF;
|
||||
if(!stamp.seconds() == 0.0) {
|
||||
odomTF = rtabmap_ros::getTransform(odomMsg.header.frame_id, frameId_, stamp, *tfBuffer_, waitForTransform_);
|
||||
}
|
||||
if(odomTF.isNull())
|
||||
{
|
||||
static bool shown = false;
|
||||
@@ -2442,6 +2449,27 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagD
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef WITH_FIDUCIAL_MSGS
|
||||
void CoreWrapper::fiducialDetectionsAsyncCallback(const fiducial_msgs::msg::FiducialTransformArray::SharedPtr fiducialDetections)
|
||||
{
|
||||
if(!paused_)
|
||||
{
|
||||
for(unsigned int i=0; i<fiducialDetections.transforms.size(); ++i)
|
||||
{
|
||||
geometry_msgs::PoseWithCovarianceStamped p;
|
||||
p.pose.pose.orientation = fiducialDetections.transforms[i].transform.rotation;
|
||||
p.pose.pose.position.x = fiducialDetections.transforms[i].transform.translation.x;
|
||||
p.pose.pose.position.y = fiducialDetections.transforms[i].transform.translation.y;
|
||||
p.pose.pose.position.z = fiducialDetections.transforms[i].transform.translation.z;
|
||||
p.header = fiducialDetections.header;
|
||||
uInsert(tags_,
|
||||
std::make_pair(fiducialDetections.transforms[i].fiducial_id,
|
||||
std::make_pair(p, 0.0f)));
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void CoreWrapper::imuAsyncCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
|
||||
{
|
||||
if(!paused_)
|
||||
@@ -4128,6 +4156,40 @@ void CoreWrapper::publishStats(const rclcpp::Time & stamp)
|
||||
mapGraphPub_->publish(std::move(msg));
|
||||
}
|
||||
|
||||
if(odomCachePub_->get_subscription_count())
|
||||
{
|
||||
rtabmap_ros::msg::MapGraph::UniquePtr msg(new rtabmap_ros::msg::MapGraph);
|
||||
msg->header.stamp = stamp;
|
||||
msg->header.frame_id = mapFrameId_;
|
||||
|
||||
// For visualization of the constraints (MapGraph rviz plugin), we should include target nodes from the map
|
||||
std::map<int, Transform> poses = stats.odomCachePoses();
|
||||
// transform in map frame
|
||||
for(std::map<int, Transform>::iterator iter=poses.begin();
|
||||
iter!=poses.end();
|
||||
++iter)
|
||||
{
|
||||
iter->second = stats.mapCorrection() * iter->second;
|
||||
}
|
||||
for(std::multimap<int, rtabmap::Link>::const_iterator iter=stats.odomCacheConstraints().begin();
|
||||
iter!=stats.odomCacheConstraints().end();
|
||||
++iter)
|
||||
{
|
||||
std::map<int, Transform>::const_iterator pter = stats.poses().find(iter->second.to());
|
||||
if(pter != stats.poses().end())
|
||||
{
|
||||
poses.insert(*pter);
|
||||
}
|
||||
}
|
||||
rtabmap_ros::mapGraphToROS(
|
||||
poses,
|
||||
stats.odomCacheConstraints(),
|
||||
stats.mapCorrection(),
|
||||
*msg);
|
||||
|
||||
odomCachePub_->publish(std::move(msg));
|
||||
}
|
||||
|
||||
if(localGridObstacle_->get_subscription_count() && !stats.getLastSignatureData().sensorData().gridObstacleCellsRaw().empty())
|
||||
{
|
||||
pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(stats.getLastSignatureData().sensorData().gridObstacleCellsRaw()));
|
||||
|
||||
@@ -513,6 +513,13 @@ void infoFromROS(const rtabmap_ros::msg::Info & info, rtabmap::Statistics & stat
|
||||
stat.setLocalPath(info.local_path);
|
||||
stat.setCurrentGoalId(info.current_goal_id);
|
||||
|
||||
std::map<int, rtabmap::Transform> poses;
|
||||
std::multimap<int, rtabmap::Link> constraints;
|
||||
rtabmap::Transform t;
|
||||
mapGraphFromROS(info.odom_cache, poses, constraints, t);
|
||||
stat.setOdomCachePoses(poses);
|
||||
stat.setOdomCacheConstraints(constraints);
|
||||
|
||||
// Statistics data
|
||||
for(unsigned int i=0; i<info.stats_keys.size() && i<info.stats_values.size(); i++)
|
||||
{
|
||||
@@ -548,6 +555,7 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::msg::Info & info)
|
||||
info.labels_values = uValues(stats.labels());
|
||||
info.local_path = stats.localPath();
|
||||
info.current_goal_id = stats.currentGoalId();
|
||||
mapGraphToROS(stats.odomCachePoses(), stats.odomCacheConstraints(), stats.mapCorrection(), info.odom_cache);
|
||||
|
||||
// Statistics data
|
||||
info.stats_keys = uKeys(stats.data());
|
||||
|
||||
Reference in New Issue
Block a user