From 9a8435d44eb9e4ed315d1c75126c0f9d1ba9de1a Mon Sep 17 00:00:00 2001 From: Hugo_laptop Date: Fri, 10 Jul 2020 17:00:29 +0200 Subject: [PATCH 1/4] Huge commit: added fully integrated walking+lidar+aicp, fixed exit cond on tests, fixed install and dependencies --- CMakeLists.txt | 4 +- launch/talos_spawn_hs_ouster.launch | 29 ++++ package.xml | 3 +- scripts/start_sot_online_walking.py | 5 +- scripts/start_sot_ouster_walking.py | 202 ++++++++++++++++++++++++++++ scripts/start_sot_talos_balance.py | 7 +- scripts/start_talos_gazebo_kine.py | 5 +- scripts/test_kine.py | 6 +- scripts/test_online_walking.py | 25 ++-- scripts/test_sot_talos_balance.py | 14 +- tests/test_online_walking.test | 2 +- tests/test_ouster_walking.test | 7 + tests/test_sot_talos_balance.test | 2 +- 13 files changed, 286 insertions(+), 25 deletions(-) create mode 100644 launch/talos_spawn_hs_ouster.launch create mode 100755 scripts/start_sot_ouster_walking.py mode change 100644 => 100755 scripts/test_online_walking.py create mode 100644 tests/test_ouster_walking.test diff --git a/CMakeLists.txt b/CMakeLists.txt index 2bc40d3..55c78b2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,6 +59,7 @@ catkin_install_python(PROGRAMS scripts/start_talos_gazebo_kine.py scripts/start_sot_talos_balance.py scripts/start_sot_online_walking.py + scripts/start_sot_ouster_walking.py scripts/test_kine.py scripts/test_sot_talos_balance.py scripts/test_online_walking.py @@ -77,5 +78,6 @@ 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) + add_rostest(tests/test_online_walking.test) + add_rostest(tests/test_ouster_walking.test) endif() diff --git a/launch/talos_spawn_hs_ouster.launch b/launch/talos_spawn_hs_ouster.launch new file mode 100644 index 0000000..35fed83 --- /dev/null +++ b/launch/talos_spawn_hs_ouster.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 1c33dd9..7c1418e 100644 --- a/package.xml +++ b/package.xml @@ -25,5 +25,6 @@ talos_gazebo roscontrol_sot_talos dynamic_graph_bridge_msgs - + sot-pattern-generator + talos_bauzil diff --git a/scripts/start_sot_online_walking.py b/scripts/start_sot_online_walking.py index b2c123b..c4b2fd4 100755 --- a/scripts/start_sot_online_walking.py +++ b/scripts/start_sot_online_walking.py @@ -143,7 +143,10 @@ def runTest(self): print("Stop roscore") roscore.terminate() - r.sleep() + try: + r.sleep() + except rospy.ROSInterruptException: + rospy.logwarn("Exiting test") if __name__ == '__main__': import rosunit diff --git a/scripts/start_sot_ouster_walking.py b/scripts/start_sot_ouster_walking.py new file mode 100755 index 0000000..aa0f032 --- /dev/null +++ b/scripts/start_sot_ouster_walking.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python +# O. Stasse 17/01/2020 +# LAAS, CNRS +# from package robotpkg-talos-data +# Modified by H. Lefevre 02/07/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 depends on the timing of the simulation + # which can be different from one computer to another. + # Therefore check only dy and dz. + dx=0.0; + dy=gzGetModelPropResp.pose.position.y--1.51 + dz=gzGetModelPropResp.pose.position.z-0.997 + ldistance = math.sqrt(dx*dx+dy*dy+dz*dz) + f.write("dist:"+str(ldistance)) + f.close() + if ldistance<0.1: + self.assertTrue(True,msg="Converged to the desired position") + else: + self.assertFalse(True, + msg="Did not converge to the desired position") + + def runTest(self): + # Start roscore + import subprocess + roscore = subprocess.Popen('roscore') + time.sleep(2) + + # Get the path to talos_data + arospack = rospkg.RosPack() + talos_data_path = arospack.get_path('talos_data') + talos_bauzil_path = arospack.get_path('talos_bauzil') + talos_test_path = arospack.get_path('talos_integration_tests') + + # 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_bauzil_path+'/launch/script_walking_bauzil.launch', + 'world_name:=bauzil_skins', + #'robot:=full_v2', + 'robot:=full_v2_ouster', + '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_test_path+'/launch/talos_spawn_hs_ouster.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(3) + + # Launch test + pkg_name='talos_integration_tests' + executable='test_online_walking.py' + node_name='test_online_walking_py' + test_sot_ouster_walking_node = roslaunch.core.\ + Node(pkg_name, executable,name=node_name) + + launch_test_sot_ouster_walking=roslaunch.scriptapi.ROSLaunch() + launch_test_sot_ouster_walking.start() + + test_sot_ouster_walking_process = launch_test_sot_ouster_walking.\ + launch(test_sot_ouster_walking_node) + + time.sleep(3) + + # Publish odometry + launch_odom = roslaunch.parent.ROSLaunchParent(uuid, + [talos_bauzil_path+'/launch/talos_odometry.launch']) + launch_odom.start() + rospy.loginfo("Talos odometry started") + + time.sleep(2) + + # Start mapping + aicp_path = arospack.get_path('aicp_ros') + cli_args = [aicp_path+'/launch/aicp_mapping.launch', + 'pose_odom:=/odom_aicp', + 'lidar_channel:=/os1_cloud_node/points', + 'inertial_frame:=/world', + 'fixed_frame:=/world' + ] + + roslaunch_args = cli_args[1:] + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)] + + launch_aicp = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) + launch_aicp.start() + rospy.loginfo("aicp_mapping started") + + + + rospy.sleep(3) + r = rospy.Rate(10) + while not rospy.is_shutdown(): + # Test if sot_online_walking is finished or not + if not test_sot_ouster_walking_process.is_alive(): + + self.validation_through_gazebo() + + + # If it is finished then find exit status. + if test_sot_ouster_walking_process.exit_code != 0: + exit_status = "test_ouster_walking failed" + self.assertFalse(True,exit_status) + else: + exit_status="None" + + print("Stopping SoT") + launch_roscontrol_sot_talos.shutdown() + # print("Shutting down spawners") + # launch_gazebo_spawn_hs.shutdown() + # launch_bringup.shutdown() + print("Stopping Odometry") + launch_odom.shutdown() + print("Stopping Gazebo") + launch_gazebo_alone.shutdown() + print("Stopping Mapping") + launch_aicp.shutdown() + + + rospy.signal_shutdown(exit_status) + + # Terminate the roscore subprocess + print("Stop roscore") + roscore.terminate() + + + try: + r.sleep() + except rospy.ROSInterruptException: + rospy.logwarn("Exiting test") + + + +if __name__ == '__main__': + import rosunit + rosunit.unitrun(PKG_NAME,'test_online_walking',TestSoTTalos) + diff --git a/scripts/start_sot_talos_balance.py b/scripts/start_sot_talos_balance.py index bd3394c..cb88ed6 100755 --- a/scripts/start_sot_talos_balance.py +++ b/scripts/start_sot_talos_balance.py @@ -32,7 +32,7 @@ def validation_through_gazebo(self): ldistance = math.sqrt(dx*dx+dy*dy+dz*dz) f.write("dist:"+str(ldistance)) f.close() - if ldistance<0.009: + if ldistance<0.04: self.assertTrue(True,msg="Converged to the desired position") else: self.assertFalse(True, @@ -138,7 +138,10 @@ def runTest(self): print("Stop roscore") roscore.terminate() - r.sleep() + try: + r.sleep() + except rospy.ROSInterruptException: + rospy.logwarn("Exiting test") if __name__ == '__main__': import rosunit diff --git a/scripts/start_talos_gazebo_kine.py b/scripts/start_talos_gazebo_kine.py index add2cbb..c1212f4 100755 --- a/scripts/start_talos_gazebo_kine.py +++ b/scripts/start_talos_gazebo_kine.py @@ -133,7 +133,10 @@ def runTest(self): print("Stop roscore") roscore.terminate() - r.sleep() + try: + r.sleep() + except rospy.ROSInterruptException: + rospy.logwarn("Exiting test") if __name__ == '__main__': diff --git a/scripts/test_kine.py b/scripts/test_kine.py index c879ecc..42fce44 100755 --- a/scripts/test_kine.py +++ b/scripts/test_kine.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#!/usr/bin/env python import sys import rospy import time @@ -21,6 +21,8 @@ def handleRunCommandClient(code): PKG_NAME='talos_integration_tests' +rospy.init_node('test_kine', anonymous=True) + def runTest(): # Waiting for services try: @@ -56,7 +58,7 @@ def runTest(): handleRunCommandClient("gotoNd(robot.taskRH,target,'111',(4.9,0.9,0.01,0.9))") handleRunCommandClient("robot.sot.push(robot.taskRH.task.name)") - time.sleep(10) + rospy.sleep(10) except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e) diff --git a/scripts/test_online_walking.py b/scripts/test_online_walking.py old mode 100644 new mode 100755 index e9625f6..be7312b --- a/scripts/test_online_walking.py +++ b/scripts/test_online_walking.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#!/usr/bin/env python import sys import rospy import rospkg @@ -68,9 +68,11 @@ def wait_for_dynamic_graph(): rospy.loginfo("Stack of Tasks launched") +#initializing node to read ros time +rospy.init_node('test_online_walking', anonymous=True) #run_test(appli_file_name, verbosity=1,interactive=False) -time.sleep(5) +rospy.sleep(3) # Connect ZMP reference and reset controllers print('Connect ZMP reference') @@ -91,13 +93,18 @@ def wait_for_dynamic_graph(): print('Executing the trajectory') handleRunCommandClient('robot.triggerPG.sin.value = 1') -time.sleep(4) -handleRunCommandClient('robot.pg.velocitydes.value=(0.2,0.0,0.0)') -time.sleep(7) -handleRunCommandClient('robot.pg.velocitydes.value=(0.3,0.0,0.0)') -time.sleep(9) -handleRunCommandClient('robot.pg.velocitydes.value=(0.0,0.0,0.0)') -time.sleep(9) + +rospy.sleep(1) + + +handleRunCommandClient('robot.pg.velocitydes.value=(0.5,0.0,0.0)') +rospy.sleep(5) +handleRunCommandClient('robot.pg.velocitydes.value=(0.7,0.0,0.0)') +rospy.sleep(4) +handleRunCommandClient('robot.pg.velocitydes.value=(0.9,0.0,0.0)') +rospy.sleep(10) +handleRunCommandClient('robot.pg.velocitydes.value=(0.0,0.0,0.0)') +rospy.sleep(3) handleRunCommandClient('from sot_talos_balance.create_entities_utils import dump_tracer') handleRunCommandClient('dump_tracer(robot.tracer)') diff --git a/scripts/test_sot_talos_balance.py b/scripts/test_sot_talos_balance.py index 3ab1555..d3b368d 100755 --- a/scripts/test_sot_talos_balance.py +++ b/scripts/test_sot_talos_balance.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#!/usr/bin/env python import sys import rospy import time @@ -19,6 +19,8 @@ def handleRunCommandClient(code): PKG_NAME='talos_integration_tests' +rospy.init_node('test_balance_walking', anonymous=True) + '''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 @@ -26,7 +28,7 @@ def handleRunCommandClient(code): # get the file path for rospy_tutorials appli_file_name = join(dirname(abspath(__file__)), 'appli_dcmZmpControl_file.py') -time.sleep(2) +rospy.sleep(2) rospy.loginfo("Stack of Tasks launched") test_folder = 'TestKajita2003StraightWalking64/20cm' @@ -40,12 +42,12 @@ def handleRunCommandClient(code): handleRunCommandClient('from talos_integration_tests.appli_dcmZmpControl_file import init_sot_talos_balance') handleRunCommandClient('init_sot_talos_balance(robot,\''+test_folder+'\')') -time.sleep(5) +rospy.sleep(3) runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty) runCommandStartDynamicGraph() -time.sleep(5) +rospy.sleep(3) # Connect ZMP reference and reset controllers print('Connect ZMP reference') handleRunCommandClient('from dynamic_graph import plug') @@ -59,6 +61,6 @@ def handleRunCommandClient(code): handleRunCommandClient('robot.dcm_control.Ki.value = Ki_dcm') print('Executing the trajectory') -time.sleep(1) +rospy.sleep(1) handleRunCommandClient('robot.triggerTrajGen.sin.value = 1') -time.sleep(25) +rospy.sleep(20) diff --git a/tests/test_online_walking.test b/tests/test_online_walking.test index b07f205..a6f4c17 100644 --- a/tests/test_online_walking.test +++ b/tests/test_online_walking.test @@ -1,7 +1,7 @@ - + diff --git a/tests/test_ouster_walking.test b/tests/test_ouster_walking.test new file mode 100644 index 0000000..ef82eaa --- /dev/null +++ b/tests/test_ouster_walking.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/tests/test_sot_talos_balance.test b/tests/test_sot_talos_balance.test index 1423050..4ec636b 100644 --- a/tests/test_sot_talos_balance.test +++ b/tests/test_sot_talos_balance.test @@ -1,7 +1,7 @@ - + From e7c0c81de4359ab3d2a2bc3322ddbf6602fa6d44 Mon Sep 17 00:00:00 2001 From: Hugo_laptop Date: Thu, 13 Aug 2020 09:20:11 +0200 Subject: [PATCH 2/4] integrating and exporting new test --- CMakeLists.txt | 9 +- package.xml | 1 + scripts/start_torque_control.py | 161 ++++++++++++++++++++++ tests/test_torque_control_on_spot.test | 6 + tests/test_torque_control_on_spot_pg.test | 6 + tests/test_torque_control_walk_20.test | 6 + tests/test_torque_control_walk_20_pg.test | 6 + 7 files changed, 194 insertions(+), 1 deletion(-) create mode 100755 scripts/start_torque_control.py create mode 100644 tests/test_torque_control_on_spot.test create mode 100644 tests/test_torque_control_on_spot_pg.test create mode 100644 tests/test_torque_control_walk_20.test create mode 100644 tests/test_torque_control_walk_20_pg.test diff --git a/CMakeLists.txt b/CMakeLists.txt index 55c78b2..96eb00d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,6 +60,7 @@ catkin_install_python(PROGRAMS scripts/start_sot_talos_balance.py scripts/start_sot_online_walking.py scripts/start_sot_ouster_walking.py + scripts/start_torque_control.py scripts/test_kine.py scripts/test_sot_talos_balance.py scripts/test_online_walking.py @@ -79,5 +80,11 @@ if(CATKIN_ENABLE_TESTING) add_rostest(tests/test_kine.test) add_rostest(tests/test_sot_talos_balance.test) add_rostest(tests/test_online_walking.test) - add_rostest(tests/test_ouster_walking.test) + add_rostest(tests/test_ouster_walking.test) + add_rostest(tests/test_torque_control_on_spot.test) #add_rostest(tests/test_torque_control.test ARGS arg1:=true arg2:=false) + add_rostest(tests/test_torque_control_walk_20.test) + add_rostest(tests/test_torque_control_on_spot_pg.test) + add_rostest(tests/test_torque_control_walk_20_pg.test) + + endif() diff --git a/package.xml b/package.xml index 7c1418e..d1187d4 100644 --- a/package.xml +++ b/package.xml @@ -27,4 +27,5 @@ dynamic_graph_bridge_msgs sot-pattern-generator talos_bauzil + talos-torque-control diff --git a/scripts/start_torque_control.py b/scripts/start_torque_control.py new file mode 100755 index 0000000..674f8e5 --- /dev/null +++ b/scripts/start_torque_control.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python +# H. Lefevre 05/08/2020 +# LAAS, CNRS + +import os +import rospy +import time +import roslaunch +import rospkg +import unittest +import math +from sys import argv + +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 depends on the timing of the simulation + # which can be different from one computer to another. + # Therefore check only dy and dz. + dx=0.0; + dy=gzGetModelPropResp.pose.position.y-0.0038 + dz=gzGetModelPropResp.pose.position.z-1.00152 + ldistance = math.sqrt(dx*dx+dy*dy+dz*dz) + f.write("dist:"+str(ldistance)) + f.close() + if ldistance<0.05: + self.assertTrue(True,msg="Converged to the desired position") + else: + self.assertFalse(True, + msg="Did not converge 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', + 'debug:=true' + ] + 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_effort.launch']) + launch_roscontrol_sot_talos.start() + rospy.loginfo("roscontrol_sot_talos started") + + time.sleep(5) + pkg_name='talos-torque-control' + executable='sim_walk_torque.py' + node_name='sim_walk_torque_py' + + if len(argv)==3 and argv[2]=='pattern_generator': + mode=str(argv[1]+' '+argv[2]) + else: + mode=str(argv[1]) + + sim_walk_torque_node = roslaunch.core.\ + Node(pkg_name, executable,name=node_name,args=mode) + + launch_sim_walk_torque=roslaunch.scriptapi.ROSLaunch() + launch_sim_walk_torque.start() + + sim_walk_torque_process = launch_sim_walk_torque.\ + launch(sim_walk_torque_node) + + r = rospy.Rate(10) + while not rospy.is_shutdown(): + # Test if sim_walk_torque is finished or not + if not sim_walk_torque_process.is_alive(): + + self.validation_through_gazebo() + + + # If it is finished then find exit status. + if sim_walk_torque_process.exit_code != 0: + exit_status = "sim_walk_torque failed" + self.assertFalse(True,exit_status) + 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() + + try: + r.sleep() + except rospy.ROSInterruptException: + rospy.logwarn("Exiting test") + +if __name__ == '__main__': + import rosunit + rosunit.unitrun(PKG_NAME,'sim_walk_torque',TestSoTTalos) diff --git a/tests/test_torque_control_on_spot.test b/tests/test_torque_control_on_spot.test new file mode 100644 index 0000000..9b49bc9 --- /dev/null +++ b/tests/test_torque_control_on_spot.test @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/tests/test_torque_control_on_spot_pg.test b/tests/test_torque_control_on_spot_pg.test new file mode 100644 index 0000000..6f5ab91 --- /dev/null +++ b/tests/test_torque_control_on_spot_pg.test @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/tests/test_torque_control_walk_20.test b/tests/test_torque_control_walk_20.test new file mode 100644 index 0000000..2928d82 --- /dev/null +++ b/tests/test_torque_control_walk_20.test @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/tests/test_torque_control_walk_20_pg.test b/tests/test_torque_control_walk_20_pg.test new file mode 100644 index 0000000..5a2dcc9 --- /dev/null +++ b/tests/test_torque_control_walk_20_pg.test @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file From 30991ea66a087724fa6a39e71fae5368ece691fc Mon Sep 17 00:00:00 2001 From: Hugo_laptop Date: Tue, 25 Aug 2020 13:23:51 +0200 Subject: [PATCH 3/4] Added walk_vel tests, changed arguments for torque tests --- scripts/start_torque_control.py | 28 ++++++++++++++++------ tests/test_torque_control_on_spot.test | 2 +- tests/test_torque_control_on_spot_pg.test | 2 +- tests/test_torque_control_walk_20.test | 2 +- tests/test_torque_control_walk_20_pg.test | 2 +- tests/test_torque_walk_vel_on_spot.test | 6 +++++ tests/test_torque_walk_vel_on_spot_pg.test | 6 +++++ tests/test_torque_walk_vel_walk_20.test | 6 +++++ tests/test_torque_walk_vel_walk_20_pg.test | 6 +++++ 9 files changed, 49 insertions(+), 11 deletions(-) create mode 100644 tests/test_torque_walk_vel_on_spot.test create mode 100644 tests/test_torque_walk_vel_on_spot_pg.test create mode 100644 tests/test_torque_walk_vel_walk_20.test create mode 100644 tests/test_torque_walk_vel_walk_20_pg.test diff --git a/scripts/start_torque_control.py b/scripts/start_torque_control.py index 674f8e5..316bf72 100755 --- a/scripts/start_torque_control.py +++ b/scripts/start_torque_control.py @@ -36,7 +36,7 @@ def validation_through_gazebo(self): ldistance = math.sqrt(dx*dx+dy*dy+dz*dz) f.write("dist:"+str(ldistance)) f.close() - if ldistance<0.05: + if ldistance<0.07: self.assertTrue(True,msg="Converged to the desired position") else: self.assertFalse(True, @@ -59,8 +59,7 @@ def runTest(self): cli_args = [talos_data_path+'/launch/talos_gazebo_alone.launch', 'world:=empty_forced', - 'enable_leg_passive:=false', - 'debug:=true' + 'enable_leg_passive:=false' ] roslaunch_args = cli_args[1:] roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments\ @@ -108,13 +107,28 @@ def runTest(self): time.sleep(5) pkg_name='talos-torque-control' - executable='sim_walk_torque.py' - node_name='sim_walk_torque_py' - if len(argv)==3 and argv[2]=='pattern_generator': + if len(argv)==7 and argv[1]=='vel' and argv[3]=='pattern_generator': + #starting sim_walk_vel with type of walk and pg + mode=str(argv[2]+' '+argv[3]) + executable='sim_walk_vel.py' + node_name='sim_walk_vel_py' + elif len(argv)==6 and argv[1]=='vel': + #starting sim_walk_vel without pg + mode=str(argv[2]) + executable='sim_walk_vel.py' + node_name='sim_walk_vel_py' + elif len(argv)==6 and argv[2]=='pattern_generator': + #pg argument but length of 6 means no vel argument --> sim_walk_torque mode=str(argv[1]+' '+argv[2]) - else: + executable='sim_walk_torque.py' + node_name='sim_walk_torque_py' + elif len(argv)==5: + #walk type only will mean walk_torque mode=str(argv[1]) + executable='sim_walk_torque.py' + node_name='sim_walk_torque_py' + else: raise ValueError(str(len(argv))+' '+str(argv[0])+' '+str(argv[1])+' '+str(argv[2])+' '+str(argv[3])+' '+str(argv[4])+' '+str(argv[5])) sim_walk_torque_node = roslaunch.core.\ Node(pkg_name, executable,name=node_name,args=mode) diff --git a/tests/test_torque_control_on_spot.test b/tests/test_torque_control_on_spot.test index 9b49bc9..19770dc 100644 --- a/tests/test_torque_control_on_spot.test +++ b/tests/test_torque_control_on_spot.test @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/tests/test_torque_control_on_spot_pg.test b/tests/test_torque_control_on_spot_pg.test index 6f5ab91..64b1dce 100644 --- a/tests/test_torque_control_on_spot_pg.test +++ b/tests/test_torque_control_on_spot_pg.test @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/tests/test_torque_control_walk_20.test b/tests/test_torque_control_walk_20.test index 2928d82..e416ac3 100644 --- a/tests/test_torque_control_walk_20.test +++ b/tests/test_torque_control_walk_20.test @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/tests/test_torque_control_walk_20_pg.test b/tests/test_torque_control_walk_20_pg.test index 5a2dcc9..e9de494 100644 --- a/tests/test_torque_control_walk_20_pg.test +++ b/tests/test_torque_control_walk_20_pg.test @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/tests/test_torque_walk_vel_on_spot.test b/tests/test_torque_walk_vel_on_spot.test new file mode 100644 index 0000000..eb1d316 --- /dev/null +++ b/tests/test_torque_walk_vel_on_spot.test @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/tests/test_torque_walk_vel_on_spot_pg.test b/tests/test_torque_walk_vel_on_spot_pg.test new file mode 100644 index 0000000..784ae8b --- /dev/null +++ b/tests/test_torque_walk_vel_on_spot_pg.test @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/tests/test_torque_walk_vel_walk_20.test b/tests/test_torque_walk_vel_walk_20.test new file mode 100644 index 0000000..f9fab1a --- /dev/null +++ b/tests/test_torque_walk_vel_walk_20.test @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/tests/test_torque_walk_vel_walk_20_pg.test b/tests/test_torque_walk_vel_walk_20_pg.test new file mode 100644 index 0000000..6e562a4 --- /dev/null +++ b/tests/test_torque_walk_vel_walk_20_pg.test @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file From 2027ec3cf0a0a9eefebf4581ac4f12f839da0af7 Mon Sep 17 00:00:00 2001 From: Hugo_laptop Date: Tue, 25 Aug 2020 16:53:07 +0200 Subject: [PATCH 4/4] Modified called script from talos_torque_control --- scripts/start_torque_control.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/scripts/start_torque_control.py b/scripts/start_torque_control.py index 316bf72..024a4f4 100755 --- a/scripts/start_torque_control.py +++ b/scripts/start_torque_control.py @@ -111,22 +111,22 @@ def runTest(self): if len(argv)==7 and argv[1]=='vel' and argv[3]=='pattern_generator': #starting sim_walk_vel with type of walk and pg mode=str(argv[2]+' '+argv[3]) - executable='sim_walk_vel.py' + executable='integ_sim_walk_vel.py' node_name='sim_walk_vel_py' elif len(argv)==6 and argv[1]=='vel': #starting sim_walk_vel without pg mode=str(argv[2]) - executable='sim_walk_vel.py' + executable='integ_sim_walk_vel.py' node_name='sim_walk_vel_py' elif len(argv)==6 and argv[2]=='pattern_generator': #pg argument but length of 6 means no vel argument --> sim_walk_torque mode=str(argv[1]+' '+argv[2]) - executable='sim_walk_torque.py' + executable='integ_sim_walk_torque.py' node_name='sim_walk_torque_py' elif len(argv)==5: #walk type only will mean walk_torque mode=str(argv[1]) - executable='sim_walk_torque.py' + executable='integ_sim_walk_torque.py' node_name='sim_walk_torque_py' else: raise ValueError(str(len(argv))+' '+str(argv[0])+' '+str(argv[1])+' '+str(argv[2])+' '+str(argv[3])+' '+str(argv[4])+' '+str(argv[5]))