i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 17:57:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added nav2 integration (fixed latched /map, connected nav2_msgs/NavigateToGoal action to rtabmap, updated turteblebot3 example launch file to work with navigation)

Browse Source
This commit is contained in:
matlabbe
2022-01-15 20:18:12 -05:00
parent 99db34506c
commit bc45e99ba5
9 changed files with 278 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CoreWrapper.h
+9 -13
View File
@@ -83,10 +83,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <apriltag_msgs/msg/april_tag_detection_array.hpp>
#endif
#ifdef WITH_MOVE_BASE_MSGS
#include <move_base_msgs/action/move_base.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#endif
//#define WITH_FIDUCIAL_MSGS
#ifdef WITH_FIDUCIAL_MSGS
@@ -106,6 +104,9 @@ public:
explicit CoreWrapper(const rclcpp::NodeOptions & options);
virtual ~CoreWrapper();
using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNav2 = rclcpp_action::ClientGoalHandle<NavigateToPose>;
private:
bool odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Time stamp);
bool odomTFUpdate(const rclcpp::Time & stamp); // TF odom
@@ -242,13 +243,10 @@ private:
void publishStats(const rclcpp::Time & stamp);
void publishCurrentGoal(const rclcpp::Time & stamp);
#ifdef WITH_MOVE_BASE_MSGS
using MoveBase = move_base_msgs::action::MoveBase;
using GoalHandleMoveBase = rclcpp_action::ClientGoalHandle<MoveBase>;
void goalResponseCallback(std::shared_future<GoalHandleMoveBase::SharedPtr> future);
void feedbackCallback(GoalHandleMoveBase::SharedPtr, const std::shared_ptr<const MoveBase::Feedback> feedback);
void resultCallback(const GoalHandleMoveBase::WrappedResult & result);
#endif
void goalResponseCallback(std::shared_future<GoalHandleNav2::SharedPtr> future);
void resultCallback(const GoalHandleNav2::WrappedResult & result);
void publishLocalPath(const rclcpp::Time & stamp);
void publishGlobalPath(const rclcpp::Time & stamp);
void republishMaps();
@@ -364,9 +362,7 @@ private:
rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapBinarySrv_;
rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapFullSrv_;
#endif
#ifdef WITH_MOVE_BASE_MSGS
rclcpp_action::Client<MoveBase>::SharedPtr moveBaseClient_;
#endif
rclcpp_action::Client<NavigateToPose>::SharedPtr nav2Client_;
std::thread* transformThread_;
bool tfThreadRunning_;