Esempi in Python per update_path

Esempio n. 1

File: control_laser.py Progetto: Calm-wy/kwc-ros-pkg

#!/usr/bin/env python

PKG = "pr2_mechanism_controllers"

import rostools

rostools.update_path(PKG)

import sys
import os
import string

import rospy
from std_msgs import *

from pr2_mechanism_controllers.srv import *


def print_usage(exit_code=0):
    print '''Usage:
    control.py <controller> sine <period> <amplitude> <offset>
       - Defines a sine sweep for the laser controller
'''
    sys.exit(exit_code)


if __name__ == '__main__':
    if len(sys.argv) != 6:
        print_usage()
    if sys.argv[2] == 'sine':
        profile_type = 4  # Implies a sine sweep

Esempio n. 2

File: standalone_visualizer.py Progetto: Calm-wy/kwc-ros-pkg

#!/usr/bin/python

import rostools
import rostools.packspec

rostools.update_path('ogre_visualizer')

import wx
import ogre_visualizer
import ogre_tools


class VisualizerFrame(wx.Frame):
    _CONFIG_WINDOW_X = "/Window/X"
    _CONFIG_WINDOW_Y = "/Window/Y"
    _CONFIG_WINDOW_WIDTH = "/Window/Width"
    _CONFIG_WINDOW_HEIGHT = "/Window/Height"

    def __init__(self,
                 parent,
                 id=wx.ID_ANY,
                 title='Standalone Visualizer',
                 pos=wx.DefaultPosition,
                 size=(800, 600),
                 style=wx.DEFAULT_FRAME_STYLE):
        wx.Frame.__init__(self, parent, id, title, pos, size, style)

        self._config = wx.Config("standalone_visualizer")

        ogre_tools.initializeOgre()

Esempio n. 3

# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id: test_embed_msg.py 1986 2008-08-26 23:57:56Z sfkwc $

## Integration test for empty services to test serializers
## and transport

PKG = 'mechanism_control'
NAME = 'test_ms'
PUBTOPIC = "chatter"
PUBNODE = 'mechanism_control'
SUBTOPIC = 'mechanism_state'

import rostools
rostools.update_path(PKG)

import sys, time
import unittest

# ad-hoc import here as we don't want to change the manifest for test dependencies
rostools.update_path('rostest')
import rospy, rostest
from mechanism_control.msg import MechanismState, ActuatorState, JointState

MSG = MechanismState

TIMEOUT = 10.0  #seconds


class TestMechanismState(unittest.TestCase):

Esempio n. 4

File: test_2dnav.py Progetto: Calm-wy/kwc-ros-pkg

# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.
#

## Gazebo test 2dnav stack

PKG = '2dnav_gazebo'
NAME = 'test_2dnav'

import math
import rostools
rostools.update_path(PKG)
rostools.update_path('rostest')
rostools.update_path('std_msgs')
rostools.update_path('robot_msgs')
rostools.update_path('rostools')
rostools.update_path('rospy')
rostools.update_path('transformations')

import sys, unittest
import os, os.path, threading, time
import rospy, rostest, rostools
from std_msgs.msg import *
from rostools.msg import *
from transformations import *
from numpy import *

Esempio n. 5

# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.
#

# Written by Timothy Hunter <*****@*****.**> 2008

from xml.dom.minidom import parseString

import rostools
import sys
import rospy
rostools.update_path('robot_srvs')
from robot_srvs.srv import *

#TODO: add a controller factory mechanism

# Loads specific controller interfaces.
rostools.update_path('pr2_controllers')
from pr2_controllers import *

# Loads basic controller interfaces.
rostools.update_path('generic_controllers')
from generic_controllers import *

# Loads logging utilities (used for record/replay)
rostools.update_path('wxpy_ros')
import wxpy_ros

Esempio n. 6

# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# 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: Melonee Wise

PKG = "joint_qualification_controllers"

import rostools; rostools.update_path(PKG)

import wx

import numpy
import wxmpl
import matplotlib

from robot_msgs.msg import *
from std_msgs.msg import *
from robot_srvs.srv import *

import sys
import os
import string
from time import sleep

Esempio n. 7

File: test_ms_cpp.py Progetto: goretkin/kwc-ros-pkg

# 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.
#
# Revision $Id: test_embed_msg.py 1986 2008-08-26 23:57:56Z sfkwc $

## Integration test for empty services to test serializers
## and transport

PKG = 'mechanism_control'
NAME = 'test_ms'
PUBTOPIC = "chatter"
PUBNODE = 'mechanism_control'
SUBTOPIC = 'mechanism_state'

import rostools; rostools.update_path(PKG)

import sys, time
import unittest

# ad-hoc import here as we don't want to change the manifest for test dependencies
rostools.update_path('rostest')
import rospy, rostest
from mechanism_control.msg import MechanismState, ActuatorState, JointState

MSG = MechanismState

TIMEOUT = 10.0 #seconds

class TestMechanismState(unittest.TestCase):

Esempio n. 8

File: mechanism.py Progetto: Calm-wy/kwc-ros-pkg

#! /usr/bin/env python
# Wrappers around the services provided by MechanismControlNode

import rostools
rostools.update_path('mechanism_control')
rostools.update_path('std_srvs')

import rospy, sys
from mechanism_control.srv import *
import std_srvs.srv


