网站开发要计入无形资产吗,一个网站的制作特点,网站开发 零基础,用户要承担暖气费的税吗以下代码实现了#xff1a;Interactive Marker通过topic一直发送其状态#xff0c;而不只是交互时才发送。 几个要点#xff1a;
通过定时器rospy.Timer实现PublishInteractiveMarkerServer feedback.pose的类型是geometry_msgs/Pose#xff0c;而不是geometry_msgs/PoseS…以下代码实现了Interactive Marker通过topic一直发送其状态而不只是交互时才发送。 几个要点
通过定时器rospy.Timer实现PublishInteractiveMarkerServer 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 obstacles pose all the timeself.pub rospy.Publisher(/obstacle_pose, Pose, queue_size1)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_6dofFalse):int_marker InteractiveMarker()int_marker.header.frame_id worldint_marker.pose.position positionint_marker.scale 1int_marker.name Obstacleint_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_xcontrol.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_xcontrol.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_zcontrol.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_zcontrol.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_ycontrol.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_ycontrol.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()
