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

Making nav2_msgs optional (not available yet on jazzy). Fixed compilation errors on jazzy with vision_opencv related h->hpp headers.

Browse Source
This commit is contained in:
matlabbe
2024-06-04 22:42:06 -07:00
parent b8741910e4
commit 911e4e8941
31 changed files with 139 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_slam/CMakeLists.txt
+15 -7
View File
@@ -14,7 +14,6 @@ find_package(ament_cmake REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
@@ -28,12 +27,9 @@ find_package(rtabmap_msgs REQUIRED)
find_package(rtabmap_util REQUIRED)
find_package(rtabmap_sync REQUIRED)
IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0)
ADD_DEFINITIONS("-DNAV_MSGS_FOXY")
ENDIF()
#optional
find_package(apriltag_msgs)
find_package(nav2_msgs)
IF(WIN32)
add_compile_options(-bigobj)
@@ -48,7 +44,6 @@ SET(Libraries
cv_bridge
geometry_msgs
nav_msgs
nav2_msgs
rclcpp
rclcpp_components
sensor_msgs
@@ -80,6 +75,19 @@ SET(Libraries
)
ENDIF(apriltag_msgs_FOUND)
# If nav2_msgs is found, add definition
IF(nav2_msgs_FOUND)
MESSAGE(STATUS "WITH nav2_msgs")
ADD_DEFINITIONS("-DWITH_NAV2_MSGS")
SET(Libraries
${Libraries}
nav2_msgs
)
IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0)
ADD_DEFINITIONS("-DNAV_MSGS_FOXY")
ENDIF()
ENDIF(nav2_msgs_FOUND)
############################
## Declare a cpp library
############################
@@ -128,4 +136,4 @@ install(DIRECTORY include/
FILES_MATCHING PATTERN "*.h"
)
ament_package()
ament_package()
rtabmap_slam/include/rtabmap_slam/CoreWrapper.h
+8
View File
@@ -88,8 +88,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <apriltag_msgs/msg/april_tag_detection_array.hpp>
#endif
#ifdef WITH_NAV2_MSGS
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#endif
//#define WITH_FIDUCIAL_MSGS
#ifdef WITH_FIDUCIAL_MSGS
@@ -109,8 +111,10 @@ public:
explicit CoreWrapper(const rclcpp::NodeOptions & options);
virtual ~CoreWrapper();
#ifdef WITH_NAV2_MSGS
using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNav2 = rclcpp_action::ClientGoalHandle<NavigateToPose>;
#endif
private:
bool odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Time stamp);
@@ -246,12 +250,14 @@ private:
void publishStats(const rclcpp::Time & stamp);
void publishCurrentGoal(const rclcpp::Time & stamp);
#ifdef WITH_NAV2_MSGS
#ifdef NAV_MSGS_FOXY
void goalResponseCallback(std::shared_future<GoalHandleNav2::SharedPtr> future);
#else
void goalResponseCallback(const GoalHandleNav2::SharedPtr & goal_handle);
#endif
void resultCallback(const GoalHandleNav2::WrappedResult & result);
#endif
void publishLocalPath(const rclcpp::Time & stamp);
void publishGlobalPath(const rclcpp::Time & stamp);
@@ -373,7 +379,9 @@ private:
rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapBinarySrv_;
rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapFullSrv_;
#endif
#ifdef WITH_NAV2_MSGS
rclcpp_action::Client<NavigateToPose>::SharedPtr nav2Client_;
#endif
std::thread* transformThread_;
bool tfThreadRunning_;
rtabmap_slam/src/CoreWrapper.cpp
+26 -2
View File
@@ -36,9 +36,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <std_msgs/msg/bool.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <sensor_msgs/image_encodings.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <pcl_conversions/pcl_conversions.h>
#if PCL_VERSION_COMPARE(>, 1, 12, 0)
#include <pcl/common/io.h>
#else
#include <pcl/io/io.h>
#endif
#include <visualization_msgs/msg/marker_array.hpp>
@@ -196,6 +204,12 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
initialPoseStr = this->declare_parameter("initial_pose", initialPoseStr);
useActionForGoal_ = this->declare_parameter("use_action_for_goal", useActionForGoal_);
#ifndef WITH_NAV2_MSGS
if(useActionForGoal_)
{
RCLCPP_ERROR(this->get_logger(), "rtabmap: Cannot enable use_action_for_goal because rtabmap_slam is not built with nav2_msgs support.");
}
#endif
useSavedMap_ = this->declare_parameter("use_saved_map", useSavedMap_);
genScan_ = this->declare_parameter("gen_scan", genScan_);
genScanMaxDepth_ = this->declare_parameter("gen_scan_max_depth", genScanMaxDepth_);
@@ -2199,7 +2213,11 @@ void CoreWrapper::process(
{
// Don't send status yet if nav2 actionlib is used unless it failed,
// let nav2 finish reaching the goal
#ifdef WITH_NAV2_MSGS
if(nav2Client_ == 0 || rtabmap_.getPathStatus() <= 0)
#else
if(rtabmap_.getPathStatus() <= 0)
#endif
{
if(rtabmap_.getPathStatus() > 0)
{
@@ -2209,10 +2227,12 @@ void CoreWrapper::process(
else if(rtabmap_.getPathStatus() <= 0)
{
RCLCPP_WARN(this->get_logger(), "Planning: Plan failed!");
#ifdef WITH_NAV2_MSGS
if(nav2Client_.get()!=NULL && nav2Client_->action_server_is_ready())
{
nav2Client_->async_cancel_all_goals();
}
#endif
}
if(goalReachedPub_->get_subscription_count())
@@ -3964,11 +3984,12 @@ void CoreWrapper::cancelGoalCallback(
goalReachedPub_->publish(result);
}
}
#ifdef WITH_NAV2_MSGS
if(nav2Client_.get() != NULL && nav2Client_->action_server_is_ready())
{
nav2Client_->async_cancel_all_goals();
}
#endif
}
void CoreWrapper::setLabelCallback(
@@ -4364,6 +4385,7 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
poseMsg.header.frame_id = mapFrameId_;
poseMsg.header.stamp = stamp;
rtabmap_conversions::transformToPoseMsg(currentMetricGoal_, poseMsg.pose);
#ifdef WITH_NAV2_MSGS
if(useActionForGoal_)
{
if(nav2Client_.get() == NULL || !nav2Client_->action_server_is_ready())
@@ -4395,6 +4417,7 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
RCLCPP_ERROR(this->get_logger(), "Cannot connect to navigate_to_pose action server!");
}
}
#endif
if(nextMetricGoalPub_->get_subscription_count())
{
nextMetricGoalPub_->publish(poseMsg);
@@ -4405,7 +4428,7 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
}
}
}
#ifdef WITH_NAV2_MSGS
void CoreWrapper::goalResponseCallback(
#ifdef NAV_MSGS_FOXY
std::shared_future<GoalHandleNav2::SharedPtr> future)
@@ -4473,6 +4496,7 @@ void CoreWrapper::resultCallback(
latestNodeWasReached_ = false;
}
}
#endif
void CoreWrapper::publishLocalPath(const rclcpp::Time & stamp)
{