def list_controller_types():
    s = rospy.ServiceProxy('list_controller_types', ListControllerTypes)
    resp = s.call(ListControllerTypesRequest())
    for t in resp.types:
        print t


def list_controllers():
    rospy.wait_for_service('list_controllers')
    s = rospy.ServiceProxy('list_controllers', ListControllers)
    resp = s.call(ListControllersRequest())
    for c in resp.controllers:
        print c


def spawn_controller(xml):
    rospy.wait_for_service('spawn_controller')
    s = rospy.ServiceProxy('spawn_controller', SpawnController)
    resp = s.call(SpawnControllerRequest(xml))

Esempio n. 9

File: mechanism.py Progetto: janfrs/kwc-ros-pkg

#! /usr/bin/env python
# Wrappers around the services provided by MechanismControlNode

import rostools
rostools.update_path('robot_srvs')
rostools.update_path('std_srvs')

import rospy, sys
from robot_srvs.srv import *
import std_srvs.srv

def list_controller_types():
    s = rospy.ServiceProxy('list_controller_types', ListControllerTypes)
    resp = s.call(ListControllerTypesRequest())
    for t in resp.types:
        print t

def list_controllers():
    rospy.wait_for_service('list_controllers')
    s = rospy.ServiceProxy('list_controllers', ListControllers)
    resp = s.call(ListControllersRequest())
    for c in resp.controllers:
        print c

def spawn_controller(xml):
    rospy.wait_for_service('spawn_controller')
    s = rospy.ServiceProxy('spawn_controller', SpawnController)
    resp = s.call(SpawnControllerRequest(xml))
    print ','.join([str(k) for k in resp.ok])
    for i in range(len(resp.ok)):
        if resp.ok[i] != 0:

Esempio n. 10

File: teleop_joint_effort.py Progetto: Calm-wy/kwc-ros-pkg

# 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.

# This script brings up an effort controller on your joint of choice
# and use a joystick to command it
#
# Author: Eric berger

import random
CONTROLLER_NAME = "quick_effort_controller_%08d" % random.randint(0, 10**8 - 1)

import sys

import rostools
rostools.update_path('teleop_joint_effort')
import rospy
from std_msgs.msg import *
from joy.msg import Joy
from mechanism_control import mechanism
from robot_srvs.srv import SpawnController, KillController


def xml_for(joint):
    return "\
<controller name=\"%s\" type=\"JointEffortControllerNode\">\
<joint name=\"%s\" />\
</controller>" % (CONTROLLER_NAME, joint)


def main():

Esempio n. 11

# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.
#

## Gazebo collision validation

NAME = 'test_slide'

import rostools
rostools.update_path('gazebo_plugin')
rostools.update_path('std_msgs')
rostools.update_path('robot_msgs')
rostools.update_path('rostest')
rostools.update_path('rospy')

import unittest, sys, os, math
import time
import rospy, rostest
from std_msgs.msg import *
from robot_msgs.msg import *

TARGET_X = -5.4 + 25.65  #contains offset specified in P3D for base, alternatively, use the gripper roll ground truths
TARGET_Y = 0.0 + 25.65  #contains offset specified in P3D for base, alternatively, use the gripper roll ground truths
TARGET_Z = 3.8
TARGET_RAD = 4.5

Esempio n. 12

File: listen.py Progetto: goretkin/kwc-ros-pkg

#!/usr/bin/env python

import rostools
rostools.update_path('phase_space')

import sys, traceback, logging, rospy
from phase_space.msg import PhaseSpaceSnapshot
from phase_space.msg import PhaseSpaceMarker

NAME = 'phase_space_listener'

def callback(data):
    timeSeconds = data.header.stamp.secs + data.header.stamp.nsecs*1e-9
    print rospy.get_caller_id(), "I heard [%.6f]"%(timeSeconds)
    print "  Frame #:     %u" % (data.frameNum)
    print "  Num Markers: %u" % (len(data.markers))
    for j in data.markers:
      #print "  ID: %d" % j.id
      #print "    x:         %.4f" % j.location.x
      #print "    y:         %.4f" % j.location.y
      #print "    z:         %.4f" % j.location.z
      #print "    cond:      %.4f" % j.condition
      print "  ID=%03d Pos=(% 08.4f,% 08.4f,% 08.4f) Cond=%f" % (j.id, j.location.x, j.location.y, j.location.z, j.condition)
    print " "

def listener_with_user_data():
    print "Starting Listener"
    rospy.TopicSub("/phase_space_snapshot", PhaseSpaceSnapshot, callback)
    rospy.ready(NAME, anonymous=True)
    print "Spinning"
    rospy.spin()

Esempio n. 13

File: calibrate.py Progetto: janfrs/kwc-ros-pkg

# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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: Stuart Glaser

import rostools
import copy
import threading
import sys, os
from time import sleep

# Loads interface with the robot.
rostools.update_path("mechanism_bringup")
import rospy
from std_msgs.msg import *
from robot_srvs.srv import *
from robot_mechanism_controllers.srv import *


def slurp(filename):
    f = open(filename)
    stuff = f.read()
    f.close()
    return stuff


rospy.wait_for_service("spawn_controller")

Esempio n. 14

File: standalone_camera.py Progetto: janfrs/kwc-ros-pkg

#!/usr/bin/python

import rostools
rostools.update_path('wx_camera_panel')

