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 pythonimport rospy
import copyfrom interactive_markers.interactive_marker_server import *
from visualization_msgs.msg import *
from geometry_msgs.msg import Point
from geometry_msgs.msg import Poseclass ObstaclePublisher:def __init__(self, obs_init_position: list):# self.server = Noneself.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 orientationself.make6DofMarker(False, InteractiveMarkerControl.MOVE_3D, position, False)self.ps = Pose()self.ps.position = position# a topic to publish obstacle's pose all the timeself.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.poseself.server.applyChanges()def makeBox(self, msg):marker = Marker()marker.type = Marker.SPHEREmarker.scale.x = msg.scale * 0.2marker.scale.y = msg.scale * 0.2marker.scale.z = msg.scale * 0.2marker.color.r = 0.8marker.color.g = 0.1marker.color.b = 0.1marker.color.a = 1.0return markerdef makeBoxControl(self, msg):control = InteractiveMarkerControl()control.always_visible = Truecontrol.markers.append(self.makeBox(msg))msg.controls.append(control)return control###################################################################### Marker Creationdef normalizeQuaternion(self, quaternion_msg):norm = quaternion_msg.x**2 + quaternion_msg.y**2 + quaternion_msg.z**2 + quaternion_msg.w**2s = norm ** (-0.5)quaternion_msg.x *= squaternion_msg.y *= squaternion_msg.z *= squaternion_msg.w *= sdef make6DofMarker(self, fixed, interaction_mode, position, show_6dof=False):int_marker = InteractiveMarker()int_marker.header.frame_id = "world"int_marker.pose.position = positionint_marker.scale = 1int_marker.name = "Obstacle"int_marker.description = "Obstacle"# insert a obstacleself.makeBoxControl(int_marker)int_marker.controls[0].interaction_mode = interaction_modeif show_6dof:control = InteractiveMarkerControl()control.orientation.w = 1control.orientation.x = 1control.orientation.y = 0control.orientation.z = 0self.normalizeQuaternion(control.orientation)control.name = "rotate_x"control.interaction_mode = InteractiveMarkerControl.ROTATE_AXISif fixed:control.orientation_mode = InteractiveMarkerControl.FIXEDint_marker.controls.append(control)control = InteractiveMarkerControl()control.orientation.w = 1control.orientation.x = 1control.orientation.y = 0control.orientation.z = 0self.normalizeQuaternion(control.orientation)control.name = "move_x"control.interaction_mode = InteractiveMarkerControl.MOVE_AXISif fixed:control.orientation_mode = InteractiveMarkerControl.FIXEDint_marker.controls.append(control)control = InteractiveMarkerControl()control.orientation.w = 1control.orientation.x = 0control.orientation.y = 1control.orientation.z = 0self.normalizeQuaternion(control.orientation)control.name = "rotate_z"control.interaction_mode = InteractiveMarkerControl.ROTATE_AXISif fixed:control.orientation_mode = InteractiveMarkerControl.FIXEDint_marker.controls.append(control)control = InteractiveMarkerControl()control.orientation.w = 1control.orientation.x = 0control.orientation.y = 1control.orientation.z = 0self.normalizeQuaternion(control.orientation)control.name = "move_z"control.interaction_mode = InteractiveMarkerControl.MOVE_AXISif fixed:control.orientation_mode = InteractiveMarkerControl.FIXEDint_marker.controls.append(control)control = InteractiveMarkerControl()control.orientation.w = 1control.orientation.x = 0control.orientation.y = 0control.orientation.z = 1self.normalizeQuaternion(control.orientation)control.name = "rotate_y"control.interaction_mode = InteractiveMarkerControl.ROTATE_AXISif fixed:control.orientation_mode = InteractiveMarkerControl.FIXEDint_marker.controls.append(control)control = InteractiveMarkerControl()control.orientation.w = 1control.orientation.x = 0control.orientation.y = 0control.orientation.z = 1self.normalizeQuaternion(control.orientation)control.name = "move_y"control.interaction_mode = InteractiveMarkerControl.MOVE_AXISif fixed:control.orientation_mode = InteractiveMarkerControl.FIXEDint_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()