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 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)