import wx
import wx_camera_panel
import sys
from optparse import OptionParser

parser = OptionParser(usage="usage: %prog camera_namespace [options]")
parser.add_option("--ptz", action="store_true", dest="ptz", help="Enable PTZ controls")
parser.add_option("--fs", action="store_true", dest="fs", help="Enable Fullscreen Mode")
parser.add_option("--minimal", action="store_true", dest="minimal", help="Enable Minimal Mode")
(options, args) = parser.parse_args()

if (len(args) == 0):
    print( "No namespace specified!\n" )
    parser.print_usage()
    sys.exit(1)
    
name = args[0]

app = wx.PySimpleApp()
titlebar_string = "Camera " + name
frame = wx.Frame(None, wx.ID_ANY, titlebar_string, wx.DefaultPosition, wx.Size( 500, 500 ) )

camera_panel = wx_camera_panel.CameraPanel(frame)
camera_panel.setEnabled(True)

if (options.ptz != None):

Esempio n. 15

# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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: Stuart Glaser

import rostools
import copy
import threading

# Loads interface with the robot.
rostools.update_path('teleop_robot')
import rospy
from mechanism_control.srv import *
from robot_mechanism_controllers.srv import *


def slurp(filename):
    f = open(filename)
    stuff = f.read()
    f.close()
    return stuff


rospy.wait_for_service('spawn_controller')
spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
kill_controller = rospy.ServiceProxy('kill_controller', KillController)

Esempio n. 16

File: listen.py Progetto: janfrs/kwc-ros-pkg

#!/usr/bin/env python

import rostools
rostools.update_path('mechanism_control')

import sys, traceback, logging, rospy
from mechanism_control.msg import MechanismState

NAME = 'joint_listener'

def callback(data):
    print rospy.get_caller_id(), "I heard [%s], %d"%(data.time, len(data.joint_states))
    for j in data.joint_states:
      print "  joint: %s" % j.name
      print "    position:         %.4f" % j.position
      print "    velocity:         %.4f" % j.velocity
      print "    applied_effort:   %.4f" % j.applied_effort
      print "    commanded_effort: %.4f" % j.commanded_effort
    for a in data.actuator_states:
      print "  actuator: %s" % a.name
      print "    encoder_count:         %d" % a.encoder_count
      print "    position:         %d" % a.position
      print "    timestamp: %.4f" % a.timestamp
      print "    encoder_velocity: %.4f" % a.encoder_velocity
      print "    velocity: %.4f" % a.velocity
      print "    calibration_reading: %d" % a.calibration_reading
      print "    last_calibration_high_transition: %d" % a.last_calibration_high_transition
      print "    last_calibration_low_transition: %d" % a.last_calibration_low_transition
      print "    is_enabled: %d" % a.is_enabled
      print "    run_stop_hit: %d" % a.run_stop_hit
      print "    last_requested_current: %.4f" % a.last_requested_current

Esempio n. 17

File: calibrate.py Progetto: goretkin/kwc-ros-pkg

# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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: Stuart Glaser

import rostools
import copy
import threading
import sys
from time import sleep

# Loads interface with the robot.
rostools.update_path('teleop_robot')
import rospy
from std_msgs.msg import *
from robot_srvs.srv import *
from robot_mechanism_controllers.srv import *

def slurp(filename):
    f = open(filename)
    stuff = f.read()
    f.close()
    return stuff

rospy.wait_for_service('spawn_controller')
spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
kill_controller = rospy.ServiceProxy('kill_controller', KillController)

Esempio n. 18

File: robot_interface.py Progetto: janfrs/kwc-ros-pkg

# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.
#

# Written by Timothy Hunter <*****@*****.**> 2008

from xml.dom.minidom import parseString

import rostools
import sys
import rospy
rostools.update_path('mechanism_control')
from mechanism_control.srv import *

#TODO: add a controller factory mechanism

# Loads specific controller interfaces.
rostools.update_path('pr2_controllers')
from pr2_controllers import *

# Loads basic controller interfaces.
rostools.update_path('generic_controllers')
from generic_controllers import *

# Loads logging utilities (used for record/replay)
rostools.update_path('wxpy_ros')
import wxpy_ros

Esempio n. 19

File: indefinite_nav.py Progetto: janfrs/kwc-ros-pkg

#! /usr/bin/python

import rostools
rostools.update_path('trajectory_rollout')

import sys, time, traceback, logging, rospy, random
from std_msgs.msg import Planner2DGoal
from std_msgs.msg import Planner2DState

NAME = 'indefinite_nav'

goals = [
 [50.250, 6.863, 3.083], 
 [18.550, 11.762, 2.554],
 [53.550, 20.163, 0.00],
 [18.850, 28.862, 0.00],
 [47.250, 39.162, 1.571],
 [11.450, 39.662, 0.00]
 ]

chrg_stations = [
 [33.844, 36.379, -1.571]
]

first = True

def indefinite_nav():
  def callback(state):
    send_goal(state.done)

  def send_goal(done):

Esempio n. 20

File: directed.py Progetto: Calm-wy/kwc-ros-pkg

import rostools
rostools.update_path('videre_face_detection')
import rospy
import rostest
import videre_face_detection
from visualodometer import VisualOdometer, FeatureDetectorStar, DescriptorSchemeCalonder, DescriptorSchemeSAD
import camera
from std_msgs.msg import Image, ImageArray, String, PointStamped
import visual_odometry as VO
import starfeature
import calonder

