16 from __future__
import print_function
19 from gazebo_msgs.srv
import ApplyBodyWrench
20 from geometry_msgs.msg
import Point, Wrench, Vector3
22 if __name__ ==
'__main__':
23 print(
'Apply programmed perturbation to vehicle', rospy.get_namespace())
24 rospy.init_node(
'set_body_wrench')
26 if rospy.is_shutdown():
27 print(
'ROS master not running!')
31 if rospy.has_param(
'~starting_time'):
32 starting_time = rospy.get_param(
'~starting_time')
34 print(
'Starting time= {} s'.format(starting_time))
37 if rospy.has_param(
'~duration'):
38 duration = rospy.get_param(
'~duration')
41 print(
'Duration not set, leaving node...')
44 print(
'Duration [s]=', (
'Inf.' if duration < 0
else duration))
47 if rospy.has_param(
'~force'):
48 force = rospy.get_param(
'~force')
51 raise rospy.ROSException(
'Invalid force vector')
53 print(
'Force [N]=', force)
56 if rospy.has_param(
'~torque'):
57 torque = rospy.get_param(
'~torque')
59 raise rospy.ROSException(
'Invalid torque vector')
61 print(
'Torque [N]=', torque)
64 rospy.wait_for_service(
'/gazebo/apply_body_wrench', timeout=10)
65 except rospy.ROSException:
66 print(
'Service not available! Closing node...')
70 apply_wrench = rospy.ServiceProxy(
'/gazebo/apply_body_wrench', ApplyBodyWrench)
71 except rospy.ServiceException
as e:
72 print(
'Service call failed, error=', e)
75 ns = rospy.get_namespace().replace(
'/',
'')
77 body_name =
'%s/base_link' % ns
79 if starting_time >= 0:
80 rate = rospy.Rate(100)
81 while rospy.get_time() < starting_time:
85 wrench.force = Vector3(*force)
86 wrench.torque = Vector3(*torque)
93 rospy.Duration(duration))
96 print(
'Body wrench perturbation applied!')
97 print(
'\tFrame: ', body_name)
98 print(
'\tDuration [s]: ', duration)
99 print(
'\tForce [N]: ', force)
100 print(
'\tTorque [Nm]: ', torque)