Interactive Marker Publish Pose All the Time (Interactive Marker通过topic一直发送其状态)

以下代码实现了：Interactive Marker通过topic一直发送其状态，而不只是交互时才发送。
几个要点：

• 通过定时器rospy.Timer实现Publish
• InteractiveMarkerServer feedback.pose的类型是geometry_msgs/Pose，而不是geometry_msgs/PoseStamped

#!/usr/bin/env python

import rospy
import copy

from interactive_markers.interactive_marker_server import *
from visualization_msgs.msg import *
from geometry_msgs.msg import Point
from geometry_msgs.msg import Pose


class ObstaclePublisher:
    def __init__(self, obs_init_position: list):
        # self.server = None
        self.server = InteractiveMarkerServer("obstacle_controls")
        position = Point(obs_init_position[0], obs_init_position[1], obs_init_position[2])
        
        # include orientation
        # self.make6DofMarker(False, InteractiveMarkerControl.MOVE_ROTATE_3D, position, True)
        
        # without orientation
        self.make6DofMarker(False, InteractiveMarkerControl.MOVE_3D, position, False)

        self.ps = Pose()
        self.ps.position = position
        # a topic to publish obstacle's pose all the time
        self.pub = rospy.Publisher('/obstacle_pose', Pose, queue_size=1)
        rospy.Timer(rospy.Duration(0.02), self.publish_obs_pose)
        rospy.loginfo("Publishing pose of the obstacle at topic: " + str(self.pub.name))

        self.server.applyChanges()

    def processFeedback(self, feedback):
       	rospy.loginfo("You are operating the obstacle.")
        self.ps = feedback.pose
        self.server.applyChanges()

    def makeBox(self, msg):
        marker = Marker()
        marker.type = Marker.SPHERE
        marker.scale.x = msg.scale * 0.2
        marker.scale.y = msg.scale * 0.2
        marker.scale.z = msg.scale * 0.2
        marker.color.r = 0.8
        marker.color.g = 0.1
        marker.color.b = 0.1
        marker.color.a = 1.0

        return marker

    def makeBoxControl(self, msg):
        control = InteractiveMarkerControl()
        control.always_visible = True
        control.markers.append(self.makeBox(msg))
        msg.controls.append(control)
        return control

    #####################################################################
    # Marker Creation
    def normalizeQuaternion(self, quaternion_msg):
        norm = quaternion_msg.x**2 + quaternion_msg.y**2 + quaternion_msg.z**2 + quaternion_msg.w**2
        s = norm ** (-0.5)
        quaternion_msg.x *= s
        quaternion_msg.y *= s
        quaternion_msg.z *= s
        quaternion_msg.w *= s

    def make6DofMarker(self, fixed, interaction_mode, position, show_6dof=False):
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "world"
        int_marker.pose.position = position
        int_marker.scale = 1

        int_marker.name = "Obstacle"
        int_marker.description = "Obstacle"

        # insert a obstacle
        self.makeBoxControl(int_marker)
        int_marker.controls[0].interaction_mode = interaction_mode

        if show_6dof:
            control = InteractiveMarkerControl()
            control.orientation.w = 1
            control.orientation.x = 1
            control.orientation.y = 0
            control.orientation.z = 0
            self.normalizeQuaternion(control.orientation)
            control.name = "rotate_x"
            control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
            if fixed:
                control.orientation_mode = InteractiveMarkerControl.FIXED
            int_marker.controls.append(control)

            control = InteractiveMarkerControl()
            control.orientation.w = 1
            control.orientation.x = 1
            control.orientation.y = 0
            control.orientation.z = 0
            self.normalizeQuaternion(control.orientation)
            control.name = "move_x"
            control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
            if fixed:
                control.orientation_mode = InteractiveMarkerControl.FIXED
            int_marker.controls.append(control)

            control = InteractiveMarkerControl()
            control.orientation.w = 1
            control.orientation.x = 0
            control.orientation.y = 1
            control.orientation.z = 0
            self.normalizeQuaternion(control.orientation)
            control.name = "rotate_z"
            control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
            if fixed:
                control.orientation_mode = InteractiveMarkerControl.FIXED
            int_marker.controls.append(control)

            control = InteractiveMarkerControl()
            control.orientation.w = 1
            control.orientation.x = 0
            control.orientation.y = 1
            control.orientation.z = 0
            self.normalizeQuaternion(control.orientation)
            control.name = "move_z"
            control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
            if fixed:
                control.orientation_mode = InteractiveMarkerControl.FIXED
            int_marker.controls.append(control)

            control = InteractiveMarkerControl()
            control.orientation.w = 1
            control.orientation.x = 0
            control.orientation.y = 0
            control.orientation.z = 1
            self.normalizeQuaternion(control.orientation)
            control.name = "rotate_y"
            control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
            if fixed:
                control.orientation_mode = InteractiveMarkerControl.FIXED
            int_marker.controls.append(control)

            control = InteractiveMarkerControl()
            control.orientation.w = 1
            control.orientation.x = 0
            control.orientation.y = 0
            control.orientation.z = 1
            self.normalizeQuaternion(control.orientation)
            control.name = "move_y"
            control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
            if fixed:
                control.orientation_mode = InteractiveMarkerControl.FIXED
            int_marker.controls.append(control)

        self.server.insert(int_marker, self.processFeedback)

    def publish_obs_pose(self, *args):
        self.pub.publish(self.ps)


if __name__ == "__main__":
    rospy.init_node("obstacle_controls")

    op = ObstaclePublisher([0.6, 0.6, 0.6])

    rospy.spin()