from stereo import SparseStereoFrame

import sys
sys.path.append('lib')

import Image
import ImageChops
import ImageDraw

import random
import time
import math
import numpy

import os
import copy

DEBUG = True
SAVE_PICS = True

Esempio n. 21

File: testslide.py Progetto: janfrs/kwc-ros-pkg

# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.
#

## Gazebo collision validation

NAME = "testslide"

import rostools

rostools.update_path("gazebo_plugin")
rostools.update_path("std_msgs")
rostools.update_path("robot_msgs")
rostools.update_path("rostest")
rostools.update_path("rospy")

import unittest, sys, os, math
import time
import rospy, rostest
from std_msgs.msg import *
from robot_msgs.msg import *

TARGET_X = -5.4 + 25.65  # contains offset specified in P3D for base, alternatively, use the gripper roll ground truths
TARGET_Y = 0.0 + 25.65  # contains offset specified in P3D for base, alternatively, use the gripper roll ground truths
TARGET_Z = 3.8
TARGET_RAD = 4.5

Esempio n. 22

File: sos.py Progetto: Calm-wy/kwc-ros-pkg

#!/usr/bin/python

import rostools
rostools.update_path('visual_odometry')
import rostest
import rospy

import vop

import sys
sys.path.append('lib')

import visual_odometry as VO
import Image as Image
import ImageChops as ImageChops
import ImageDraw as ImageDraw
import ImageFilter as ImageFilter

import random
import unittest
import math
import copy

from stereo import DenseStereoFrame, SparseStereoFrame
from visualodometer import VisualOdometer, Pose, DescriptorSchemeCalonder, DescriptorSchemeSAD, FeatureDetectorFast, FeatureDetector4x4, FeatureDetectorStar, FeatureDetectorHarris
import fast
from math import *

import camera

import numpy

Esempio n. 23

# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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: Stuart Glaser

import rostools
import copy
import threading
import sys, os
from time import sleep

# Loads interface with the robot.
rostools.update_path('mechanism_bringup')
import rospy
from std_msgs.msg import *
from robot_srvs.srv import *
from robot_mechanism_controllers.srv import *

def slurp(filename):
    f = open(filename)
    stuff = f.read()
    f.close()
    return stuff

rospy.wait_for_service('spawn_controller')


def calibrate(config):

Esempio n. 24

File: test_node.py Progetto: janfrs/kwc-ros-pkg

#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.

import rostools
import rostools.packspec
rostools.update_path('qualification')

import rospy
from std_srvs.srv import *

rospy.init_node("test_node", anonymous=True)

def callback(msg):
  print("Got msg")
  return EmptyResponse()

rospy.Service('self_test', Empty, callback)
print("Test Spinning...")
rospy.spin()

Esempio n. 25

File: test_slide.py Progetto: janfrs/kwc-ros-pkg

# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.
#

## Gazebo collision validation 

NAME = 'test_slide'

import rostools
rostools.update_path('gazebo_plugin')
rostools.update_path('std_msgs')
rostools.update_path('robot_msgs')
rostools.update_path('rostest')
rostools.update_path('rospy')

import unittest, sys, os, math
import time
import rospy, rostest
from std_msgs.msg import *
from robot_msgs.msg import *

TARGET_X = -5.4 + 25.65 #contains offset specified in P3D for base, alternatively, use the gripper roll ground truths
TARGET_Y = 0.0 + 25.65 #contains offset specified in P3D for base, alternatively, use the gripper roll ground truths
TARGET_Z = 3.8
TARGET_RAD = 4.5

Esempio n. 26

File: reset.py Progetto: janfrs/kwc-ros-pkg

#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.

import rostools
rostools.update_path('pr2_gui')

import wx
from wx import xrc
import rospy
import rospy.service
import std_srvs.srv

PKG='pr2_gui'

class ResetPanel(wx.Panel):
    def __init__(self, parent):
        wx.Panel.__init__(self, parent)
        
        xrc_path = rostools.packspec.get_pkg_dir(PKG) + '/xrc/reset_panel.xrc'
        

Esempio n. 27

# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.
#

## A basic node to listen to and display incoming diagnostic messages using wx

import rostools
rostools.update_path('IBPSBatteryInterface')

import sys
import rospy
from robot_msgs.msg import *

import wx
import threading, time

import IBPSBatteryInterface
from IBPSBatteryInterface.ibps_panel import *

NAME = 'pr2_batteries'


class MainWindow(wx.Frame):

Esempio n. 28

File: random_positions.py Progetto: janfrs/kwc-ros-pkg

# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# 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.


import rostools

rostools.update_path("pr2_mechanism_controllers")
rostools.update_path("std_msgs")
rostools.update_path("rospy")

import random, time
import rospy
from std_msgs.msg import *
from robot_msgs.msg import *

pub = rospy.Publisher("/arm_position/set_command", PointStamped)


def p(x, y, z):
    m = PointStamped()
    m.header.frame_id = "base_link"
    m.point.x = x

Esempio n. 29

File: sos.py Progetto: janfrs/kwc-ros-pkg

#!/usr/bin/python

import rostools
rostools.update_path('visual_odometry')
import rostest
import rospy

import vop

import sys
sys.path.append('lib')

