[test] Add online walking test.

It does not look very different from the previous but it generates the
CoM and foot-steps trajectories in real time.
The sot-talos-balance test reads precomputed trajectories.

CMakeLists.txt

@@ -58,10 +58,13 @@ add_project_dependency(dynamic_graph_bridge REQUIRED)
 catkin_install_python(PROGRAMS
   scripts/appli.py
   scripts/appli_dcmZmpControl_file.py
+  scripts/appli_online_walking.py
   scripts/start_talos_gazebo_kine.py
   scripts/start_sot_talos_balance.py
+  scripts/start_sot_online_walking.py
   scripts/test_kine.py
   scripts/test_sot_talos_balance.py
+  scripts/test_online_walking.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 foreach(dir launch tests)
@@ -75,4 +78,5 @@ if(CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest(tests/test_kine.test)
   add_rostest(tests/test_sot_talos_balance.test)
+  add_rostest(tests/test_online_walking.test)  
 endif()

README.md

@@ -24,6 +24,7 @@ Preparing your environment variables:
 source ./test_ws/install/setup.bash
 source $HOME/bin/setup-opt-robotpkg.sh
 ```
+
 # First integration tests
 
 ## Kinematic SoT on gazebo
@@ -50,3 +51,11 @@ There is a you tube video showing what to expect:
 
 [![Alt text](http://i3.ytimg.com/vi/Hd46shZ22dM/hqdefault.jpg)](https://youtu.be/Hd46shZ22dM)
 
+## Real time on-line walking on gazebo
+
+To launch:
+```
+rostest talos_integration_tests test_online_walking.test
+```
+The robot is supposed to walk forward 2.8 m and reached position [2.12,0.012,1.00]
+

scripts/appli_online_walking.py

+# flake8: noqa
+
+import sys
+
+from math import sqrt
+
+import numpy as np
+from dynamic_graph import plug
+from dynamic_graph.sot.core import SOT, Derivator_of_Vector
+from dynamic_graph.sot.core import FeaturePosture, MatrixHomoToPoseQuaternion
+from dynamic_graph.sot.core import Task, RPYToMatrix
+from dynamic_graph.sot.core.matrix_util import matrixToTuple
+# RPYToMatrix deuxieme pour essai ajout waistattitudeabsolute
+from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
+from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKineCom, gotoNd
+from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio 
+from dynamic_graph.tracer_real_time import TracerRealTime
+from rospkg import RosPack
+
+import sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
+import sot_talos_balance.talos.control_manager_conf as cm_conf
+import sot_talos_balance.talos.ft_calibration_conf as ft_conf
+import sot_talos_balance.talos.parameter_server_conf as param_server_conf
+from sot_talos_balance.create_entities_utils import *
+
+from dynamic_graph.sot.pattern_generator import PatternGenerator
+
+# 09.04.20 est a 100 dans dcmZmpControl_file, used to be 10
+cm_conf.CTRL_MAX = 100.0  # temporary hack
+
+robot.timeStep = robot.device.getTimeStep()
+dt = robot.timeStep
+
+# --- Pendulum parameters
+robot_name = 'robot'
+robot.dynamic.com.recompute(0)
+robotDim = robot.dynamic.getDimension()
+mass = robot.dynamic.data.mass[0]
+h = robot.dynamic.com.value[2]
+g = 9.81
+omega = sqrt(g / h)
+
+# --- Parameter server
+robot.param_server = create_parameter_server(param_server_conf, dt)
+
+# --- Initial feet and waist
+robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle'])
+robot.dynamic.createOpPoint('RF', robot.OperationalPointsMap['right-ankle'])
+robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist'])
+robot.dynamic.LF.recompute(0)
+robot.dynamic.RF.recompute(0)
+robot.dynamic.WT.recompute(0)
+
+# -------------------------- DESIRED TRAJECTORY --------------------------
+
+rospack = RosPack()  
+
+# -------------------------- PATTERN GENERATOR --------------------------
+
+robot.pg = PatternGenerator('pg')
+
+# MODIFIED WITH MY PATHS
+talos_data_folder = rospack.get_path('talos_data')
+robot.pg.setURDFpath(talos_data_folder + '/urdf/talos_reduced_wpg.urdf')
+robot.pg.setSRDFpath(talos_data_folder + '/srdf/talos_wpg.srdf')
+## END MODIFIED
+
+robot.pg.buildModel()
+
+robot.pg.parseCmd(":samplingperiod 0.005")
+robot.pg.parseCmd(":previewcontroltime 1.6")
+robot.pg.parseCmd(":omega 0.0")
+robot.pg.parseCmd(':stepheight 0.05')
+robot.pg.parseCmd(':doublesupporttime 0.2')
+robot.pg.parseCmd(':singlesupporttime 1.0')
+robot.pg.parseCmd(":armparameters 0.5")
+robot.pg.parseCmd(":LimitsFeasibility 0.0")
+robot.pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
+robot.pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.7 3.0")
+robot.pg.parseCmd(":UpperBodyMotionParameters -0.1 -1.0 0.0")
+robot.pg.parseCmd(":comheight 0.876681")
+robot.pg.parseCmd(":setVelReference  0.1 0.0 0.0")
+
+robot.pg.parseCmd(":SetAlgoForZmpTrajectory Naveau")
+
+plug(robot.dynamic.position, robot.pg.position)
+plug(robot.dynamic.com, robot.pg.com)
+#plug(robot.dynamic.com, robot.pg.comStateSIN)
+plug(robot.dynamic.LF, robot.pg.leftfootcurrentpos)
+plug(robot.dynamic.RF, robot.pg.rightfootcurrentpos)
+robotDim = len(robot.dynamic.velocity.value)
+robot.pg.motorcontrol.value = robotDim * (0, )
+robot.pg.zmppreviouscontroller.value = (0, 0, 0)
+
+robot.pg.initState()
+
+robot.pg.parseCmd(':setDSFeetDistance 0.162')
+
+robot.pg.parseCmd(':NaveauOnline')
+robot.pg.parseCmd(':numberstepsbeforestop 2')
+robot.pg.parseCmd(':setfeetconstraint XY 0.091 0.0489')
+
+robot.pg.parseCmd(':deleteallobstacles')
+robot.pg.parseCmd(':feedBackControl false')
+robot.pg.parseCmd(':useDynamicFilter true')
+
+robot.pg.velocitydes.value = (0.1, 0.0, 0.0)  # DEFAULT VALUE (0.1,0.0,0.0)
+
+# -------------------------- TRIGGER --------------------------
+
+robot.triggerPG = BooleanIdentity('triggerPG')
+robot.triggerPG.sin.value = 0
+plug(robot.triggerPG.sout, robot.pg.trigger)
+
+# --------- Interface with controller entities -------------
+
+wp = DummyWalkingPatternGenerator('dummy_wp')
+wp.init()
+# #wp.displaySignals()
+wp.omega.value = omega
+
+# 22.04 after modifying pg.cpp, new way to try and connect the waist
+plug(robot.pg.waistattitudematrixabsolute, wp.waist)
+plug(robot.pg.leftfootref, wp.footLeft)
+plug(robot.pg.rightfootref, wp.footRight)
+plug(robot.pg.comref, wp.com)
+plug(robot.pg.dcomref, wp.vcom)
+plug(robot.pg.ddcomref, wp.acom)
+
+robot.wp = wp
+
+# --- Compute the values to use them in initialization
+robot.wp.comDes.recompute(0)
+robot.wp.dcmDes.recompute(0)
+robot.wp.zmpDes.recompute(0)
+
+## END ADDED
+
+# -------------------------- ESTIMATION --------------------------
+
+# --- Base Estimation
+robot.device_filters = create_device_filters(robot, dt)
+robot.imu_filters = create_imu_filters(robot, dt)
+robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
+
+robot.m2qLF = MatrixHomoToPoseQuaternion('m2qLF')
+plug(robot.dynamic.LF, robot.m2qLF.sin)
+plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat)
+robot.m2qRF = MatrixHomoToPoseQuaternion('m2qRF')
+plug(robot.dynamic.RF, robot.m2qRF.sin)
+plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat)
+
+# --- Conversion
+e2q = EulerToQuat('e2q')
+plug(robot.base_estimator.q, e2q.euler)
+robot.e2q = e2q
+
+# --- Kinematic computations
+robot.rdynamic = DynamicPinocchio("real_dynamics")
+robot.rdynamic.setModel(robot.dynamic.model)
+robot.rdynamic.setData(robot.rdynamic.model.createData())
+plug(robot.base_estimator.q, robot.rdynamic.position)
+robot.rdynamic.velocity.value = [0.0] * robotDim
+robot.rdynamic.acceleration.value = [0.0] * robotDim
+
+# --- CoM Estimation
+cdc_estimator = DcmEstimator('cdc_estimator')
+cdc_estimator.init(dt, robot_name)
+plug(robot.e2q.quaternion, cdc_estimator.q)
+plug(robot.base_estimator.v, cdc_estimator.v)
+robot.cdc_estimator = cdc_estimator
+
+# --- DCM Estimation
+estimator = DummyDcmEstimator("dummy")
+estimator.omega.value = omega
+estimator.mass.value = 1.0
+plug(robot.cdc_estimator.c, estimator.com)
+plug(robot.cdc_estimator.dc, estimator.momenta)
+estimator.init()
+robot.estimator = estimator
+
+# --- Force calibration
+robot.ftc = create_ft_calibrator(robot, ft_conf)
+
+# --- ZMP estimation
+zmp_estimator = SimpleZmpEstimator("zmpEst")
+robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link')
+robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link')
+plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
+plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
+plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
+plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
+zmp_estimator.init()
+robot.zmp_estimator = zmp_estimator
+
+# -------------------------- ADMITTANCE CONTROL --------------------------
+
+# --- DCM controller
+Kp_dcm = [8.0] * 3
+Ki_dcm = [0.0, 0.0, 0.0]  # zero (to be set later)
+gamma_dcm = 0.2
+
+dcm_controller = DcmController("dcmCtrl")
+
+dcm_controller.Kp.value = Kp_dcm
+dcm_controller.Ki.value = Ki_dcm
+dcm_controller.decayFactor.value = gamma_dcm
+dcm_controller.mass.value = mass
+dcm_controller.omega.value = omega
+
+plug(robot.cdc_estimator.c, dcm_controller.com)
+plug(robot.estimator.dcm, dcm_controller.dcm)
+
+plug(robot.wp.zmpDes, dcm_controller.zmpDes)
+plug(robot.wp.dcmDes, dcm_controller.dcmDes)
+
+dcm_controller.init(dt)
+
+robot.dcm_control = dcm_controller
+
+Ki_dcm = [1.0, 1.0, 1.0]  # this value is employed later
+
+# --- CoM admittance controller
+Kp_adm = [0.0, 0.0, 0.0]  # zero (to be set later)
+
+com_admittance_control = ComAdmittanceController("comAdmCtrl")
+com_admittance_control.Kp.value = Kp_adm
+plug(robot.zmp_estimator.zmp, com_admittance_control.zmp)
+com_admittance_control.zmpDes.value = robot.wp.zmpDes.value
+# should be plugged to robot.dcm_control.zmpRef
+plug(robot.wp.acomDes, com_admittance_control.ddcomDes)
+
+com_admittance_control.init(dt)
+com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0])
+
+robot.com_admittance_control = com_admittance_control
+
+Kp_adm = [15.0, 15.0, 0.0]  # this value is employed later
+
+# --- Control Manager
+robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot')
+robot.cm.addCtrlMode('sot_input')
+robot.cm.setCtrlMode('all', 'sot_input')
+robot.cm.addEmergencyStopSIN('zmp')
+
+# -------------------------- SOT CONTROL --------------------------
+
+# --- Upper body
+robot.taskUpperBody = Task('task_upper_body')
+robot.taskUpperBody.feature = FeaturePosture('feature_upper_body')
+
+q = list(robot.dynamic.position.value)
+robot.taskUpperBody.feature.state.value = q
+robot.taskUpperBody.feature.posture.value = q
+
+robot.taskUpperBody.feature.selectDof(18, True)
+robot.taskUpperBody.feature.selectDof(19, True)
+robot.taskUpperBody.feature.selectDof(20, True)
+robot.taskUpperBody.feature.selectDof(21, True)
+robot.taskUpperBody.feature.selectDof(22, True)
+robot.taskUpperBody.feature.selectDof(23, True)
+robot.taskUpperBody.feature.selectDof(24, True)
+robot.taskUpperBody.feature.selectDof(25, True)
+robot.taskUpperBody.feature.selectDof(26, True)
+robot.taskUpperBody.feature.selectDof(27, True)
+robot.taskUpperBody.feature.selectDof(28, True)
+robot.taskUpperBody.feature.selectDof(29, True)
+robot.taskUpperBody.feature.selectDof(30, True)
+robot.taskUpperBody.feature.selectDof(31, True)
+robot.taskUpperBody.feature.selectDof(32, True)
+robot.taskUpperBody.feature.selectDof(33, True)
+robot.taskUpperBody.feature.selectDof(34, True)
+robot.taskUpperBody.feature.selectDof(35, True)
+robot.taskUpperBody.feature.selectDof(36, True)
+robot.taskUpperBody.feature.selectDof(37, True)
+
+robot.taskUpperBody.controlGain.value = 100.0
+robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
+plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
+
+# --- CONTACTS
+robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle'])
+robot.contactLF.feature.frame('desired')
+robot.contactLF.gain.setConstant(300)
+plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position)  #.errorIN?
+locals()['contactLF'] = robot.contactLF
+
+robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle'])
+robot.contactRF.feature.frame('desired')
+robot.contactRF.gain.setConstant(300)
+plug(robot.wp.footRightDes, robot.contactRF.featureDes.position)  #.errorIN?
+locals()['contactRF'] = robot.contactRF
+
+# --- COM height
+robot.taskComH = MetaTaskKineCom(robot.dynamic, name='comH')
+plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN)
+robot.taskComH.task.controlGain.value = 100.
+robot.taskComH.feature.selec.value = '100'
+
+# --- COM
+robot.taskCom = MetaTaskKineCom(robot.dynamic)
+plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
+plug(robot.com_admittance_control.dcomRef, robot.taskCom.featureDes.errordotIN)
+robot.taskCom.task.controlGain.value = 0
+robot.taskCom.task.setWithDerivative(True)
+robot.taskCom.feature.selec.value = '011'
+
+# --- Waist
+
+robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT', robot.OperationalPointsMap['waist'])
+robot.keepWaist.feature.frame('desired')
+robot.keepWaist.gain.setConstant(300)
+plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)  #de base
+robot.keepWaist.feature.selec.value = '111000'
+locals()['keepWaist'] = robot.keepWaist
+
+# --- SOT solver
+robot.sot = SOT('sot')
+robot.sot.setSize(robot.dynamic.getDimension())
+
+# --- Plug SOT control to device through control manager
+plug(robot.sot.control, robot.cm.ctrl_sot_input)
+plug(robot.cm.u_safe, robot.device.control)
+
+robot.sot.push(robot.taskUpperBody.name)
+robot.sot.push(robot.contactRF.task.name)
+robot.sot.push(robot.contactLF.task.name)
+robot.sot.push(robot.taskComH.task.name)
+robot.sot.push(robot.taskCom.task.name)
+robot.sot.push(robot.keepWaist.task.name)
+
+# --- Fix robot.dynamic inputs
+plug(robot.device.velocity, robot.dynamic.velocity)
+robot.dvdt = Derivator_of_Vector("dv_dt")
+robot.dvdt.dt.value = dt
+plug(robot.device.velocity, robot.dvdt.sin)
+plug(robot.dvdt.sout, robot.dynamic.acceleration)
+
+# -------------------------- PLOTS --------------------------
+
+# --- ROS PUBLISHER
+
+## THIS PARAGRAPH QUITE DIFFERENT, TO CHECK
+
+robot.publisher = create_rospublish(robot, 'robot_publisher')
+
+## ADDED
+create_topic(robot.publisher, robot.pg, 'comref', robot=robot,
+    data_type='vector')  # desired CoM
+create_topic(robot.publisher, robot.pg, 'dcomref', robot=robot,data_type='vector')
+
+create_topic(robot.publisher, robot.wp, 'waist', robot=robot, data_type='matrixHomo')
+create_topic(robot.publisher, robot.keepWaist.featureDes,'position',robot=robot,  data_type='matrixHomo')
+create_topic( robot.publisher, robot.dynamic, 'WT', robot=robot,data_type='matrixHomo')
+create_topic(robot.publisher, robot.pg, 'waistattitudematrixabsolute',robot=robot,  data_type='matrixHomo')  ## que font ces lignes exactement ??
+
+create_topic( robot.publisher, robot.pg,'leftfootref', robot=robot,   data_type='matrixHomo')
+create_topic( robot.publisher, robot.wp, 'footLeft', robot=robot,data_type='matrixHomo')
+create_topic(robot.publisher, robot.pg, 'rightfootref', robot=robot,data_type='matrixHomo')
+create_topic( robot.publisher, robot.wp, 'footRight', robot=robot,data_type='matrixHomo')
+
+## --- TRACER
+robot.tracer = TracerRealTime("com_tracer")
+robot.tracer.setBufferSize(80 * (2**20))
+robot.tracer.open('/tmp', 'dg_', '.dat')
+
+robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
+
+addTrace(robot.tracer, robot.pg, 'waistattitudeabsolute')
+# fin
+
+addTrace(robot.tracer, robot.wp, 'comDes')  # desired CoM
+
+addTrace(robot.tracer, robot.cdc_estimator, 'c')  # estimated CoM
+addTrace(robot.tracer, robot.cdc_estimator, 'dc')  # estimated CoM velocity
+
+addTrace(robot.tracer, robot.pg, 'comref')
+addTrace(robot.tracer, robot.pg, 'dcomref')
+addTrace(robot.tracer, robot.pg, 'ddcomref')
+
+addTrace(robot.tracer, robot.pg, 'rightfootref')
+addTrace(robot.tracer, robot.pg, 'leftfootref')
+
+addTrace(robot.tracer, robot.pg, 'rightfootcontact')
+addTrace(robot.tracer, robot.pg, 'leftfootcontact')
+addTrace(robot.tracer, robot.pg, 'SupportFoot')
+
+addTrace(robot.tracer, robot.dynamic, 'com')  # resulting SOT CoM
+addTrace(robot.tracer, robot.dynamic, 'LF')  # left foot
+addTrace(robot.tracer, robot.dynamic, 'RF')  # right foot
+
+robot.tracer.start()

scripts/start_sot_online_walking.py

+#!/usr/bin/env python
+# O. Stasse 17/01/2020
+# LAAS, CNRS
+
+import os
+import rospy
+import time
+import roslaunch
+import rospkg
+import unittest
+import math
+
+from gazebo_msgs.srv import *
+
+from std_srvs.srv import Empty
+
+PKG_NAME='talos_integration_tests'
+
+class TestSoTTalos(unittest.TestCase):
+
+    def validation_through_gazebo(self):
+        gzGetModelPropReq = rospy.ServiceProxy('/gazebo/get_model_state',
+                                               GetModelState)
+        gzGetModelPropResp = gzGetModelPropReq(model_name='talos')
+        f=open("/tmp/output.dat","w+")
+        f.write("x:"+str(gzGetModelPropResp.pose.position.x)+"\n")
+        f.write("y:"+str(gzGetModelPropResp.pose.position.y)+"\n")
+        f.write("z:"+str(gzGetModelPropResp.pose.position.z)+"\n")
+        dx=gzGetModelPropResp.pose.position.x-2.8331
+        dy=gzGetModelPropResp.pose.position.y-0.0405
+        dz=gzGetModelPropResp.pose.position.z-1.0019
+        ldistance = math.sqrt(dx*dx+dy*dy+dz*dz)
+        f.write("dist:"+str(ldistance))
+        f.close()
+        if ldistance<0.009:
+            self.assertTrue(True,msg="Converged to the desired position")
+        else:
+            self.assertFalse(False,
+                             msg="Did not converged to the desired position")
+
+    def runTest(self):
+        # Start roscore
+        import subprocess
+        roscore = subprocess.Popen('roscore')
+        time.sleep(1)
+
+        # Get the path to talos_data
+        arospack = rospkg.RosPack()
+        talos_data_path = arospack.get_path('talos_data')
+
+        # Start talos_gazebo
+        rospy.init_node('starting_talos_gazebo', anonymous=True)
+        uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
+        roslaunch.configure_logging(uuid)
+
+        cli_args = [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)]
+
+        launch_gazebo_alone = roslaunch.parent.\
+            ROSLaunchParent(uuid, roslaunch_file)
+        launch_gazebo_alone.start()
+        rospy.loginfo("talos_gazebo_alone started")
+
+        rospy.wait_for_service("/gazebo/pause_physics")
+        gazebo_pause_physics = rospy.ServiceProxy('/gazebo/pause_physics',
+                                                  Empty)
+        gazebo_pause_physics()
+
+        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,
+        #    [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()
+
+        # Start roscontrol
+        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.start()
+        rospy.loginfo("roscontrol_sot_talos started")
+
+        time.sleep(5)
+        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)
+
+        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)
+
+        r = rospy.Rate(10)
+        while not rospy.is_shutdown():
+            # Test if sot_online_walking is finished or not
+            if not test_sot_online_walking_process.is_alive():
+
+                self.validation_through_gazebo()
+
+
+                # If it is finished then find exit status.
+                if test_sot_online_walking_process.exit_code != 0:
+                    exit_status = "test_online_walking failed"
+                else:
+                    exit_status=None
+
+                print("Stopping SoT")
+                launch_roscontrol_sot_talos.shutdown()
+                print("Stopping Gazebo")
+                launch_gazebo_alone.shutdown()
+
+                rospy.signal_shutdown(exit_status)
+
+                # Terminate the roscore subprocess
+                print("Stop roscore")
+                roscore.terminate()
+
+            r.sleep()
+
+if __name__ == '__main__':
+    import rosunit
+    rosunit.unitrun(PKG_NAME,'test_online_walking',TestSoTTalos)

scripts/test_online_walking.py

+#! /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 *