Class to pause rtabmap

rtabmap_pauser.py

#!/usr/bin/env python

import os
from copy import deepcopy
import rospy
from tf2_msgs.msg import TFMessage


"""
Hacky class that pauses RTABMAP nodes
rtabmap and rgbd_odometry
to save CPU. It keeps publishing the last known
transform in between map->odom.
Also pauses mbf_costmap_nav.

Author: Sammy Pfeiffer <Sammy.Pfeiffer@student.uts.edu.au>
"""


class RTABMAPPauser(object):
    def __init__(self):
        self.tf_pub = rospy.Publisher('/tf', TFMessage,
                                      queue_size=1)
        self.last_msg = None
        self.timer = None
        self.tf_sub = rospy.Subscriber('/tf', TFMessage,
                                       self.tf_cb, queue_size=1)
        rospy.loginfo("RTABMAPPAuser initialized.")

    def tf_cb(self, msg):
        for t in msg.transforms:
            if t.header.frame_id == 'map' and \
               t.child_frame_id == 'odom':
                self.last_msg = t
                break

    def pub_last_transform(self, _):
        msg = deepcopy(self.last_msg)
        msg.header.stamp = rospy.Time.now()
        tfm = TFMessage()
        tfm.transforms.append(msg)
        self.tf_pub.publish(tfm)

    def pause_rtabmap(self):
        # wait for transform map -> odom
        while self.last_msg is None:
            rospy.sleep(0.1)
        # Keep publishing it at 20hz until rtabmap is re-started
        self.timer = rospy.Timer(rospy.Duration(0.05), self.pub_last_transform)
        os.system("killall -s STOP rtabmap rgbd_odometry mbf_costmap_nav")

    def continue_rtabmap(self):
        os.system("killall -s CONT rtabmap rgbd_odometry mbf_costmap_nav")
        self.timer.shutdown()


if __name__ == '__main__':
    rospy.init_node("rtabmap_pauser_test")
    rtabmap_manager = RTABMAPPauser()
    rtabmap_manager.pause_rtabmap()
    rospy.sleep(5.0)
    rtabmap_manager.continue_rtabmap()