Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

publish smach viewer image #37

Merged
merged 2 commits into from
Jul 23, 2022
Merged

publish smach viewer image #37

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4c3e4c3
publish smach viewer image
knorth55 Jul 8, 2021
68df466
Merge branch 'melodic-devel' into image-publish
knorth55 Jul 20, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 25 additions & 1 deletion smach_viewer/scripts/smach_viewer.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
import roslib

from smach_msgs.msg import SmachContainerStatus,SmachContainerInitialStatusCmd,SmachContainerStructure
from sensor_msgs.msg import Image

import sys
import os
Expand All @@ -45,6 +46,8 @@
import StringIO
import colorsys
import time
import cv_bridge
import numpy as np
import base64

import wxversion
Expand Down Expand Up @@ -625,6 +628,8 @@ def __init__(self):
self._structure_subs = {}
self._status_subs = {}

self._pub = rospy.Publisher('~image', Image, queue_size=1)

self.Bind(wx.EVT_IDLE,self.OnIdle)
self.Bind(wx.EVT_CLOSE,self.OnQuit)

Expand All @@ -644,6 +649,11 @@ def __init__(self):
self._update_tree_thread = threading.Thread(target=self._update_tree)
self._update_tree_thread.start()

self.timer = wx.Timer(self, 0)
self.Bind(wx.EVT_TIMER, self.OnTimer)
self.timer.Start(200)


def OnQuit(self,event):
"""Quit Event: kill threads and wait for join."""
with self._update_cond:
Expand All @@ -653,7 +663,6 @@ def OnQuit(self,event):
self._server_list_thread.join()
self._update_graph_thread.join()
self._update_tree_thread.join()

event.Skip()

def update_graph(self):
Expand Down Expand Up @@ -1026,6 +1035,21 @@ def _update_server_list(self):
# self.server_combo.SetStringSelection(self._servers[0])
# self.set_server(self._servers[0])

def OnTimer(self, event):
# image
context = wx.ClientDC(self)
memory = wx.MemoryDC()
x, y = self.ClientSize
bitmap = wx.EmptyBitmap(x, y, -1)
memory.SelectObject(bitmap)
memory.Blit(0, 0, x, y, context, 0, 0)
memory.SelectObject(wx.NullBitmap)
buf = wx.ImageFromBitmap(bitmap).GetDataBuffer()
img = np.frombuffer(buf, dtype=np.uint8)
bridge = cv_bridge.CvBridge()
img_msg = bridge.cv2_to_imgmsg(img.reshape((y, x, 3)), encoding='rgb8')
self._pub.publish(img_msg)

def ShowControlsDialog(self,event):
dial = wx.MessageDialog(None,
"Pan: Arrow Keys\nZoom: PageUp / PageDown\nZoom To Fit: F\nRefresh: R",
Expand Down