~csiro-asl/csiro-asl-ros-drivers/trunk

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
#! /usr/bin/python

# 
# CSIRO Autonomous Systems Laboratory
# Queensland Centre for Advanced Technologies
# PO Box 883, Kenmore, QLD 4069, Australia
# http://www.ict.csiro.au/
#  
# Copyright (c) CSIRO 2010
# 
# Nick Hillier
# 
# Simple script to test physics plugins
#

import roslib

roslib.load_manifest('odephysics_ORplugin')

from openravepy import *

import signal
def signal_handler(signal, frame):
    exit()

signal.signal(signal.SIGINT, signal_handler)

# initialise OpenRave
dt = 0.005 # Simulation Timestep
env = Environment()
# optionally attach viewer
env.SetViewer('qtcoin') # attaching a viewer seems to stuff up the timesteps, so we set them AFTER attaching the viewer
env.StopSimulation()
# load an xml file to be the environment
fname = roslib.packages.get_pkg_dir('odephysics_ORplugin')+'/test/test_ackerman_steer.env.xml'
env.Load(fname) 
# get a reference to the test robot
robots = env.GetRobots()
testbot = []
for i in range(len(robots)):
    if robots[i].GetName() == 'chassis_and_wheels': # the name is specified in xml file
        testbot = robots[i]
        break

# setup controller
cont = RaveCreateController(env,"car_combinedcontroller")
import numpy as np
testbot.SetController(cont)
cont.SetDesired(([0, 0, 0, 0, 0, 0]))

#start simulation
with env:
    env.StartSimulation(timestep=dt,realtime=True) # limits the simulation to running at, or slower, than realtime.

import time
steer = 0.3
wheels = 3.0
dn = 1.0
while(1):
    dn = -dn
    cont.SetDesired(([dn*steer, 0, dn*steer, 0, wheels, wheels]))
    time.sleep(5)