import visual_odometry as VO
import Image as Image
import ImageChops as ImageChops
import ImageDraw as ImageDraw
import ImageFilter as ImageFilter

import random
import unittest
import math
import copy

from stereo import DenseStereoFrame, SparseStereoFrame
from visualodometer import VisualOdometer, Pose, DescriptorSchemeCalonder, DescriptorSchemeSAD, FeatureDetectorFast, FeatureDetector4x4, FeatureDetectorStar, FeatureDetectorHarris
import fast
from math import *

import camera

import numpy

Esempio n. 30

File: test_ms.py Progetto: janfrs/kwc-ros-pkg

# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id: test_embed_msg.py 1986 2008-08-26 23:57:56Z sfkwc $

## Integration test for empty services to test serializers
## and transport

PKG = "mechanism_control"
NAME = "test_ms"
PUBTOPIC = "chatter"
LPNODE = "listenerpublisher"
SUBTOPIC = "listenerpublisher"

import rostools

rostools.update_path(PKG)

import sys, time
import unittest

# ad-hoc import here as we don't want to change the manifest for test dependencies
rostools.update_path("rostest")
import rospy, rostest
from mechanism_control.msg import MechanismState, ActuatorState, JointState

MSG = MechanismState

TIMEOUT = 10.0  # seconds


class TestMechanismState(unittest.TestCase):

Esempio n. 31

# 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.

# This script brings up an effort controller on your joint of choice
# and allows you to type in the desired efforts.
#
# Author: Stuart Glaser

import random
CONTROLLER_NAME = "quick_effort_controller_%08d" % random.randint(0,10**8-1)

import sys

import rostools
rostools.update_path('elbow_life_test')
import rospy
from std_msgs.msg import *
from mechanism_control import mechanism
from robot_srvs.srv import SpawnController, KillController
from time import sleep

def xml_for(joint):
    return "\
<controller name=\"%s\" type=\"JointEffortControllerNode\">\
<joint name=\"%s\" />\
</controller>" % (CONTROLLER_NAME, joint)

def main():
    joint = "r_elbow_flex_joint"

Esempio n. 32

File: pressure.py Progetto: Calm-wy/kwc-ros-pkg

#!/usr/bin/env python

import rostools
rostools.update_path('ethercat_hardware')

import sys, traceback, logging, rospy
from ethercat_hardware.msg import PressureState

NAME = 'pressure_listener'


def callback(data):
    print rospy.get_caller_id(), "I heard %d pressure readings @ %s" % (len(
        data.data0), data.header.stamp)
    for x in xrange(22):
        print "%04x %04x" % (data.data0[x] & 0xff00, data.data1[x] & 0xff00)
        #print "%04x %04x" % (data.data0[x] & 0xffff, data.data1[x] & 0xffff)


def listener_with_user_data():
    rospy.TopicSub("/pressure", PressureState, callback)
    rospy.ready(NAME, anonymous=True)
    rospy.spin()


if __name__ == '__main__':
    try:
        listener_with_user_data()
    except KeyboardInterrupt, e:
        pass
    print "exiting"

Esempio n. 33

# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.
#

## A basic node to listen to and display incoming diagnostic messages using wx

import rostools
rostools.update_path('runtime_monitor')

import sys
import rospy
from robot_msgs.msg import *

import wx
import threading, time

import runtime_monitor
from runtime_monitor.monitor_panel import *

NAME = 'runtime_monitor'


class MainWindow(wx.Frame):

Esempio n. 34

File: controllers.py Progetto: Calm-wy/kwc-ros-pkg

#!/usr/bin/env python

import rostools
rostools.update_path('robot_mechanism_controllers')

import rospy, sys
from robot_mechanism_controllers.srv import *
from robot_srvs.srv import *
from std_msgs.msg import *
from time import sleep


class SendMessageOnSubscribeAndExit(rospy.SubscribeListener):
    def __init__(self, msg):
        self.msg = msg
        print "Waiting for subscriber..."

    def peer_subscribe(self, topic_name, topic_publish, peer_publish):
        peer_publish(self.msg)
        sleep(0.1
              )  # TODO: change this line when flushing messages is implemented
        rospy.signal_shutdown("Done")
        sys.exit(0)


def list_controllers():
    rospy.wait_for_service('list_controllers')
    s = rospy.ServiceProxy('list_controllers', ListControllers)
    resp = s.call(ListControllersRequest())
    for t in resp.controllers:
        print t

Esempio n. 35

# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.

## Some common ROS communication utilities. See the wiki documentation and the demo scripts for how to use it.

import rostools
rostools.update_path('rospy')
from rostools import msgspec as msgspec
from rospy import client
import rospy
import copy

# Timestamp imports
import rostools.msg._Time 
_time_class_name = 'rostools.msg._Time.Time'

import wxplot
import wxros

__all__ = ['RosChannel', 'RosSubscriber', 'RosMessageHandler', 'getTopicsList', 'getMessageHandler']

_ros_timestamp = None

Esempio n. 36

#! /usr/bin/python

import rostools
rostools.update_path('trajectory_rollout')

import sys, time, traceback, logging, rospy, random
from std_msgs.msg import Planner2DGoal
from std_msgs.msg import Planner2DState

NAME = 'indefinite_nav'

goals = [[19, 11, 0], [19, 17, 1.7]]
first = True


