def applyChanges(self): """Apply changes made since the last call to this method and broadcast to clients.""" with self.mutex: if len(self.pending_updates.keys()) == 0: return update_msg = InteractiveMarkerUpdate() update_msg.type = InteractiveMarkerUpdate.UPDATE for name, update in self.pending_updates.items(): if update.update_type == UpdateContext.FULL_UPDATE: if name in self.marker_contexts: marker_context = self.marker_contexts[name] else: self.node.get_logger().debug( 'Creating new context for ' + name) # create a new int_marker context marker_context = MarkerContext( self.node.get_clock().now()) marker_context.default_feedback_callback = update.default_feedback_callback marker_context.feedback_callbacks = update.feedback_callbacks self.marker_contexts[name] = marker_context marker_context.int_marker = update.int_marker update_msg.markers.append(marker_context.int_marker) elif update.update_type == UpdateContext.POSE_UPDATE: if name not in self.marker_contexts: self.node.get_logger().error( 'Pending pose update for non-existing marker found. ' 'This is a bug in InteractiveMarkerServer.') continue marker_context = self.marker_contexts[name] marker_context.int_marker.pose = update.int_marker.pose marker_context.int_marker.header = update.int_marker.header pose_update = InteractiveMarkerPose() pose_update.header = marker_context.int_marker.header pose_update.pose = marker_context.int_marker.pose pose_update.name = marker_context.int_marker.name update_msg.poses.append(pose_update) elif update.update_type == UpdateContext.ERASE: if name in self.marker_contexts: marker_context = self.marker_contexts[name] del self.marker_contexts[name] update_msg.erases.append(name) self.pending_updates = {} self.seq_num += 1 self.publish(update_msg)
def applyChanges(self): with self.mutex: if len(self.pending_updates.keys()) == 0: return update_msg = InteractiveMarkerUpdate() update_msg.type = InteractiveMarkerUpdate.UPDATE for name, update in self.pending_updates.items(): if update.update_type == UpdateContext.FULL_UPDATE: try: marker_context = self.marker_contexts[name] except: rospy.logdebug("Creating new context for " + name) # create a new int_marker context marker_context = MarkerContext() marker_context.default_feedback_cb = update.default_feedback_cb marker_context.feedback_cbs = update.feedback_cbs self.marker_contexts[name] = marker_context marker_context.int_marker = update.int_marker update_msg.markers.append(marker_context.int_marker) elif update.update_type == UpdateContext.POSE_UPDATE: try: marker_context = self.marker_contexts[name] marker_context.int_marker.pose = update.int_marker.pose marker_context.int_marker.header = update.int_marker.header pose_update = InteractiveMarkerPose() pose_update.header = marker_context.int_marker.header pose_update.pose = marker_context.int_marker.pose pose_update.name = marker_context.int_marker.name update_msg.poses.append(pose_update) except: rospy.logerr("""\ Pending pose update for non-existing marker found. This is a bug in InteractiveMarkerInterface.""" ) elif update.update_type == UpdateContext.ERASE: try: marker_context = self.marker_contexts[name] del self.marker_contexts[name] update_msg.erases.append(name) except: pass self.pending_updates = dict() self.seq_num += 1 self.publish(update_msg) self.publishInit()
def applyChanges(self): with self.mutex: if len(self.pending_updates.keys()) == 0: return update_msg = InteractiveMarkerUpdate() update_msg.type = InteractiveMarkerUpdate.UPDATE for name, update in self.pending_updates.items(): if update.update_type == UpdateContext.FULL_UPDATE: try: marker_context = self.marker_contexts[name] except: rospy.logdebug("Creating new context for " + name) # create a new int_marker context marker_context = MarkerContext() marker_context.default_feedback_cb = update.default_feedback_cb marker_context.feedback_cbs = update.feedback_cbs self.marker_contexts[name] = marker_context marker_context.int_marker = update.int_marker update_msg.markers.append(marker_context.int_marker) elif update.update_type == UpdateContext.POSE_UPDATE: try: marker_context = self.marker_contexts[name] marker_context.int_marker.pose = update.int_marker.pose marker_context.int_marker.header = update.int_marker.header pose_update = InteractiveMarkerPose() pose_update.header = marker_context.int_marker.header pose_update.pose = marker_context.int_marker.pose pose_update.name = marker_context.int_marker.name update_msg.poses.append(pose_update) except: rospy.logerr("""\ Pending pose update for non-existing marker found. This is a bug in InteractiveMarkerInterface.""") elif update.update_type == UpdateContext.ERASE: try: marker_context = self.marker_contexts[name] del self.marker_contexts[name] update_msg.erases.append(name) except: pass self.pending_updates = dict() self.seq_num += 1 self.publish(update_msg) self.publishInit()
def __init__(self, root_topic): self.locker = Lock() self.obstacle = None self.root_topic = '/' + root_topic self.update_ = rospy.Publisher(self.root_topic + "/update", InteractiveMarkerUpdate, queue_size=100) self.init_ = rospy.Publisher(self.root_topic + '/update_full', InteractiveMarkerInit, queue_size=100) self.projection_ = rospy.Publisher(self.root_topic + '/projection', PointStamped, queue_size=1) self.seq_num = 0 self.server_id = self.root_topic self.Current = InteractiveMarkerUpdate() #self.Candidate = InteractiveMarkerFeedback() self.CurrentP = PointStamped() rospy.Subscriber(self.root_topic + '/feedback', InteractiveMarkerFeedback, self.RvizFeedback, queue_size=10) rospy.Timer(rospy.Duration(0.1), self.standbycb) self.PublishInit()
def ClearAll(self, event): print 'clear data' self.update_ = rospy.Publisher(self.root_topic+"/update", InteractiveMarkerUpdate ,queue_size=10) self.init_ = rospy.Publisher(self.root_topic+'/update_full', InteractiveMarkerInit, queue_size=10) self.projection_ = rospy.Publisher(self.root_topic+'/projection', PoseArray, queue_size=1) self.Current = InteractiveMarkerUpdate() self.Candidate = InteractiveMarkerFeedback() self.CurrentP = Pose() self.PublishInit()
def talker(): pub = rospy.Publisher("/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update", InteractiveMarkerUpdate) rospy.init_node('talker', anonymous=True) r = rospy.Rate(1) int_marker = InteractiveMarker() int_marker.name = "EE:goal_link_t" controller = InteractiveMarkerControl() controller.name = '_u1' int_marker.controls = controller p = InteractiveMarkerPose() updater = InteractiveMarkerUpdate() while not rospy.is_shutdown(): p.pose.position.x += 0.5 updater.markers = int_marker updater.poses = p print updater pub.publish(updater) r.sleep()
def ClearAll(self, event): #with self.locker: self.update_ = rospy.Publisher(self.root_topic + "/update", InteractiveMarkerUpdate, queue_size=1) self.init_ = rospy.Publisher(self.root_topic + '/update_full', InteractiveMarkerInit, queue_size=1) self.Current = InteractiveMarkerUpdate() self.Candidate = InteractiveMarkerFeedback() self.CurrentP = Pose() self.PublishInit()
def define(self, root_topic): self.locker = Lock() self.q_size = 10 self.seq_num = 0 self.root_topic = '/' + root_topic self.server_id = root_topic self.obstacle = None self.Current = InteractiveMarkerUpdate() self.Candidate = InteractiveMarkerFeedback() self.CurrentP = Pose() self.Marker_Context = dict() self.update_ = rospy.Publisher(self.root_topic + "/update", InteractiveMarkerUpdate, queue_size=self.q_size) self.init_ = rospy.Publisher(self.root_topic + '/update_full', InteractiveMarkerInit, queue_size=self.q_size)
def standbycb(self, event): with self.locker: standby = InteractiveMarkerUpdate() standby.type = InteractiveMarkerUpdate.KEEP_ALIVE self.PublishCB(standby)
def keepAlive(self, msg): empty_msg = InteractiveMarkerUpdate() empty_msg.type = empty_msg.KEEP_ALIVE self.publish(empty_msg)
def StandBy(self, event): #with self.locker: stand_BY = InteractiveMarkerUpdate() stand_BY.type = InteractiveMarkerUpdate.KEEP_ALIVE self.UpdateCB(stand_BY)
def standbycb(self, event): with self.locker: standby=InteractiveMarkerUpdate() standby.type = InteractiveMarkerUpdate.KEEP_ALIVE self.PublishCB(standby)
def StandBy(self, event): #with self.locker: stand_BY=InteractiveMarkerUpdate() stand_BY.type = InteractiveMarkerUpdate.KEEP_ALIVE self.UpdateCB(stand_BY)