diff --git a/CMakeLists.txt b/CMakeLists.txt index e701cc6d22732b9695a778794c44515e5b0833be..99821fc0c7b6658e56b2e87b8c215c10109ac990 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,7 +46,7 @@ ENDIF(SUFFIX_SO_VERSION) INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib) -FOREACH(dir config gazebo meshes robots urdf scripts srdf launch) +FOREACH(dir config gazebo meshes robots urdf srdf launch) INSTALL(DIRECTORY ${dir} DESTINATION share/${PROJECT_NAME}) ENDFOREACH(dir) diff --git a/scripts/start_talos_gazebo.py b/scripts/start_talos_gazebo.py deleted file mode 100755 index 78935e0ca8f7350c5137b2b4dee2ea01803b16df..0000000000000000000000000000000000000000 --- a/scripts/start_talos_gazebo.py +++ /dev/null @@ -1,70 +0,0 @@ -#!/usr/bin/env python -# O. Stasse 17/01/2020 -# LAAS, CNRS - -import os -import rospy -import time -import roslaunch -import rospkg - -from std_srvs.srv import Empty - -# 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, -# [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() - -# 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") - -rospy.spin() - diff --git a/scripts/start_talos_gazebo_16_04.py b/scripts/start_talos_gazebo_16_04.py deleted file mode 100644 index 3a73661699284d476789e8a71c5552584fdc30bb..0000000000000000000000000000000000000000 --- a/scripts/start_talos_gazebo_16_04.py +++ /dev/null @@ -1,55 +0,0 @@ -#!/usr/bin/env python -# O. Stasse 17/01/2020 -# LAAS, CNRS - -# This file is a temporary fix for the ubuntu version 16.04 of the script start_talos_gazebo.py -# The path to talos_data cannot be retreive using the rospkg.RosPack() and get_path('talos_data') in 16.04. -# This should be investigated (see issue #4 "Hardcoded talos_data path for 16.04 in script start_talos_gazebo_16_04.py") - -import os -import rospy -import time -import roslaunch -from std_srvs.srv import Empty - -# Start roscore -import subprocess -roscore = subprocess.Popen('roscore') -time.sleep(1) - -# Start talos_gazebo -rospy.init_node('starting_talos_gazebo', anonymous=True) -uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) -roslaunch.configure_logging(uuid) - -launch_gazebo_alone = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/talos_data/launch/talos_gazebo_alone.launch"]) -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(3) -# Spawn talos model in gazebo -launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/talos_data/launch/talos_gazebo_spawn_hs.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(3) -gazebo_unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) -gazebo_unpause_physics() - -# Start roscontrol -launch_bringup = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/talos_bringup/launch/talos_bringup.launch"]) -launch_bringup.start() -rospy.loginfo("talos_bringup started") - -# # Start sot -# Start sot in another terminal with "roslaunch roscontrol_sot_talos sot_talos_controller_gazebo.launch" -# in order to have the logs saved. Otherwise the data are not correctly dumped when the process is killed. - - -rospy.spin() -