mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +08:00
Merge branch 'ros2' of github.com:introlab/rtabmap_ros into jazzy-devel
This commit is contained in:
matlabbe
@@ -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()
|
||||
|
||||
@@ -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_;
|
||||
|
||||
@@ -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,13 @@ 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.");
|
||||
useActionForGoal_ = false;
|
||||
}
|
||||
#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_);
|
||||
@@ -633,45 +648,46 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
}
|
||||
|
||||
// setup services
|
||||
updateSrv_ = this->create_service<std_srvs::srv::Empty>("update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
resetSrv_ = this->create_service<std_srvs::srv::Empty>("reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
pauseSrv_ = this->create_service<std_srvs::srv::Empty>("pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
resumeSrv_ = this->create_service<std_srvs::srv::Empty>("resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
loadDatabaseSrv_ = this->create_service<rtabmap_msgs::srv::LoadDatabase>("load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
triggerNewMapSrv_ = this->create_service<std_srvs::srv::Empty>("trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
backupDatabase_ = this->create_service<std_srvs::srv::Empty>("backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
detectMoreLoopClosuresSrv_ = this->create_service<rtabmap_msgs::srv::DetectMoreLoopClosures>("detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
globalBundleAdjustmentSrv_ = this->create_service<rtabmap_msgs::srv::GlobalBundleAdjustment>("global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
cleanupLocalGridsSrv_ = this->create_service<rtabmap_msgs::srv::CleanupLocalGrids>("cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setModeLocalizationSrv_ = this->create_service<std_srvs::srv::Empty>("set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setModeMappingSrv_ = this->create_service<std_srvs::srv::Empty>("set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getNodeDataSrv_ = this->create_service<rtabmap_msgs::srv::GetNodeData>("get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getMapDataSrv_ = this->create_service<rtabmap_msgs::srv::GetMap>("get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getMapData2Srv_ = this->create_service<rtabmap_msgs::srv::GetMap2>("get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getMapSrv_ = this->create_service<nav_msgs::srv::GetMap>("get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getProbMapSrv_ = this->create_service<nav_msgs::srv::GetMap>("get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
publishMapDataSrv_ = this->create_service<rtabmap_msgs::srv::PublishMap>("publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getPlanSrv_ = this->create_service<nav_msgs::srv::GetPlan>("get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getPlanNodesSrv_ = this->create_service<rtabmap_msgs::srv::GetPlan>("get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setGoalSrv_ = this->create_service<rtabmap_msgs::srv::SetGoal>("set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
cancelGoalSrv_ = this->create_service<std_srvs::srv::Empty>("cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLabelSrv_ = this->create_service<rtabmap_msgs::srv::SetLabel>("set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
listLabelsSrv_ = this->create_service<rtabmap_msgs::srv::ListLabels>("list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
removeLabelSrv_ = this->create_service<rtabmap_msgs::srv::RemoveLabel>("remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
addLinkSrv_ = this->create_service<rtabmap_msgs::srv::AddLink>("add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getNodesInRadiusSrv_ = this->create_service<rtabmap_msgs::srv::GetNodesInRadius>("get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
const std::string servicePrefix = get_name() + std::string("/");
|
||||
updateSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
resetSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
pauseSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
resumeSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
loadDatabaseSrv_ = this->create_service<rtabmap_msgs::srv::LoadDatabase>(servicePrefix + "load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
triggerNewMapSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
backupDatabase_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
detectMoreLoopClosuresSrv_ = this->create_service<rtabmap_msgs::srv::DetectMoreLoopClosures>(servicePrefix + "detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
globalBundleAdjustmentSrv_ = this->create_service<rtabmap_msgs::srv::GlobalBundleAdjustment>(servicePrefix + "global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
cleanupLocalGridsSrv_ = this->create_service<rtabmap_msgs::srv::CleanupLocalGrids>(servicePrefix + "cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setModeLocalizationSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setModeMappingSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getNodeDataSrv_ = this->create_service<rtabmap_msgs::srv::GetNodeData>(servicePrefix + "get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getMapDataSrv_ = this->create_service<rtabmap_msgs::srv::GetMap>(servicePrefix + "get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getMapData2Srv_ = this->create_service<rtabmap_msgs::srv::GetMap2>(servicePrefix + "get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getMapSrv_ = this->create_service<nav_msgs::srv::GetMap>(servicePrefix + "get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getProbMapSrv_ = this->create_service<nav_msgs::srv::GetMap>(servicePrefix + "get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
publishMapDataSrv_ = this->create_service<rtabmap_msgs::srv::PublishMap>(servicePrefix + "publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getPlanSrv_ = this->create_service<nav_msgs::srv::GetPlan>(servicePrefix + "get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getPlanNodesSrv_ = this->create_service<rtabmap_msgs::srv::GetPlan>(servicePrefix + "get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setGoalSrv_ = this->create_service<rtabmap_msgs::srv::SetGoal>(servicePrefix + "set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
cancelGoalSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLabelSrv_ = this->create_service<rtabmap_msgs::srv::SetLabel>(servicePrefix + "set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
listLabelsSrv_ = this->create_service<rtabmap_msgs::srv::ListLabels>(servicePrefix + "list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
removeLabelSrv_ = this->create_service<rtabmap_msgs::srv::RemoveLabel>(servicePrefix + "remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
addLinkSrv_ = this->create_service<rtabmap_msgs::srv::AddLink>(servicePrefix + "add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getNodesInRadiusSrv_ = this->create_service<rtabmap_msgs::srv::GetNodesInRadius>(servicePrefix + "get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
|
||||
#ifdef WITH_OCTOMAP_MSGS
|
||||
#ifdef RTABMAP_OCTOMAP
|
||||
octomapBinarySrv_ = this->create_service<octomap_msgs::srv::GetOctomap>("octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
octomapFullSrv_ = this->create_service<octomap_msgs::srv::GetOctomap>("octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
octomapBinarySrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
octomapFullSrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
#endif
|
||||
#endif
|
||||
//private services
|
||||
setLogDebugSrv_ = this->create_service<std_srvs::srv::Empty>("log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogInfoSrv_ = this->create_service<std_srvs::srv::Empty>("log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogWarnSrv_ = this->create_service<std_srvs::srv::Empty>("log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogErrorSrv_ = this->create_service<std_srvs::srv::Empty>("log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogDebugSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogInfoSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogWarnSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogErrorSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
|
||||
int optimizeIterations = 0;
|
||||
Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations);
|
||||
@@ -732,7 +748,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
}
|
||||
auto node = rclcpp::Node::make_shared("rtabmap");
|
||||
image_transport::TransportHints hints(this);
|
||||
defaultSub_ = image_transport::create_subscription(node.get(), "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile());
|
||||
defaultSub_ = image_transport::create_subscription(node.get(), "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile());
|
||||
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "\n%s subscribed to:\n %s", get_name(), defaultSub_.getTopic().c_str());
|
||||
@@ -834,7 +850,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
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));
|
||||
republishNodeDataSub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>(servicePrefix+"republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1));
|
||||
|
||||
parametersClient_ = std::make_shared<rclcpp::SyncParametersClient>(this);
|
||||
auto on_parameter_event_callback =
|
||||
@@ -2199,7 +2215,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 +2229,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 +3986,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 +4387,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,17 +4419,16 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
|
||||
RCLCPP_ERROR(this->get_logger(), "Cannot connect to navigate_to_pose action server!");
|
||||
}
|
||||
}
|
||||
else
|
||||
#endif
|
||||
if(nextMetricGoalPub_->get_subscription_count())
|
||||
{
|
||||
nextMetricGoalPub_->publish(poseMsg);
|
||||
if(!useActionForGoal_)
|
||||
{
|
||||
lastPublishedMetricGoal_ = currentMetricGoal_;
|
||||
}
|
||||
lastPublishedMetricGoal_ = currentMetricGoal_;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#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)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user