def indefinite_nav():
    def callback(state):
        send_goal(state.done)

    def send_goal(done):
        global first
        if first or done == 1:
            goal_pts = goals[random.randint(0, len(goals) - 1)]
            goal = Planner2DGoal()
            goal.goal.x = goal_pts[0]
            goal.goal.y = goal_pts[1]
            goal.goal.th = goal_pts[2]
            goal.enable = 1
            first = False
            print "New Goal: x: %.2f, y: %.2f, th: %.2f" % (
                goal.goal.x, goal.goal.y, goal.goal.th)

Esempio n. 37

File: test_pycv.py Progetto: Calm-wy/kwc-ros-pkg

#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.

import rostools
rostools.update_path('cam_viewer_py')

import sys
import time

import numpy as np
import opencv as cv
import opencv.highgui as hg
import Image as PImage

from std_msgs.msg import Image
from std_msgs.msg import ImageArray
import rospy
import pyrob.util

#def callback_array(iar):

Esempio n. 38

File: standalone_visualizer.py Progetto: janfrs/kwc-ros-pkg

#!/usr/bin/python

import rostools
import rostools.packspec
rostools.update_path('ogre_visualizer')

import os
import shutil
import glob
import wx
import ogre_visualizer
import ogre_tools

class VisualizerFrame(wx.Frame):
    _CONFIG_WINDOW_X="/Window/X"
    _CONFIG_WINDOW_Y="/Window/Y"
    _CONFIG_WINDOW_WIDTH="/Window/Width"
    _CONFIG_WINDOW_HEIGHT="/Window/Height"
    
    _CONFIG_EXTENSION="vcg"
    
    def __init__(self, parent, id=wx.ID_ANY, title='Standalone Visualizer', pos=wx.DefaultPosition, size=(800, 600), style=wx.DEFAULT_FRAME_STYLE):
        wx.Frame.__init__(self, parent, id, title, pos, size, style)
        
        ogre_tools.initializeOgre()
        visualizer_panel = ogre_visualizer.VisualizationPanel(self)
        
        self._package_path = rostools.packspec.get_pkg_dir('ogre_visualizer')
        self._global_config_path = os.path.join(self._package_path, "configs")
        
        self._visualizer_panel = visualizer_panel

Esempio n. 39

File: random_wrenches.py Progetto: Calm-wy/kwc-ros-pkg

#       this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# 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.


import rostools; rostools.update_path('pr2_mechanism_controllers')
rostools.update_path('std_msgs')
rostools.update_path('rospy')

import random, time
import rospy
import sys
from std_msgs.msg import *
from robot_msgs.msg import *

pub = rospy.Publisher('/arm_wrench/command', Wrench)

def p(fx, fy, fz, tx, ty, tz):
  m = Wrench()
  m.force.x = fx
  m.force.y = fy

Esempio n. 40

File: monitor_panel.py Progetto: goretkin/kwc-ros-pkg

# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.
#

PKG = 'runtime_monitor'

import rostools
rostools.update_path(PKG)

import sys
import rospy
from robot_msgs.msg import *

import wx
from wx import xrc
from wx import html

import threading, time
import StringIO

class MonitorPanel(wx.Panel):
    def __init__(self, parent):
        wx.Panel.__init__(self, parent, wx.ID_ANY)

Esempio n. 41

File: controllers.py Progetto: janfrs/kwc-ros-pkg

#!/usr/bin/env python

import rostools
rostools.update_path('mechanism_control')
rostools.update_path('robot_mechanism_controllers')
rostools.update_path('pr2_mechanism_controllers')

import rospy, sys
from mechanism_control.srv import *
from robot_mechanism_controllers.srv import *
from pr2_mechanism_controllers.srv import *

def list_controllers():
    rospy.wait_for_service('list_controllers')
    s = rospy.ServiceProxy('list_controllers', ListControllers)
    resp = s.call(ListControllersRequest())
    for t in resp.controllers:
        print t

def set_controller(controller, command):
    rospy.wait_for_service(controller + '/set_command')
    s = rospy.ServiceProxy(controller + '/set_command', SetCommand)
    resp = s.call(SetCommandRequest(command))
    print resp.command

def set_controller_vector(controller, command):
    rospy.wait_for_service(controller + '/set_command')
    s = rospy.ServiceProxy(controller + '/set_command', SetVectorCommand)
    resp = s(*command)

def get_controller(controller):

Esempio n. 42

File: visualizer_panel.py Progetto: Calm-wy/kwc-ros-pkg

#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.

import rostools
rostools.update_path('pr2_gui')

import wx
import ogre_visualizer


class DefaultVisualizationPanel(ogre_visualizer.VisualizationPanel):
    def __init__(self, parent):
        ogre_visualizer.VisualizationPanel.__init__(self, parent)

    def createDefaultVisualizers(self):
        self.createGridVisualizer("Grid", True)
        self.createAxesVisualizer("Origin Axes", False)
        self.createMarkerVisualizer("Visualization Markers", True)

        robot_vis = self.createRobotModelVisualizer("Robot Model", False)

Esempio n. 43

File: launch_gazebo.py Progetto: Calm-wy/kwc-ros-pkg

# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.
#

# Written by Timothy Hunter <*****@*****.**> 2008
from __future__ import division

import rostools

# Loads interface with the robot.
rostools.update_path('teleop_robot')
from teleop_robot import *

