Skip to content
Snippets Groups Projects
Commit f5599b37 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

Moved and split run_test_utils

parent 3cbabf92
No related branches found
No related tags found
No related merge requests found
......@@ -157,7 +157,6 @@ IF(BUILD_PYTHON_INTERFACE)
IF(BUILD_TEST)
INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/test/__init__.py
python/${SOTTALOSBALANCE_PYNAME}/test/link_state_publisher.py
python/${SOTTALOSBALANCE_PYNAME}/test/run_test_utils.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj_checkfeedback.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_ffSubscriber.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_zmpEstimator.py
......@@ -178,6 +177,8 @@ IF(BUILD_PYTHON_INTERFACE)
python/${SOTTALOSBALANCE_PYNAME}/utils/plot_utils.py
python/${SOTTALOSBALANCE_PYNAME}/utils/sot_utils.py
python/${SOTTALOSBALANCE_PYNAME}/utils/filter_utils.py
python/${SOTTALOSBALANCE_PYNAME}/utils/run_test_utils.py
python/${SOTTALOSBALANCE_PYNAME}/utils/gazebo_utils.py
DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME}/utils)
ENDIF(BUILD_UTILS)
ENDIF(BUILD_PYTHON_INTERFACE)
......
from sot_talos_balance.test.run_test_utils import run_test, runCommandClient
from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient
from time import sleep
run_test('appli_COMTraj.py')
......
......@@ -14,7 +14,7 @@ from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
import matplotlib.pyplot as plt
import numpy as np
from sot_talos_balance.test.run_test_utils import apply_force
from sot_talos_balance.utils.gazebo_utils import apply_force
def main(robot):
dt = robot.timeStep;
......
'''This module contains utilities for interacting with Gazebo.'''
import rospy
from gazebo_msgs.srv import ApplyBodyWrench
from geometry_msgs.msg import Wrench
def apply_force(force,duration, body_name = "talos::torso_2_link"):
'''Gazebo service call for applying a force on a body.'''
rospy.wait_for_service('/gazebo/apply_body_wrench')
apply_body_wrench_proxy = rospy.ServiceProxy('/gazebo/apply_body_wrench',ApplyBodyWrench)
wrench = Wrench()
wrench.force.x = force[0]
wrench.force.y = force[1]
wrench.force.z = force[2]
wrench.torque.x = 0
wrench.torque.y = 0
wrench.torque.z = 0
apply_body_wrench_proxy(body_name = body_name, wrench = wrench, duration = rospy.Duration(duration))
......@@ -7,9 +7,6 @@ from std_srvs.srv import *
from dynamic_graph_bridge.srv import *
from dynamic_graph_bridge_msgs.srv import *
from gazebo_msgs.srv import ApplyBodyWrench
from geometry_msgs.msg import Wrench
runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
def launch_script(code,title,description = ""):
......@@ -49,15 +46,3 @@ def run_test(appli):
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def apply_force(force,duration, body_name = "talos::torso_2_link"):
'''Gazebo service call for applying a force on a body.'''
rospy.wait_for_service('/gazebo/apply_body_wrench')
apply_body_wrench_proxy = rospy.ServiceProxy('/gazebo/apply_body_wrench',ApplyBodyWrench)
wrench = Wrench()
wrench.force.x = force[0]
wrench.force.y = force[1]
wrench.force.z = force[2]
wrench.torque.x = 0
wrench.torque.y = 0
wrench.torque.z = 0
apply_body_wrench_proxy(body_name = body_name, wrench = wrench, duration = rospy.Duration(duration))
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment