diff --git a/scripts/start_sot_online_walking.py b/scripts/start_sot_online_walking.py
index 741d4ce14cf5f6a4492ffcffc938815561becf04..ecb32d8f7a693934d554bcd8246538eff4c96fbd 100755
--- a/scripts/start_sot_online_walking.py
+++ b/scripts/start_sot_online_walking.py
@@ -2,16 +2,14 @@
# O. Stasse 17/01/2020
# LAAS, CNRS
-import os
-import rospy
+import math
import time
-import roslaunch
-import rospkg
import unittest
-import math
-
-from gazebo_msgs.srv import *
+import roslaunch
+import rospkg
+import rospy
+from gazebo_msgs.srv import GetModelState
from std_srvs.srv import Empty
PKG_NAME = 'talos_integration_tests'
@@ -58,11 +56,9 @@ class TestSoTTalos(unittest.TestCase):
talos_data_path + '/launch/talos_gazebo_alone.launch', 'world:=empty_forced', 'enable_leg_passive:=false'
]
roslaunch_args = cli_args[1:]
- roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments\
- (cli_args)[0], roslaunch_args)]
+ roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)]
- launch_gazebo_alone = roslaunch.parent.\
- ROSLaunchParent(uuid, roslaunch_file)
+ launch_gazebo_alone = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file)
launch_gazebo_alone.start()
rospy.loginfo("talos_gazebo_alone started")
@@ -72,31 +68,28 @@ class TestSoTTalos(unittest.TestCase):
time.sleep(5)
# Spawn talos model in gazebo
- launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,\
- [talos_data_path+'/launch/talos_gazebo_spawn_hs.launch'])
- #launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,
+ launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(
+ uuid, [talos_data_path + '/launch/talos_gazebo_spawn_hs.launch'])
+ # launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,
# [ros_data_path+'/launch/talos_gazebo_spawn_hs_wide.launch'])
launch_gazebo_spawn_hs.start()
rospy.loginfo("talos_gazebo_spawn_hs started")
rospy.wait_for_service("/gains/arm_left_1_joint/set_parameters")
time.sleep(5)
- gazebo_unpause_physics = rospy.\
- ServiceProxy('/gazebo/unpause_physics', Empty)
+ gazebo_unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
gazebo_unpause_physics()
# Start roscontrol
- launch_bringup = roslaunch.parent.ROSLaunchParent(uuid,\
- [talos_data_path+'/launch/talos_bringup.launch'])
+ launch_bringup = roslaunch.parent.ROSLaunchParent(uuid, [talos_data_path + '/launch/talos_bringup.launch'])
launch_bringup.start()
rospy.loginfo("talos_bringup started")
# Start sot
roscontrol_sot_talos_path = arospack.get_path('roscontrol_sot_talos')
- launch_roscontrol_sot_talos =roslaunch.parent.ROSLaunchParent(uuid,\
- [roscontrol_sot_talos_path+\
- '/launch/sot_talos_controller_gazebo.launch'])
+ launch_roscontrol_sot_talos = roslaunch.parent.ROSLaunchParent(
+ uuid, [roscontrol_sot_talos_path + '/launch/sot_talos_controller_gazebo.launch'])
launch_roscontrol_sot_talos.start()
rospy.loginfo("roscontrol_sot_talos started")
@@ -104,14 +97,12 @@ class TestSoTTalos(unittest.TestCase):
pkg_name = 'talos_integration_tests'
executable = 'test_online_walking.py'
node_name = 'test_online_walking_py'
- test_sot_online_walking_node = roslaunch.core.\
- Node(pkg_name, executable,name=node_name)
+ test_sot_online_walking_node = roslaunch.core.Node(pkg_name, executable, name=node_name)
launch_test_sot_online_walking = roslaunch.scriptapi.ROSLaunch()
launch_test_sot_online_walking.start()
- test_sot_online_walking_process = launch_test_sot_online_walking.\
- launch(test_sot_online_walking_node)
+ test_sot_online_walking_process = launch_test_sot_online_walking.launch(test_sot_online_walking_node)
r = rospy.Rate(10)
while not rospy.is_shutdown():
diff --git a/scripts/start_sot_talos_balance.py b/scripts/start_sot_talos_balance.py
index 55446491e3c38cf5e27d4486477ae3603cbe4952..fa66ccb4c1690d3d50608c817ac443129c0f2805 100755
--- a/scripts/start_sot_talos_balance.py
+++ b/scripts/start_sot_talos_balance.py
@@ -2,16 +2,14 @@
# O. Stasse 17/01/2020
# LAAS, CNRS
-import os
-import rospy
+import math
import time
-import roslaunch
-import rospkg
import unittest
-import math
-
-from gazebo_msgs.srv import *
+import roslaunch
+import rospkg
+import rospy
+from gazebo_msgs.srv import GetModelState
from std_srvs.srv import Empty
PKG_NAME = 'talos_integration_tests'
@@ -67,31 +65,28 @@ class TestSoTTalos(unittest.TestCase):
time.sleep(5)
# Spawn talos model in gazebo
- launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,\
- [talos_data_path+'/launch/talos_gazebo_spawn_hs.launch'])
- #launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,
+ launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(
+ uuid, [talos_data_path + '/launch/talos_gazebo_spawn_hs.launch'])
+ # launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,
# [ros_data_path+'/launch/talos_gazebo_spawn_hs_wide.launch'])
launch_gazebo_spawn_hs.start()
rospy.loginfo("talos_gazebo_spawn_hs started")
rospy.wait_for_service("/gains/arm_left_1_joint/set_parameters")
time.sleep(5)
- gazebo_unpause_physics = rospy.\
- ServiceProxy('/gazebo/unpause_physics', Empty)
+ gazebo_unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
gazebo_unpause_physics()
# Start roscontrol
- launch_bringup = roslaunch.parent.ROSLaunchParent(uuid,\
- [talos_data_path+'/launch/talos_bringup.launch'])
+ launch_bringup = roslaunch.parent.ROSLaunchParent(uuid, [talos_data_path + '/launch/talos_bringup.launch'])
launch_bringup.start()
rospy.loginfo("talos_bringup started")
# Start sot
roscontrol_sot_talos_path = arospack.get_path('roscontrol_sot_talos')
- launch_roscontrol_sot_talos =roslaunch.parent.ROSLaunchParent(uuid,\
- [roscontrol_sot_talos_path+\
- '/launch/sot_talos_controller_gazebo.launch'])
+ launch_roscontrol_sot_talos = roslaunch.parent.ROSLaunchParent(
+ uuid, [roscontrol_sot_talos_path + '/launch/sot_talos_controller_gazebo.launch'])
launch_roscontrol_sot_talos.start()
rospy.loginfo("roscontrol_sot_talos started")
@@ -99,14 +94,12 @@ class TestSoTTalos(unittest.TestCase):
pkg_name = 'talos_integration_tests'
executable = 'test_sot_talos_balance.py'
node_name = 'test_sot_talos_balance_py'
- test_sot_talos_balance_node = roslaunch.core.\
- Node(pkg_name, executable,name=node_name)
+ test_sot_talos_balance_node = roslaunch.core.Node(pkg_name, executable, name=node_name)
launch_test_sot_talos_balance = roslaunch.scriptapi.ROSLaunch()
launch_test_sot_talos_balance.start()
- test_sot_talos_balance_process = launch_test_sot_talos_balance.\
- launch(test_sot_talos_balance_node)
+ test_sot_talos_balance_process = launch_test_sot_talos_balance.launch(test_sot_talos_balance_node)
r = rospy.Rate(10)
while not rospy.is_shutdown():
diff --git a/scripts/start_talos_gazebo.py b/scripts/start_talos_gazebo.py
index e5e876d7ad2cb06b418c2362687eada0c8e341c7..a906b33a943fc6504165969e42566145ff93bfb0 100755
--- a/scripts/start_talos_gazebo.py
+++ b/scripts/start_talos_gazebo.py
@@ -2,16 +2,14 @@
# O. Stasse 17/01/2020
# LAAS, CNRS
-import os
-import rospy
+import subprocess
import time
+
import roslaunch
import rospkg
-
+import rospy
from std_srvs.srv import Empty
-# Start roscore
-import subprocess
roscore = subprocess.Popen('roscore')
time.sleep(1)
@@ -40,7 +38,7 @@ time.sleep(5)
# Spawn talos model in gazebo
launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,
[talos_data_path + '/launch/talos_gazebo_spawn_hs.launch'])
-#launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,
+# launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,
# [talos_data_path+'/launch/talos_gazebo_spawn_hs_wide.launch'])
launch_gazebo_spawn_hs.start()
rospy.loginfo("talos_gazebo_spawn_hs started")
diff --git a/scripts/start_talos_gazebo_kine.py b/scripts/start_talos_gazebo_kine.py
index cd248e9c0b2759e44791f39f36504fd978773520..9b0a173cc9e99bc8567c8bccdfe5fce15980dc01 100755
--- a/scripts/start_talos_gazebo_kine.py
+++ b/scripts/start_talos_gazebo_kine.py
@@ -2,16 +2,14 @@
# O. Stasse 17/01/2020
# LAAS, CNRS
-import os
-import rospy
+import math
import time
-import roslaunch
-import rospkg
import unittest
-import math
-
-from gazebo_msgs.srv import *
+import roslaunch
+import rospkg
+import rospy
+from gazebo_msgs.srv import GetLinkState
from std_srvs.srv import Empty
PKG_NAME = 'talos_integration_tests'
@@ -70,30 +68,27 @@ class TestSoTTalos(unittest.TestCase):
time.sleep(5)
# Spawn talos model in gazebo
- launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,\
- [talos_data_path+'/launch/talos_gazebo_spawn_hs.launch'])
- #launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,
+ launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(
+ uuid, [talos_data_path + '/launch/talos_gazebo_spawn_hs.launch'])
+ # launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid,
# [talos_data_path+'/launch/talos_gazebo_spawn_hs_wide.launch'])
launch_gazebo_spawn_hs.start()
rospy.loginfo("talos_gazebo_spawn_hs started")
rospy.wait_for_service("/gains/arm_left_1_joint/set_parameters")
time.sleep(5)
- gazebo_unpause_physics = \
- rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
+ gazebo_unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
gazebo_unpause_physics()
# Start roscontrol
- launch_bringup = roslaunch.parent.ROSLaunchParent(uuid,\
- [talos_data_path+'/launch/talos_bringup.launch'])
+ launch_bringup = roslaunch.parent.ROSLaunchParent(uuid, [talos_data_path + '/launch/talos_bringup.launch'])
launch_bringup.start()
rospy.loginfo("talos_bringup started")
# Start sot
roscontrol_sot_talos_path = arospack.get_path('roscontrol_sot_talos')
- launch_roscontrol_sot_talos =roslaunch.parent.ROSLaunchParent(uuid,\
- [roscontrol_sot_talos_path+\
- '/launch/sot_talos_controller_gazebo.launch'])
+ launch_roscontrol_sot_talos = roslaunch.parent.ROSLaunchParent(
+ uuid, [roscontrol_sot_talos_path + '/launch/sot_talos_controller_gazebo.launch'])
launch_roscontrol_sot_talos.start()
rospy.loginfo("roscontrol_sot_talos started")
diff --git a/scripts/test_appli_seq_play.py b/scripts/test_appli_seq_play.py
index 4bddaea5cf862312f2f1c9c1082d428efd26dcb3..f1c8942f88b4cc72b41c534ac853cbb7dfae279f 100755
--- a/scripts/test_appli_seq_play.py
+++ b/scripts/test_appli_seq_play.py
@@ -1,21 +1,26 @@
#!/usr/bin/python
-import sys
+
import rospy
+from dynamic_graph_bridge_msgs.srv import RunCommand
+from std_srvs.srv import Empty
-from std_srvs.srv import *
-from dynamic_graph_bridge_msgs.srv import *
+try:
+ # Python 2
+ input = raw_input
+except NameError:
+ pass
def launchScript(code, title, description=""):
- raw_input(title + ': ' + description)
+ input(title + ': ' + description)
rospy.loginfo(title)
rospy.loginfo(code)
for line in code:
- if line != '' and line[0] != '#':
- print line
+ if line and not line.strip().startswith('#'):
+ print(line)
answer = runCommandClient(str(line))
rospy.logdebug(answer)
- print answer
+ print(answer)
rospy.loginfo("...done with " + title)
@@ -37,11 +42,11 @@ try:
rospy.loginfo("Stack of Tasks launched")
launchScript(initCode, 'initialize SoT')
- raw_input("Wait before starting the dynamic graph")
+ input("Wait before starting the dynamic graph")
runCommandStartDynamicGraph()
- raw_input("Wait before starting the seqplay")
+ input("Wait before starting the seqplay")
runCommandClient("aSimpleSeqPlay.start()")
-except rospy.ServiceException, e:
+except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
diff --git a/scripts/test_kine.py b/scripts/test_kine.py
index dcd2064f24eee3d28055c0d84020911a99f7ccfb..1b37318b3f28bda4b33075f8d05543bb86fcc7c4 100755
--- a/scripts/test_kine.py
+++ b/scripts/test_kine.py
@@ -1,16 +1,13 @@
#! /usr/bin/env python
import sys
-import rospy
import time
-from os.path import abspath, dirname, join
-from std_srvs.srv import *
-from dynamic_graph_bridge_msgs.srv import *
+import rospy
+from dynamic_graph_bridge_msgs.srv import RunCommand
+from std_srvs.srv import Empty
runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
-from sot_talos_balance.utils.run_test_utils import runCommandClient
-
def handleRunCommandClient(code):
out = runCommandClient(code)
@@ -60,7 +57,7 @@ def runTest():
time.sleep(10)
- except rospy.ServiceException, e:
+ except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
diff --git a/scripts/test_online_walking.py b/scripts/test_online_walking.py
index e601e0957c2649abce1275bee74e1acc271810a8..fc0acc2b4f1d1f23d64f7bb5e322d2b933d076a5 100644
--- a/scripts/test_online_walking.py
+++ b/scripts/test_online_walking.py
@@ -1,15 +1,11 @@
#! /usr/bin/env python
import sys
-import rospy
-import rospkg
import time
-import unittest
-import math
-from std_srvs.srv import *
-from dynamic_graph_bridge_msgs.srv import *
-
-from gazebo_msgs.srv import *
+import rospkg
+import rospy
+from sot_talos_balance.utils.run_test_utils import runCommandClient
+from std_srvs.srv import Empty
def handleRunCommandClient(code):
@@ -23,12 +19,6 @@ def handleRunCommandClient(code):
PKG_NAME = 'talos_integration_tests'
'''Test online walking pattern generator'''
-from sys import argv
-from sot_talos_balance.utils.run_test_utils import \
- run_ft_calibration, run_test, runCommandClient
-
-from time import sleep
-
def wait_for_dynamic_graph():
try:
@@ -51,8 +41,7 @@ rospack = rospkg.RosPack()
lpath = rospack.get_path(PKG_NAME)
print(lpath)
-appli_file_name =lpath + '/../../lib/'+PKG_NAME+\
- '/appli_online_walking.py'
+appli_file_name = lpath + '/../../lib/' + PKG_NAME + '/appli_online_walking.py'
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
rospy.loginfo("Stack of Tasks launched")
@@ -68,7 +57,7 @@ runCommandStartDynamicGraph()
rospy.loginfo("Stack of Tasks launched")
-#run_test(appli_file_name, verbosity=1,interactive=False)
+# run_test(appli_file_name, verbosity=1,interactive=False)
time.sleep(5)
# Connect ZMP reference and reset controllers
diff --git a/scripts/test_sot_talos_balance.py b/scripts/test_sot_talos_balance.py
index 7cf3b84f2dc32eea94117a42d84a99d4a6e45827..4fc4f028209ad915e30b71c7c2d3f2a02014b6c4 100755
--- a/scripts/test_sot_talos_balance.py
+++ b/scripts/test_sot_talos_balance.py
@@ -1,13 +1,11 @@
#! /usr/bin/env python
import sys
-import rospy
import time
-import unittest
-import math
from os.path import abspath, dirname, join
-from std_srvs.srv import *
-from dynamic_graph_bridge_msgs.srv import *
+import rospy
+from sot_talos_balance.utils.run_test_utils import runCommandClient
+from std_srvs.srv import Empty
def handleRunCommandClient(code):
@@ -20,8 +18,6 @@ def handleRunCommandClient(code):
PKG_NAME = 'talos_integration_tests'
'''Test CoM admittance control as described in paper, with pre-loaded movements'''
-from sot_talos_balance.utils.run_test_utils import \
- run_ft_calibration, run_test, runCommandClient
# get the file path for rospy_tutorials
appli_file_name = join(dirname(abspath(__file__)), 'appli_dcmZmpControl_file.py')
diff --git a/src/talos_integration_tests/appli.py b/src/talos_integration_tests/appli.py
index 4704e2f183c1331a883892b2fe1653fa0281e3e6..af9712933a024c3bd3f6b00526891ee225febdba 100644
--- a/src/talos_integration_tests/appli.py
+++ b/src/talos_integration_tests/appli.py
@@ -1,5 +1,5 @@
-from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.matrix_util import matrixToTuple
+from dynamic_graph.sot.core.meta_tasks_kine import (MetaTaskKine6d, MetaTaskKineCom)
from numpy import eye