## @author Hai Nguyen/[email protected] PKG = 'laser_interface' import sys, os, subprocess try: rostoolsDir = (subprocess.Popen(['rospack', 'find', 'rostools'], stdout=subprocess.PIPE).communicate()[0] or '').strip() sys.path.append(os.path.join(rostoolsDir,'src')) import rostools.launcher rostools.launcher.updateSysPath(sys.argv[0], PKG, bootstrapVersion="0.6") except ImportError: print >> sys.stderr, "\nERROR: Cannot locate rostools" sys.exit(1) #from pkg import * import rospy from std_msgs.msg import Position from std_msgs.msg import RobotBase2DOdom from std_msgs.msg import Pose2DFloat32 import sys import time pub = rospy.TopicPub('odom', RobotBase2DOdom) rospy.ready(sys.argv[0]) while not rospy.isShutdown(): odom = RobotBase2DOdom(None, Pose2DFloat32(1,2,3), Pose2DFloat32(1,2,4), 1) pub.publish(odom) time.sleep(.5) print 'published'
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # ## @author Hai Nguyen/[email protected] ##@package image_broadcaster # Broadcast image given on command line to topic 'image'. import rostools rostools.updatePath('gmmseg') import rospy import std_msgs.msg.Image as RImage import opencv.highgui as hg import opencv as cv import sys import time rospy.ready(sys.argv[0]) pub_channel = rospy.TopicPub('image', RImage) cv_image = hg.cvLoadImage(sys.argv[1]) cv.cvCvtColor(cv_image, cv_image, cv.CV_BGR2RGB) count = 1 while not rospy.isShutdown(): image = RImage(None, 'static', cv_image.width, cv_image.height, '', 'rgb24', cv_image.imageData) pub_channel.publish(image) print 'published', count count = count + 1 time.sleep(.3)
def renderGraphMain(argv, stdout, env):
# default arguments
server = "http://localhost"
server_port = rospy.DEFAULT_TEST_PORT
#check arguments for a help flag
optlist, args = getopt.getopt(argv[1:], "h?p:s:",
["help", "port=", "server=", "test"])
for o, a in optlist:
if o in ("-h", "-?", "--help"):
usage(stdout, argv[0])
return
elif o in ("--test"):
server_port = rospy.DEFAULT_TEST_PORT
elif o in ("-p", "--port"):
server_port = a
elif o in ("-s", "--server"):
server = a
serverUri = '%s:%s/' % (server, server_port)
print "Looking for server at %s" % serverUri
os.environ[rospy.ROS_MASTER_URI] = serverUri
os.environ[rospy.ROS_NODE] = NAME
os.environ[rospy.ROS_PORT] = str(0) # any
master = rospy.getMaster()
out_FlowImageString = OutflowpyImageString(".imageOut")
require(
master.addMachine('default', rospy.getRosRoot(), 'localhost', 22, '',
''))
require(
master.addNode('', 'imageViewer', 'pyImageViewer', 'imageViewer',
'default', 0))
rospy.ready()
require(
master.connectFlow(out_FlowImageString.locator, "imageViewer:imageIn",
1))
while not rospy.isShutdown():
status_code, statusMessage, [nodes, flows] = master.getGraph()
# print 'nodes' + str(nodes)
# print 'flows ' + str(flows)
print '--------------------------------------------'
print 'This is the graph:'
print 'refreshing every ' + str(sleep_time) + ' seconds'
print '--------------------------------------------'
for anode in nodes:
status_code, statusMessage, [machine, address,
port] = master.getNodeAddress(anode)
print 'NODE: ' + str(anode) + ' on ' + str(machine) + ' at ' + str(
address) + ' on port: ' + str(port)
for aflow in flows:
aflow_split = str(aflow[1]).split('.')
destination = aflow_split[0]
aflow_split_source = str(aflow[0]).split('.')
source = aflow_split_source[0]
if destination == anode:
print '\tINFLOW: ' + str(aflow_split[1]) + ' from: ' + str(
aflow[0])
if source == anode:
print '\tOUTFLOW: ' + str(
aflow_split_source[1]) + ' to: ' + str(aflow[1])
output_file = 'ROSGraph.jpeg'
pid = os.fork(
) # This is a hack to get around an underlying memory leak in
# the agraph library.
if pid == 0:
#print "starting yapgvb process"
try:
print '--------------------------------------------'
ygraph = yapgvb.Digraph('ROSGraph')
gnodes = {}
for anode in nodes:
gnodes[anode] = ygraph.add_node(label=anode)
#print "added allnodes"
#print flows
for aflow in flows:
aflow_split = str(aflow[1]).split(':')
destination = aflow_split[0]
aflow_split_source = str(aflow[0]).split(':')
source = aflow_split_source[0]
gnodes[source] >> gnodes[destination]
#print "done setting up graph for rendering"
ygraph.layout(yapgvb.engines.dot)
ygraph.render(output_file)
finally:
mpid = os.getpid()
os.kill(mpid, 9)
else:
os.wait()
# Wrap up the image and send it out over a flow
imToSend = Image.open(output_file)
future_packet = pyImageString()
future_packet.imageString = imToSend.tostring()
future_packet.imageFormat = "RGB"
future_packet.width, future_packet.height = imToSend.size
out_FlowImageString.publish(future_packet)
# don't loop too fast
time.sleep(sleep_time)
def renderGraphMain(argv, stdout, env):
# default arguments
server = "http://localhost"
server_port = rospy.DEFAULT_TEST_PORT
#check arguments for a help flag
optlist, args = getopt.getopt(argv[1:], "h?p:s:", ["help","port=","server=","test"])
for o, a in optlist:
if o in ("-h","-?","--help"):
usage(stdout, argv[0])
return
elif o in ("--test"):
server_port = rospy.DEFAULT_TEST_PORT
elif o in ("-p", "--port"):
server_port = a
elif o in ("-s", "--server"):
server = a
serverUri = '%s:%s/'%(server,server_port)
print "Looking for server at %s"%serverUri
os.environ[rospy.ROS_MASTER_URI] = serverUri
os.environ[rospy.ROS_NODE] = NAME
os.environ[rospy.ROS_PORT] = str(0) # any
master = rospy.getMaster()
out_FlowImageString = OutflowpyImageString(".imageOut")
require(master.addMachine('default', rospy.getRosRoot(), 'localhost', 22, '', ''))
require(master.addNode('','imageViewer','pyImageViewer','imageViewer','default',0))
rospy.ready()
require(master.connectFlow(out_FlowImageString.locator, "imageViewer:imageIn",1))
while not rospy.isShutdown():
status_code, statusMessage, [nodes,flows] = master.getGraph()
# print 'nodes' + str(nodes)
# print 'flows ' + str(flows)
print '--------------------------------------------'
print 'This is the graph:'
print 'refreshing every '+str(sleep_time)+' seconds'
print '--------------------------------------------'
for anode in nodes:
status_code, statusMessage, [machine, address, port] = master.getNodeAddress(anode)
print 'NODE: ' + str(anode) + ' on ' + str(machine) + ' at ' + str(address) + ' on port: ' + str(port)
for aflow in flows:
aflow_split= str(aflow[1]).split('.')
destination = aflow_split[0]
aflow_split_source = str(aflow[0]).split('.')
source = aflow_split_source[0]
if destination == anode:
print '\tINFLOW: ' + str(aflow_split[1]) + ' from: ' + str(aflow[0])
if source == anode:
print '\tOUTFLOW: ' + str(aflow_split_source[1]) + ' to: ' + str(aflow[1])
output_file = 'ROSGraph.jpeg'
pid = os.fork() # This is a hack to get around an underlying memory leak in
# the agraph library.
if pid == 0:
#print "starting yapgvb process"
try:
print '--------------------------------------------'
ygraph = yapgvb.Digraph('ROSGraph');
gnodes = {}
for anode in nodes:
gnodes[anode] = ygraph.add_node(label = anode)
#print "added allnodes"
#print flows
for aflow in flows:
aflow_split= str(aflow[1]).split(':')
destination = aflow_split[0]
aflow_split_source = str(aflow[0]).split(':')
source = aflow_split_source[0]
gnodes[source] >> gnodes[destination]
#print "done setting up graph for rendering"
ygraph.layout(yapgvb.engines.dot)
ygraph.render(output_file)
finally:
mpid = os.getpid()
os.kill(mpid,9)
else:
os.wait()
# Wrap up the image and send it out over a flow
imToSend = Image.open(output_file)
future_packet = pyImageString()
future_packet.imageString = imToSend.tostring()
future_packet.imageFormat = "RGB"
future_packet.width, future_packet.height = imToSend.size
out_FlowImageString.publish(future_packet)
# don't loop too fast
time.sleep(sleep_time)
