Exemple #1
0
## @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)
Exemple #3
0
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)
Exemple #4
0
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)