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 0be1baf26e6823464a86347129db12fafc49728e Mon Sep 17 00:00:00 2001 From: Hugo Lefevre Date: Thu, 16 Jul 2020 15:39:15 +0200 Subject: [PATCH 2/4] Update for new test with mapping --- README.md | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 3ab7b71..d9e724b 100644 --- a/README.md +++ b/README.md @@ -24,6 +24,14 @@ Preparing your environment variables: source ./test_ws/install/setup.bash source $HOME/bin/setup-opt-robotpkg.sh ``` +If your installation uses the binaries, you will need to install the following packages, on top of the required `robotpkg-sot-core-v3` and `robotpkg-py27-talos-dev`: +``` +sudo apt install robotpkg-talos-simulation robotpkg-talos-metapkg-ros-control-sot robotpkg-py27-sot-talos-balance robotpkg-py27-sot-pattern-generator-v3 robotpkg-ros-ouster-gazebo-simulation +``` + +The test using mapping and lidar still depend on the following packages, not yet integrated to robotpkg: +* [aicp_mapping](https://github.com/Gepetto/aicp_mapping), the mapping algorithm +* [talos-bauzil](https://gitlab.laas.fr/tlasguigne/talos-bauzil), the simulation of the room used for tests (to be ported on github) # First integration tests @@ -45,7 +53,7 @@ To launch: ``` rostest talos_integration_tests test_sot_talos_balance.test ``` -The robot is supposed to walk forward 2.8 m and reached position [2.8331,0.0405,1.0019] +The robot is supposed to walk forward 2.8 m and reach position [2.8331,0.0405,1.0019] There is a you tube video showing what to expect: @@ -57,7 +65,14 @@ 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] +The robot is supposed to walk forward 2.8 m and reach position [2.12,0.012,1.00] + +## Real time on-line walking while mapping using a lidar + +To launch: +``` +rostest talos_integration_tests test_ouster_walking.test +``` # Docker From ad9e5cd118a1b0cae2b88d7341e097a85500337a Mon Sep 17 00:00:00 2001 From: Hugo Lefevre Date: Fri, 24 Jul 2020 11:02:39 +0200 Subject: [PATCH 3/4] Necessary modifications to get SoT odometry --- scripts/start_sot_ouster_walking.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/scripts/start_sot_ouster_walking.py b/scripts/start_sot_ouster_walking.py index aa0f032..4ea723e 100755 --- a/scripts/start_sot_ouster_walking.py +++ b/scripts/start_sot_ouster_walking.py @@ -62,6 +62,7 @@ def runTest(self): roslaunch.configure_logging(uuid) cli_args = [talos_bauzil_path+'/launch/script_walking_bauzil.launch', + #'world_name:=empty', 'world_name:=bauzil_skins', #'robot:=full_v2', 'robot:=full_v2_ouster', @@ -129,7 +130,7 @@ def runTest(self): # Publish odometry launch_odom = roslaunch.parent.ROSLaunchParent(uuid, - [talos_bauzil_path+'/launch/talos_odometry.launch']) + [talos_bauzil_path+'/launch/talos_sot_odometry.launch']) launch_odom.start() rospy.loginfo("Talos odometry started") @@ -138,7 +139,7 @@ def runTest(self): # Start mapping aicp_path = arospack.get_path('aicp_ros') cli_args = [aicp_path+'/launch/aicp_mapping.launch', - 'pose_odom:=/odom_aicp', + 'pose_odom:=/odom_sot_aicp', 'lidar_channel:=/os1_cloud_node/points', 'inertial_frame:=/world', 'fixed_frame:=/world' From 6147f23155ba9e84fee7ad0c8fbf0536388a8d25 Mon Sep 17 00:00:00 2001 From: Hugo Lefevre Date: Fri, 24 Jul 2020 13:19:27 +0200 Subject: [PATCH 4/4] Exported necessay signals as ros topics for odometry --- src/talos_integration_tests/appli_online_walking.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/talos_integration_tests/appli_online_walking.py b/src/talos_integration_tests/appli_online_walking.py index d0592e8..7145436 100644 --- a/src/talos_integration_tests/appli_online_walking.py +++ b/src/talos_integration_tests/appli_online_walking.py @@ -357,6 +357,10 @@ def init_online_walking(robot): create_topic(robot.publisher, robot.pg, 'rightfootref', robot=robot,data_type='matrixHomo') create_topic( robot.publisher, robot.wp, 'footRight', robot=robot,data_type='matrixHomo') + #Odometry + create_topic(robot.publisher, robot.e2q, 'quaternion', robot=robot,data_type='vector') + create_topic(robot.publisher, robot.base_estimator, 'v', robot=robot,data_type='vector') + ## --- TRACER robot.tracer = TracerRealTime("com_tracer") robot.tracer.setBufferSize(80 * (2**20))