# Manually loads arm interface
rostools.update_path('pr2_controllers')
from pr2_controllers import *

# Import plotting/data utils
rostools.update_path('scipy')
from scipy import *

rostools.update_path('matplotlib')
from pylab import *

# Data collecting utils
rostools.update_path('wxpy_ros')

Esempio n. 44

File: ethernet_test.py Progetto: Calm-wy/kwc-ros-pkg

#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.

import rostools
rostools.update_path('qualification')

from robot_msgs.msg import *
from robot_srvs.srv import *
from qualification.msg import *
from qualification.srv import *

import rospy

NAME = 'ethernet'

import os
import sys
from StringIO import StringIO

Esempio n. 45

File: mechanism.py Progetto: janfrs/kwc-ros-pkg

#! /usr/bin/env python
# Wrappers around the services provided by MechanismControlNode

import rostools

rostools.update_path("mechanism_control")
rostools.update_path("std_srvs")

import rospy, sys
from mechanism_control.srv import *
import std_srvs.srv


def list_controller_types():
    s = rospy.ServiceProxy("list_controller_types", ListControllerTypes)
    resp = s.call(ListControllerTypesRequest())
    for t in resp.types:
        print t


def list_controllers():
    rospy.wait_for_service("list_controllers")
    s = rospy.ServiceProxy("list_controllers", ListControllers)
    resp = s.call(ListControllersRequest())
    for c in resp.controllers:
        print c


def spawn_controller(xml):
    rospy.wait_for_service("spawn_controller")
    s = rospy.ServiceProxy("spawn_controller", SpawnController)

Esempio n. 46

import rostools
rostools.update_path('star_detector')
import rostest

import sys
sys.path.append('lib')
import starfeature as L
import Image
import ImageChops
import ImageDraw

import random
import time
import unittest
import math


# A star detector object's results are invariant wrt image presentation order

def simple(im, scales = 7, threshold = 30.0, line_threshold = 10.0, line_threshold_bin = 8.0):
    sd = L.star_detector(im.size[0], im.size[1], scales, threshold, line_threshold, line_threshold_bin)
    kp = sd.detect(im.tostring())
    return [ i[1] for i in sorted([ (abs(response), (x, y, s, response)) for (x, y, s, response) in kp])]

def circle(im, x, y, r, color):
    draw = ImageDraw.Draw(im)
    box = [ int(i) for i in [ x - r, y - r, x + r, y + r ]]
    draw.pieslice(box, 0, 360, fill = color)

def noisify(im):
    random.seed(0)

Esempio n. 47

File: testbase_vw_gt.py Progetto: janfrs/kwc-ros-pkg

# 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.
#

## Gazebo test base controller vw
##   sends cmd_vel vx= 0, vy=0, vw =TARGET_VW
##   checks to see if P3D returns corresponding ground truth within TARGET_TOL of TARGET_VW
##          for a duration of TARGET_DURATION seconds

PKG = 'gazebo_plugin'
NAME = 'testbase_vw_gt'

import math
import rostools
rostools.update_path(PKG)
rostools.update_path('rostest')
rostools.update_path('std_msgs')
rostools.update_path('robot_msgs')
rostools.update_path('rostest')
rostools.update_path('rospy')


import sys, unittest
import os, os.path, threading, time
import rospy, rostest
from std_msgs.msg import *


TARGET_VW       =  0.5
TARGET_DURATION = 2.0

Esempio n. 48

File: ros_panel.py Progetto: janfrs/kwc-ros-pkg

__doc__ = """
ROS_PLOT
A small utility to plot data from ROS topics."""

##"""ROS_PLOT
##A small utility class for embedding dynamic plots in WXpython.
##"""

import pdb
import  wx
import  wx.xrc  as  xrc

import rostools
import rostools.packspec
rostools.update_path('wxpy_ros')
import wxpy_ros
import rospy

class RosFrame(wx.Frame):
    def __init__(self, parent, id):
        wx.Frame.__init__(self, parent, id, 'Topics', size=(300,300))

        xrc_path = rostools.packspec.get_pkg_dir('wxpy_ros') + '/xrc/ros_panel.xrc'
        self.res = xrc.XmlResource(xrc_path)     

        self.res.LoadPanel(self, 'main_panel')

        self.plot_panel = xrc.XRCCTRL(self,'plot_panel')
        self.time_slider = xrc.XRCCTRL(self,'time_slider')
        self.add_but = xrc.XRCCTRL(self,'add_but')

Esempio n. 49

File: random_poses.py Progetto: janfrs/kwc-ros-pkg

#       this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# 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.


import rostools; rostools.update_path('pr2_mechanism_controllers')
rostools.update_path('std_msgs')
rostools.update_path('rospy')

import random, time
import rospy
from std_msgs.msg import *
from robot_msgs.msg import *

pub = rospy.Publisher('/arm_pose/command', PoseStamped)

def p(x, y, z, rx, ry, rz, w):
  m = PoseStamped()
  m.header.frame_id = 'base_link'
  m.pose.position.x = x
  m.pose.position.y = y

Esempio n. 50

File: view_power.py Progetto: goretkin/kwc-ros-pkg

# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.
#

## A basic node to listen to and display incoming diagnostic messages using wx

import rostools
rostools.update_path('pr2_power_board')

