ORB-SLAM2 轨迹保存与ros::spin()相关问题(ctrl+c无法退出ros::spin()并进行轨迹保存,而是错误的直接退出主程序)

文章描述了一个在使用ROS处理RGBD图像时遇到的问题,即在ros::spin()过程中,使用ctrl+c无法优雅退出并保存轨迹。通过添加自定义的信号处理函数MySigintHandler,并在合适位置调用signal函数注册该处理函数,成功实现了在退出前保存数据和清理资源,从而能在按ctrl+c后继续执行后续程序。然而,当没有接收到rostopic信息时,仍然会直接退出程序,这个问题尚未解决。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

问题描述:使用ROS接收RGBD图像进行处理时(单目、双目可能也存在类似问题),在运行到ros::spin()前一切正常。当需要使用ctrl+c退出ros::spin()并继续后续代码时,出现触发一次ctrl+c直接退出整个程序的问题(无法保存轨迹)

解决:以ros_rgbd.cc为例:

           1.进入ros_rgbd.cc文件

           2.添加信号处理函数在文件中(ImageGrabber类内外都可)

void MySigintHandler(int sig)
{
    //这里主要进行退出前的数据保存、内存清理、告知其他节点等工作
    // sleep(3);
    ROS_INFO("shutting down!");
    ros::shutdown();
    sleep(1);
}

           2.在末尾Track主函数上下任意位置加入linux中的signal信号函数

signal(SIGINT, MySigintHandler);
mpSLAM->TrackRGBD(cv_ptrRGB->image, cv_ptrD->image, cv_ptrRGB->header.stamp.toSec());
// signal(SIGINT, MySigintHandler);

            3.重新编译,运行build_ros.sh

效果:解决无法保存轨迹文件的问题,在按ctrl+c后可以继续运行ros::spin()后续程序而不会直接退出程序

遗留问题:仍然无法解决当没有接收到rostopic信息时,按ctrl+c直接退出程序的问题(不过正常使用中也仅仅需要在运行一段时间后生成了关键帧数据的情况下才会保存轨迹,在没有任何图片数据时完全是空白的,无需保存轨迹)

### 集成ROSORB-SLAM2 为了使ORB-SLAM2能够在ROS环境中工作,通常的做法是创建一个专门的ROS包来封装ORB-SLAM2的功能。这涉及到编写C++代码以桥接两者之间的通信接口,确保数据流能够顺畅传递给ORB-SLAM2处理模块。 #### 创建ROS节点 首先,在`catkin_ws/src`目录下建立一个新的ROS包: ```bash cd ~/catkin_ws/src/ catkin_create_pkg orbslam2_ros std_msgs rospy roscpp cv_bridge image_transport sensor_msgs ``` 此命令创建名为`orbslam2_ros`的新包声明依赖项[^1]。 #### 修改CMakeLists.txt文件 编辑该包中的`CMakeLists.txt`文件,加入如下配置以便编译器知道如何链接外部库(即ORB-SLAM2源码)以及设置正确的路径指向安装好的第三方库如Pangolin用于可视化界面显示等。 ```cmake find_package(ORBSLAM2 REQUIRED PATHS "/path/to/orb_slam_2") include_directories(${ORBSLAM2_INCLUDE_DIRS}) link_directories(${ORBSLAM2_LIBRARY_DIRS}) add_definitions(-DUSE_ORBVOCABULARY=${ORBSLAM2_VOCABULARY_PATH}) # 添加其他必要的指令... ``` 上述脚本假设已下载构建好了ORB-SLAM2项目副本放在指定位置 `/path/to/orb_slam_2`. #### 编写订阅者和发布者类 接下来定义具体的逻辑实现部分——通过继承自`ros::NodeHandle`的对象操作消息队列收发图像帧及其他传感器输入作为算法前端的数据来源;同时也要负责广播姿态变换信息至TF树形结构内供下游组件读取使用。 ```cpp // src/node.cpp #include <ros/ros.h> #include <sensor_msgs/Image.h> #include <cv_bridge/cv_bridge.h> class ORB_SLAM2_ROS { public: ORB_SLAM2_ROS() : nh_("~"), orb_slam_(new ORB_SLAM2::System(voc_file_, settings_file_, Sensor::MONOCULAR)) {} private: ros::NodeHandle nh_; std::unique_ptr<ORB_SLAM2::System> orb_slam_; void ImageCallback(const sensor_msgs::ImageConstPtr& msg) { try { cv::Mat img = cv_bridge::toCvShare(msg)->image; double timestamp = msg->header.stamp.toSec(); // Process frame using ORB-SLAM2 system. orb_slam_->TrackMonocular(img, timestamp); } catch (cv_bridge::Exception& e) { ROS_ERROR("Could not convert from '%s' to 'mono8'.", msg->encoding.c_str()); } } }; int main(int argc, char **argv){ ros::init(argc, argv, "orb_slam2_node"); ORB_SLAM2_ROS node; ros::spin(); return 0; } ``` 这段程序展示了基本框架下的回调函数模板,每当收到新的图片时就会触发一次定位更新过程[^4]. #### 发布轨迹信息 最后一步是要让系统向外发送计算所得的位置估计结果。可以通过监听ORB-SLAM内部状态变化事件的方式获取最新位姿参数,再将其转换为标准格式的消息对象后经由特定话题名传播出去共享给感兴趣的应用层实体解析应用。 ```cpp void PublishPose(const Sophus::SE3f& Tcw) const{ geometry_msgs::PoseStamped pose_msg; tf::poseEigenToMsg(Tcw.matrix(), pose_msg.pose); static int id = 0; pose_msg.header.frame_id = "world"; pose_msg.header.seq = ++id; pose_pub_.publish(pose_msg); } tf::TransformListener listener_; ros::Publisher pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("camera_pose", 1); ``` 以上就是关于怎样把ORB-SLAM2融入到Robot Operating System平台上的概述说明[^5].
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值