import sys
import rospy
from robot_msgs.msg import *

import wx
import threading, time

import pr2_power_board
from pr2_power_board.pr2_power_board_panel import *

NAME = 'pr2_power_board'

class MainWindow(wx.Frame):
    def __init__(self, parent, id, title):

Esempio n. 51

#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.

import rostools
rostools.update_path('ntp_monitor')

from robot_msgs.msg import *
import sys
import rospy
import socket
import subprocess

import time

import re

NAME = 'ntp_monitor'


def ntp_monitor():

Esempio n. 52

File: test_base_odomx_gt.py Progetto: Calm-wy/kwc-ros-pkg

# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.
#

## Gazebo test base controller vw

PKG = 'gazebo_plugin'
NAME = 'test_base_odomx_gt'

import math
import rostools
rostools.update_path(PKG)
rostools.update_path('rostest')
rostools.update_path('std_msgs')
rostools.update_path('robot_msgs')
rostools.update_path('rostest')
rostools.update_path('rospy')

import sys, unittest
import os, os.path, threading, time
import rospy, rostest
from std_msgs.msg import *

TARGET_VX = 0.5
TARGET_VY = 0.0
TARGET_VW = 0.0
TARGET_DURATION = 2.0

Esempio n. 53

File: spacenav_teleop.py Progetto: janfrs/kwc-ros-pkg

#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# 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: Stuart Glaser

import rostools; rostools.update_path('teleop_spacenav')
import rospy, sys
from robot_mechanism_controllers.srv import GetVector, SetVectorCommand
from std_msgs.msg import Vector3

def print_usage(code = 0):
    print sys.argv[0], '<cartesian position controller topic>'
    sys.exit(code)


if __name__ == '__main__':
    if len(sys.argv) < 2:
        print_usage(1)

    topic = sys.argv[1]
    get_position = rospy.ServiceProxy(topic + '/get_actual', GetVector)

Esempio n. 54

File: directed.py Progetto: Calm-wy/kwc-ros-pkg

import rostools
rostools.update_path('calonder_descriptor')
import rostest

import sys
sys.path.append('lib')
import calonder
import Image
import ImageChops
import ImageDraw

import unittest
import random
import time
import math

import fast

class TestDirected(unittest.TestCase):

    def setUp(self):
      pass

    def test_identity(self):
      im = Image.open("/u/konolige/vslam/data/indoor1/left-%04d.ppm" % 1000)
      kp = [(x-16, y-16) for (x,y) in fast.fast(im.tostring(), im.size[0], im.size[1], 150, 40)]

      cl1 = calonder.classifier()

      cl1.train(im.tostring(), im.size[0], im.size[1], kp)
      cl1.write('unittest.tree')

Esempio n. 55

#! /usr/bin/python

import math
import rostools
rostools.update_path('2dnav_pr2')

import glob
import sys, traceback, logging, rospy
from rosrecord import *
from std_msgs.msg import RobotBase2DOdom

NAME = 'log_parser'


class LogParser:
    def __init__(self):
        self.start_time = None
        self.end_time = None
        self.distance = 0.0
        self.radians = 0.0
        self.last_data = None
        self.msgs = []

    def new_odom(self, msg):
        #if this is the first message, log the time
        if self.last_data == None:
            self.start_time = msg.header.stamp
            self.end_time = msg.header.stamp
            self.last_data = msg
            return

Esempio n. 56

File: ros_simple.py Progetto: Calm-wy/kwc-ros-pkg

# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 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.

__doc__ = """
ROS_PLOT
A small utility to plot data from ROS topics."""

import wx

import rostools
rostools.update_path('wxpy_ros')
import wxpy_ros
import rospy

# The list of subtopics we want to plot. It is a map between the name of the window and the list of pairs of (path, style)
#a_plot=('/mechanism_state/actuator_states[3]/position', 'r')
#another_plot=('/mechanism_state/actuator_states[2]/position', 'b')
#a_list_of_plots=[a_plot,another_plot]
## And we specify the timespan as well
#all_plots = {'positions' : (a_list_of_plots,10)}

#Here is what it looks like:

#plots1=[('/mechanism_state/actuator_states[8]/position', 'r'),\
#('/mechanism_state/actuator_states[7]/position', 'b'), \
#('/mechanism_state/actuator_states[6]/position', 'g'), \

Esempio n. 57

import rostools
rostools.update_path('scipy')
from scipy import *
rostools.update_path('matplotlib')
from pylab import *


def fourier_coeffs(n, sigvect, T):  #function statements must end in a colon
    dt = T / (len(sigvect) - 1)
    t = arange(0, T + dt, dt)
    wo = 2 * pi / T
    cosfunc = cos(n * wo * t) * sigvect
    sinfunc = sin(n * wo * t) * sigvect
    A = integrate.trapz(cosfunc) * 2 / T * dt
    B = integrate.trapz(sinfunc) * 2 / T * dt
    return A, B, cosfunc, sinfunc


t2 = arange(1, 100, 0.01)
t2 = t2[:4096]
T = t2[-1]
print T
N = int(len(t2) / 2)
y2 = cos(2 * 3.14 * t2)
fy2 = fft(y2)
fy2 = fy2[:N]

#A1,B1,cos1,sin1=fourier_coeffs(1,y2,T)
#A2,B2,cos2,sin2=fourier_coeffs(2,y2,T)